ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。在VTK、PCL、MRPT、MeshLab等C++库或软件中都有实现，可以参见维基百科中的ICP Algorithm .

　　ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。PCL点云库已经实现了多种点云配准算法：

　　类提供了标准ICP算法的实现（The transformation is estimated based on SVD），算法迭代结束条件有如下几个:

1. 最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via )
2. 两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via )
3. 均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold (via )

　　基本用法如下：

IterativeClosestPoint
     
       icp; // Set the input source and targeticp.setInputCloud (cloud_source);icp.setInputTarget (cloud_target); // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)icp.setMaxCorrespondenceDistance (0.05);// Set the maximum number of iterations (criterion 1)icp.setMaximumIterations (50);// Set the transformation epsilon (criterion 2)icp.setTransformationEpsilon (1e-8);// Set the euclidean distance difference epsilon (criterion 3)icp.setEuclideanFitnessEpsilon (1); // Perform the alignmenticp.align (cloud_source_registered);// Obtain the transformation that aligned cloud_source to cloud_source_registeredEigen::Matrix4f transformation = icp.getFinalTransformation ();
     

　　下面是一个完整的例子：

#include 
     
      #include 
      
       #include 
       
        #include 
        
         int main (int argc, char** argv){    //Creates two pcl::PointCloud
         
           boost shared pointers and initializes them    pcl::PointCloud
          
           ::Ptr cloud_in (new pcl::PointCloud
           
            ); pcl::PointCloud
            
             ::Ptr cloud_out (new pcl::PointCloud
             
              ); // Fill in the CloudIn data cloud_in->width = 5; cloud_in->height = 1; cloud_in->is_dense = false; cloud_in->points.resize (cloud_in->width * cloud_in->height); for (size_t i = 0; i < cloud_in->points.size (); ++i) { cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); } *cloud_out = *cloud_in; //performs a simple rigid transform on the point cloud for (size_t i = 0; i < cloud_in->points.size (); ++i) cloud_out->points[i].x = cloud_in->points[i].x + 1.5f; //creates an instance of an IterativeClosestPoint and gives it some useful information pcl::IterativeClosestPoint
              
                icp; icp.setInputCloud(cloud_in); icp.setInputTarget(cloud_out); //Creates a pcl::PointCloud
               
                 to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm pcl::PointCloud
                
                  Final; //Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. icp.align(Final); //Return the state of convergence after the last align run. //If the two PointClouds align correctly then icp.hasConverged() = 1 (true). std::cout << "has converged: " << icp.hasConverged() <
                
               
              
             
            
           
          
         
        
       
      
     

　　结果如下，ICP算法计算出了正确的变换

　　在PCL官方的tutorial中还有个ICP算法交互的例子（t，网站上该例子的源代码编译时有一点问题需要修改...），该程序中按一次空格ICP迭代计算一次。可以看出，随着迭代进行，两块点云逐渐重合在一起。

 

 

参考：

How to use iterative closest point

Interactive Iterative Closest Point

PCL之ICP算法实现

PCL学习笔记二：Registration (ICP算法)