icp算法的实现_[LIDAR-SLAM] Iterative Closest Point (ICP)简单实现

迭代最近点算法（ICP）算法是Lidar SLAM中常用的点云配准方法，可以求解两组点云之间的相对位姿。
本文对最基本的ICP算法进行了介绍和简单实现，并集成为一个简化版的Odometry。

1 原理

1.1 问题：给定两组点云

求解两组点云之间的先对位姿

。问题的特殊性在于：1）两组点云之间的匹配关系是未知的；2）两组点云中点的数量不相同。

下面，首先给出已知匹配，且点个数相同的求解方方法。之后再给出未知匹配，且点数量不同的情况的求解方法：ICP。

1.2 已知匹配的解法

此时，两组点云是已经匹配好的（通过下标一一对应），且数量相等（

）。可通过最小化下式求解

这个式子可以获得一个解析解，具体步骤如下。

step 1. 去掉两组点云的质心

两组点云的质心分别为

。

两组点云去掉质心后的坐标为

step 2. 计算

step 3. 计算旋转矩阵R

对

进行SVD分解，有

。当

满秩的时候，有唯一解

step 4. 计算平移t

平移为

1.3 ICP求解方法

实际情况下，点云之间的匹配关系是不知道，且点的数量不相等的。为此，ICP算法采用迭代求解的方法。具体的迭代如下步骤。

step 1. 从两组点云中选择匹配的点对，形成匹配两组点云；
step 2. 利用1.2节中的方法求解

；

step 3. 如果相对于上一次迭代误差

的减少量

|| 超过最大迭代次数 ||

结束迭代。否则以本次迭代为初值继续迭代。

第2、3步都比较简单，关键的是第一步，如何构建匹配点。最基本的做法是对于

中的一个点

选择

中与其

欧式距离最近的点最为匹配点（可以用KD-Tree加速）。如此，对于

中的每一个点，都可以在

中找到一个对应点。这样就形成了两组已知匹配的点云，并且点数相等。

还有一个问题，有一些匹配时错误的，这些错误的匹配的表现是匹配点之间的距离过大。一般会用一个N倍平均距离的阈值

去掉那些错误匹配。这里的平均阈值可以使用上一次迭代的值。

2 实现

全部实现，请见

ydsf16/lidar_slam​github.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.