PCL：ICP点云配准

      ICP算法（Iterative Closest Point迭代最近点）是机器视觉中非常经典的算法。三维点集配准问题是计算机技术中的一个极其重要的问题，作为解决三维点集配准问题的一个应用较为广泛的算法ICP是解决三维点集配准问题的一个应用较为广泛的算法，此外在SLAM等移动机器人导航等领域也有着很大的用武之地。

      三维点集配准是一个非常重要的中间步骤，它在表面重建、三维物体识别、相机定位等问题中有着极其重要的应用。对于三维点集配准问题，研究者提出了很多解决方案但是应用最广泛的，但是应用最广泛，影响最大的还是由Besl和Mckay在1992年提出的迭代最近点算法（Iterative Closest Point，ICP），它是基于纯粹几何模型的三维物体对准算法，由于它的强大功能以及高的精确度，很快就成为了曲面配准中的主流算法。

 ICP算法有以下特点：

      1. 可以获得非常精确的配准效果；  
       2.可以处理三维点集、参数曲面等多种形式表达的曲面，也就是说该算法对曲面表示 方法独立；
       3.不必对待处理的点集进行分割和特征提取；   
       4.在较好的初值情况下，可以得到很好的算法收敛性。    
       5.该算法在搜索对应点的过程中，计算代价非常的大；   
       6.在基本的ICP算法中，在寻找对应点的时候，认为欧氏距离最近的点就是对应点，这种假设是比较武断的，它会产生一定数量的错误对应点。

ICP算法包括以下步骤：

      1） 根据点集Plk中的点坐标，在曲面S上搜索相应最近点点集Prk；  

       2） 计算两个点集的重心位置坐标，并进行点集中心化生成新的点集；   
      3） 由新的点集计算正定矩阵N，并计算N的最大特征值及其最大特征向量；   
      4） 由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转换为旋转矩阵R；    
      5） 在旋转矩阵R被确定后，由平移向量t仅仅是两个点集的重心差异，可以通过两个坐标系中的重心点和旋转矩阵确定；    
      6） 根据式(0-3)，由点集Plk计算旋转后的点集P’lk。通过Plk与P’lk计算距离平方和值为fk+1。以连续两次距离平方和之差绝对值 作为迭代判断数值；   
      7） 当 时，ICP配准算法就停止迭代，否则重复1至6步，直到满足条件 后停止迭代。

        经过了十多年的发展ICP也有着很多的变种： Point to Point就近点搜索法； Point to Plane就近点搜索算法； Point to Projection就近点搜索算法。

         ICP算法的前提条件是具有一个良好的配准初值,文中在配准初值的选取上采用 主成分分析法 ,为后续ICP算法的工作提供一个良好前提条件,增加点集预处理,点对查找上增加各种限制,采用 kd-tree 加速查找,以此对算法进行改进,并通过实例来验证本算法的有效性及合理性[1]   

PCL中ICP的实现：

       PCL中实现ICP与相关论文中一致，最终的变换矩阵都是基于SVD（奇异值分解）。

        由基类Registration派生，直接实例化对象即可：

Pcl:: IterativeClosestPoint icp

       成员函数的组成：

1. inline void inline void setSearchMethodTarget(const KdTreePtr &tree)   kdtree加速搜索，还有一个Target的函数，用法与之一致，普通的匹配可以不用使用。
2. inline void setInputSource (constPointCloudSourceConstPtr &cloud)     源点云。指将要被旋转和平移的点云。
   
3. inline void setInputTarget (constPointCloudTargetConstPtr &cloud)       目标点云。从Source到Target匹配，将源点云变换到目标点云的相对位置。
4. inline void setMaximumIterations (intnr_iterations)                              迭代次数，几十上百甚至上千都可能出现（默认为10），可以用来终止迭代。
5. inline void setTransformationEpsilon (doubleepsilon)     上次转换与当前转换的差值，这个值一般设为1e-6或者1e-10或者更小，可以用来终止迭代次数。
6. inline void setEuclideanFitnessEpsilon (doubleepsilon)      前后两次迭代方差的差值，可以用来终止迭代次数。
   
7. inline void setMaxCorrespondenceDistance (doubledistance_threshold) 如果两个点云距离大于此值，则被忽略（PCL默认距离单位是m），一般根据两个点云之间的距离估计，这个值对点云匹配的结果影响较大。
8. inline void align (PointCloudSource &output)    output为配准后点云。
   
9. inline Matrix4 getFinalTransformation ()    获取最终的转换矩阵。

      注：1.如果由于设置相关参数过于苛刻，而造成没有匹配成功法， icp.hasConverged() 会提示匹配失败，并且   icp.getFitnessScore()  为0，相关的转换矩阵为单位矩阵。

              2.输入的点云最好经过预处理，过于复杂和过多噪声的点与将会出现“Invalid (NaN, Inf) point coordinates given to nearestKSearch!”的错误。

效果展示：

        最近终于把PCL1.7.2搞定了，IDE为VS2013，系统为win7 64位。我使用的是两个近似的人体模型点云，。所需要的核心代码如下：

typedef pcl::PointXYZRGB PointT;//点类型

        pcl::IterativeClosestPoint icp;//创建icp对象
icp.setInputSource(cloud_pr);  //原点云
icp.setInputTarget(cloud_hole_pr);//目标点云
pcl::PointCloud final_cloud;//配准后点云
//icp 参数设置
    icp.setMaximumIterations(1000);  //最大迭代次数
icp.setEuclideanFitnessEpsilon(0.5);//前后两次迭代误差的差值
icp.setTransformationEpsilon(1e-10); //上次转换与当前转换的差值；
icp.setMaxCorrespondenceDistance(0.7); //忽略在此距离之外的点，对配准影响较大
//保存配准后的点
icp.align(final_cloud);
//icp匹配后的转换矩阵及得分
cout << "has converged: " << icp.hasConverged() << endl
<< "score: " << icp.getFitnessScore() << endl;
cout << icp.getFinalTransformation() << endl; 

   由上述代码片可知：一共有4个数据需要设定，这里我们修改不同的数据看下效果图（红 色为Source，绿 色为Target，蓝色为匹配后点云）:

     

      由于不是相同点云配准，加上刚刚接触PCL点云配准，配准效果还是有些不太理想。ICP属于精确匹配，并且对初始位置和相关参数的设定有一定的要求。并且匹配的点云表面复杂度不能太高。我是刚刚开始接触点云配准，有许多不足之处，有兴趣的可以多多交流。

     icp相关代码及点云文件我已上传，可直接下载使用。

   1. 三维点云ICP算法改进研究     ．知网空间 [引用日期2013-04-29]