浙江省建设监理协会管网站/一句话宣传自己的产品
迭代最近点算法（ICP）算法是Lidar SLAM中常用的点云配准方法，可以求解两组点云之间的相对位姿。
本文对最基本的ICP算法进行了介绍和简单实现，并集成为一个简化版的Odometry。
1 原理
1.1 问题：给定两组点云
求解两组点云之间的先对位姿
下面，首先给出已知匹配，且点个数相同的求解方方法。之后再给出未知匹配，且点数量不同的情况的求解方法：ICP。
1.2 已知匹配的解法
此时，两组点云是已经匹配好的（通过下标一一对应），且数量相等（
这个式子可以获得一个解析解，具体步骤如下。
step 1. 去掉两组点云的质心
两组点云的质心分别为
两组点云去掉质心后的坐标为
step 2. 计算
step 3. 计算旋转矩阵R
对
step 4. 计算平移t
平移为
1.3 ICP求解方法
实际情况下，点云之间的匹配关系是不知道，且点的数量不相等的。为此，ICP算法采用迭代求解的方法。具体的迭代如下步骤。
step 1. 从两组点云中选择匹配的点对，形成匹配两组点云；
step 2. 利用1.2节中的方法求解；
step 3. 如果相对于上一次迭代误差的减少量
|| 超过最大迭代次数 ||
结束迭代。否则以本次迭代为初值继续迭代。
第2、3步都比较简单，关键的是第一步，如何构建匹配点。最基本的做法是对于
还有一个问题，有一些匹配时错误的，这些错误的匹配的表现是匹配点之间的距离过大。一般会用一个N倍平均距离的阈值
2 实现
全部实现，请见
ydsf16/lidar_slamgithub.com
我们利用TUM提供的RGB-D数据进行测试，只使用了其中的Depth数据生成点云。通过ICP计算相邻两帧之间的位姿变换，串接形成一个Odometry。利用rgbd_dataset_freiburg1_xyz这组数据形成的轨迹如下。
3 其他变种
ICP算法在199x年就被提出来了，后面又出了很多的变种。主要的不同在于以下几点。
参考资料
[1] PCL 库实现
[2] http://ais.informatik.uni-freiburg.de/teaching/ss11/robotics/slides/17-icp.pdf
[3]Besl P J , Mckay N D . A Method for Registration of 3-D Shapes[J]. Proceedings of SPIE - The International Society for Optical Engineering, 1992, 14(3):239-256.