ICP算法学习分享3：简单代码实战及算法理解

ICP：Iterative Closest Point，顾名思义，这是一种迭代的思想。通过不断地迭代，每次在前一次的计算结果之上再计算出新的变换矩阵，最终当迭代次数满足条件或者变换矩阵收敛时停止。

设已经过粗配准后待进行细配准的两帧点云分别为P={p1,p2,⋯,pm},Q={q1,q2,⋯,qn}。通常情况下，由于深度相机视野不变，所以扫描出的每一帧点云中点集数量相等，即m = n m=nm=n），这里假设需要将P PP配准到Q QQ上。

步骤：

1.寻找P和Q上所有对应点对：遍历P或Q中所有点进行操作，如果遍历到Pi点，将Pi点放入Q所有点构成的kdtree空间，查找Q中距离Pi最近的点qi,如果 $\left | pi-qi\right |$ ＜ $\xi$ ，则认为（pi，qi）是一对对应点，其中 $\xi$ 为人为设定的阈值。（其中值得注意一点，在不同点对（Pi，Qi）与（Pj，Qj）中，完全有可能出现Qi=Qj的情况，这并不影响）遍历完P中的所有点后找到了所有对应点对。

对应点对集合Corr={(pi,qi)∣pi∈P,qi∈Q,∥pi−qi∥<ξ}，其中Qi为Q的kdtree空间与Pi最近的点。

2.设要求的变换矩阵为R，对于∀p∈Corr，都可以构造一个误差lp=∥Rp−q∥，那么对于所有对应点对，就可以利用每个点的误差求和来构造一个整体的误差函数L=∑iN∥Rpi−qi∥，这样我们的目标就成了让这个误差函数最小，就成了一个最小二乘问题。在Eigen库中，只需要输入两片点云中的对应点即可求得变换矩阵R。

3.在第二步中求得变换矩阵R后，将点云P进行变换后得到更新位置后的点云P‘，然后重复步骤1-3，知道满足条件停止迭代。

代码实战：

1 读取点云数据

首先，需要读取两个待配准的点云数据。在PCL（Point Cloud Library）中，可以使用io::PointCloudReader类来实现。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);

pcl::io::loadPCDFile<pcl::PointXYZ>("cloud1.pcd", *cloud1);
pcl::io::loadPCDFile<pcl::PointXYZ>("cloud2.pcd", *cloud2);

2 计算源点云和目标点云的法线

为了计算点到面的距离，需要先计算每个点所在平面的法线。在PCL中，可以使用NormalEstimation类来实现。

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud1);
ne.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

ne.setRadiusSearch (0.03);
ne.compute (*normals);

3 设置加权的点到面ICP变换估计

在PCL中，可以使用TransformationEstimationPointToPlaneWeighted类来实现加权的点到面ICP变换估计。

pcl::TransformationEstimationPointToPlaneWeighted<pcl::PointXYZ, pcl::PointXYZ, pcl::Normal> tecp;
tecp.setInputSource(cloud1);
tecp.setInputTarget(cloud2);
tecp.setRadiusSearch(0.02);
tecp.setNormalConsistency(true);
tecp.setNearestNeighborSearchMethod (new pcl::search::KdTree<pcl::PointXYZ>);
tecp.setTransformationEpsilon(1e-6);
tecp.setMaximumIterations(50);

4 设置ICP参数

在PCL中，可以通过setTransformationEpsilon和setMaximumIterations方法设置ICP算法的参数。

tecp.setTransformationEpsilon(1e-6);
tecp.setMaximumIterations(50);

5 执行优化算法

执行优化算法，找到最优的刚体变换。

tecp.align(*cloud1);

6 可视化

最后，可以使用PCL的visualization::PCLVisualizer类来实现点云的可视化。

pcl::visualization::PCLVisualizer viewer("ICP Algorithm");
viewer.addPointCloud(cloud1, "source_cloud");
viewer.addPointCloud(cloud2, "target_cloud");
viewer.addPointCloud(cloud1, "optimized_cloud");
viewer.spin();