PCL-基于FPFH的SAC-IA结合ICP的点云配准方法

一、相关方法原理

点云是在同一空间参考系下表达目标空间分布和目标表面特性的海量点集合，在获取物体表面每个采样点的空间坐标后，得到的是点的集合，称之为点云（Point Cloud）。点云配准（Point Cloud Registration）指的是输入同一时刻采集到的两幅处于不同坐标系下的点云点集Pt和Ps，输出一个变换T，使得T（Ps）和Pt的重合程度尽可能高，其中，变换T可分为刚性变换和非刚性变换。
点云配准过程可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。
在粗配准阶段，首先使用凸包算法计算待配准点云和目标点云的凸包顶点，而后计算两幅点云中各顶点的FPFH特征，最后根据各点的FPFH特征利用SAC-IA算法求出变换矩阵，完成粗配准；在精配准阶段，在粗配准的基础上，使用ICP算法对粗配准得到的变换矩阵做进一步的优化，得到最优结果，完成配准过程。
本博客所使用的点云配准方法流程如下图（图中粗配准和精配准过程写反了哈）所示
在这里插入图片描述

1.凸包方法

凸包（Convex Hull）是一个计算几何中的概念。在一个实数向量空间V中，对于给定集合X，所有包含X的凸集的交集S被称为X的凸包。X的凸包可以用X内所有点(X1，…Xn)的线性组合来构造。在二维平面上的点集，其凸包就是将最外层的点连接起来构成的凸多边形，如图2-1所示，它能包含点集中所有的点。
在这里插入图片描述
图2-1 二维平面凸包
对于三维空间中点集，其凸包则是凸多面体。对于如图2-2所示点云，其凸包如2-3所示，其中红色点为凸包顶点。在后续粗配准过程中，将提取到的凸包顶点作为新的待配准点云和目标点云进行配准。
在这里插入图片描述
图2-2原始点云

图2-3 原始点云对应凸包

2.FPFH特征描述

点快速特征直方图(Fast Point Feature Histogram, FPFH)通常用于描述三维点的局部特征，通过参数化查询点与紧邻点之间的空间差异，形成多维直方图对点的近邻进行几何描述。

3.SAC-IA概述

采样一致性初始配准算法（Sample Consensus Initial Aligment , SAC-IA)是一种依赖于FPFH特征的配准算法，在执行此算法之前，需要先计算点云各点的FPFH特征，算法的大致流程如下：
（1）从待配准点云中选取n个（n≥3）采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d。
（2）在目标点云中查找与待配准点云中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为待配准点云在目标点云中的一一对应点。
（3）使用SVD分解求解待配准点集与目标点集之间的变换矩阵。
（4）计算此时待配准点云与目标点云之间的误差。
这里引用Huber损失函数来描述变换后的“距离误差和”，函数如下所示：
在这里插入图片描述

其中mi为预先设定的阈值，li为对应点经过变换后的距离误差，上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换矩阵即为粗配准过程中所求的变换矩阵。

4.ICP概述

ICP（迭代最近点）算法主要应用于点云数据的配准，通过构建最小二乘法目标函数，求解坐标转换关系（旋转矩阵R和平移向量t），将连续扫描的两帧或多帧点云数据统一到同一坐标系中，或是将扫描的点云与已经建立好的地图进行配准。
ICP算法步骤如下：
（1）选取待配准点集与目标点集。对于点数量较多的点云，可以选择部分点作为待配准点集。对于点的选取，通常使用滤波方法，或基于旋转不变等特性，提取点云的特征点作为待配准点集。
（2）依次遍历待配准点云，从目标点集中选择欧式距离最近的点，组成对应点对。
（3）根据对应点对，利用最小二乘法构建目标函数，而后利用SVD分解求解变换矩阵。
（4）根据求得的变换矩阵对待配准点云进行欧式变换，并将得到的新的点云作为待配准点云。
（5）判断是否满足停止迭代条件，条件可以是达到最大迭代次数，或两次迭代中误差优化的效果小于设定阈值，或本次迭代中误差小于设定阈值。做满足某一条件则停止迭代并输出结果，否则进行下一次迭代。

二、实验代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ia_ransac.h>//采样一致性
#include <pcl/registration/icp.h>//icp配准
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <pcl/surface/convex_hull.h>
#include <time.h>//时间
#include <pcl/search/kdtree.h>using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_mid,PointCloud::Ptr pcd_final)
{//创建初始化目标pcl::visualization::PCLVisualizer viewer("registration Viewer");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> mid_h(pcd_final, 0, 255, 255);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud(pcd_src, src_h, "source cloud");viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");viewer.addPointCloud(pcd_mid,mid_h,"mid cloud");viewer.addPointCloud(pcd_final, final_h, "final cloud");while (!viewer.wasStopped()){viewer.spinOnce(100);}
}int main(int argc, char** argv)
{//加载点云文件PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准pcl::io::loadPCDFile("../bunny_src.pcd", *cloud_src_o);PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云pcl::io::loadPCDFile("../bunny_tgt.pcd", *cloud_tgt_o);clock_t start = clock();clock_t hull_start= clock();pcl::ConvexHull<pcl::PointXYZ> src_hull;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_hull_src(new pcl::search::KdTree<pcl::PointXYZ>());tree_hull_src->setInputCloud(cloud_src_o);src_hull.setSearchMethod(tree_hull_src);src_hull.setInputCloud(cloud_src_o);src_hull.setDimension(3);std::vector<pcl::Vertices> src_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr src_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);src_hull.reconstruct(*src_surface_hull, src_polygons);pcl::ConvexHull<pcl::PointXYZ> tgt_hull;tgt_hull.setInputCloud(cloud_tgt_o);tgt_hull.setDimension(3);std::vector<pcl::Vertices> tgt_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);tgt_hull.reconstruct(*tgt_surface_hull, tgt_polygons);clock_t hull_end = clock();clock_t fpfh_start = clock();//计算表面法线pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;ne_src.setInputCloud(src_surface_hull);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());ne_src.setSearchMethod(tree_src);pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);ne_src.setRadiusSearch(0.02);ne_src.compute(*cloud_src_normals);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;ne_tgt.setInputCloud(tgt_surface_hull);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());ne_tgt.setSearchMethod(tree_tgt);pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);ne_tgt.setRadiusSearch(0.02);ne_tgt.compute(*cloud_tgt_normals);//计算FPFHpcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;fpfh_src.setInputCloud(src_surface_hull);fpfh_src.setInputNormals(cloud_src_normals);pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);fpfh_src.setSearchMethod(tree_src_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_src.setRadiusSearch(0.05);fpfh_src.compute(*fpfhs_src);std::cout << "compute *cloud_src fpfh" << endl;pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;fpfh_tgt.setInputCloud(tgt_surface_hull);fpfh_tgt.setInputNormals(cloud_tgt_normals);pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);fpfh_tgt.setSearchMethod(tree_tgt_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_tgt.setRadiusSearch(0.05);fpfh_tgt.compute(*fpfhs_tgt);std::cout << "compute *cloud_tgt fpfh" << endl;clock_t fpfh_end = clock();clock_t sac_start = clock();//SACpcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;scia.setInputSource(src_surface_hull);scia.setInputTarget(tgt_surface_hull);scia.setSourceFeatures(fpfhs_src);scia.setTargetFeatures(fpfhs_tgt);//scia.setMinSampleDistance(1);//scia.setNumberOfSamples(2);//scia.setCorrespondenceRandomness(20);PointCloud::Ptr sac_result(new PointCloud);scia.align(*sac_result);std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore()<< endl;Eigen::Matrix4f sac_trans;sac_trans = scia.getFinalTransformation();std::cout << sac_trans << endl;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud_src_o, *cloud_mid, sac_trans);/*pcl::ConvexHull<pcl::PointXYZ> mid_hull;tgt_hull.setInputCloud(cloud_mid);tgt_hull.setDimension(3);std::vector<pcl::Vertices> mid_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr mid_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);tgt_hull.reconstruct(*mid_surface_hull, mid_polygons);*/clock_t sac_end = clock();clock_t icp_start = clock();//icp配准pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_icp_mid(new pcl::search::KdTree<pcl::PointXYZ>);tree_icp_mid->setInputCloud(cloud_mid);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_icp_tgt(new pcl::search::KdTree<pcl::PointXYZ>);tree_icp_tgt->setInputCloud(cloud_tgt_o);icp.setSearchMethodSource(tree_icp_mid);icp.setSearchMethodTarget(tree_icp_tgt);icp.setInputSource(cloud_mid);icp.setInputTarget(cloud_tgt_o);// 最大迭代次数icp.setMaximumIterations(20);// 均方误差icp.setEuclideanFitnessEpsilon(0.01);PointCloud::Ptr icp_result(new PointCloud);icp.align(*icp_result, sac_trans);std::cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;Eigen::Matrix4f icp_trans;icp_trans = icp.getFinalTransformation();//cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;std::cout << icp_trans << endl;pcl::transformPointCloud(*cloud_mid, *icp_result, icp_trans);clock_t icp_end = clock();clock_t end = clock();cout << "hull time:" << (double)(hull_end - hull_start) / CLOCKS_PER_SEC << " s" << endl;cout << "fpfh time:" << (double)(fpfh_end - fpfh_start) / CLOCKS_PER_SEC << " s" << endl;cout << "sac time:" << (double)(sac_end - sac_start) / CLOCKS_PER_SEC << " s" << endl;cout << "icp time:" << (double)(icp_end - icp_start) / CLOCKS_PER_SEC << " s" << endl;cout << "time:" << (double)(end - start) / CLOCKS_PER_SEC << " s" << endl;pcl::io::savePCDFileASCII("cloud_src_hull.pcd", *src_surface_hull);pcl::io::savePCDFileASCII("cloud_tgt_hull.pcd", *tgt_surface_hull);pcl::io::savePCDFileASCII("cloud_mid.pcd", *cloud_mid);pcl::io::savePCDFileASCII("cloud_result.pcd", *icp_result);//可视化visualize_pcd(cloud_src_o, cloud_tgt_o, cloud_mid,icp_result);return 0;
}