ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。PCL点云库已经实现了多种点云配准算法：
pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference
pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference
pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference
IterativeClosestPoint类提供了标准ICP算法的实现（The transformation is estimated based on SVD），算法迭代结束条件有如下几个:
基本用法如下：
IterativeClosestPoint<PointXYZ, PointXYZ> icp;
// Set the input source and target icp.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 alignment icp.align (cloud_source_registered); // Obtain the transformation that aligned cloud_source to cloud_source_registered Eigen::Matrix4f transformation = icp.getFinalTransformation ();
下面是一个完整的例子：
#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> int main (int argc, char** argv) { //Creates two pcl::PointCloud<pcl::PointXYZ> boost shared pointers and initializes them pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>); // 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<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setInputCloud(cloud_in); icp.setInputTarget(cloud_out); //Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm pcl::PointCloud<pcl::PointXYZ> 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() <<std::endl; //Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) std::cout << "score: " <<icp.getFitnessScore() << std::endl; std::cout << "----------------------------------------------------------"<< std::endl; //Get the final transformation matrix estimated by the registration method. std::cout << icp.getFinalTransformation() << std::endl; return (0); }
结果如下，ICP算法计算出了正确的变换
在PCL官方的tutorial中还有个ICP算法交互的例子（Interactive Iterative Closest Point，网站上该例子的源代码编译时有一点问题需要修改...），该程序中按一次空格ICP迭代计算一次。可以看出，随着迭代进行，两块点云逐渐重合在一起。
参考：
How to use iterative closest point
http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point
Interactive Iterative Closest Point
http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
PCL之ICP算法实现
https://segmentfault.com/a/1190000005930422
PCL学习笔记二：Registration (ICP算法)
http://blog.csdn.net/u010696366/article/details/8941938
转：https://www.cnblogs.com/21207-iHome/p/6034462.html
“”ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。“” 这句里面初始位置有一定要求，请问这个要求具体是指哪些？