[LIDAR-SLAM] Iterative Closest Point (ICP)简单实现

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

1 原理

1.1 问题：给定两组点云

$$\begin{gathered} X = \left\{ {{x_1},{x_2}, \cdots ,{x_m}} \right\} \\ P = \left\{ {{p_1},{p_2}, \cdots ,{p_n}} \right\} \\ \end{gathered} \ (1)\\$$

求解两组点云之间的先对位姿 $R,t$ 。问题的特殊性在于：1）两组点云之间的匹配关系是未知的；2）两组点云中点的数量不相同。

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

1.2 已知匹配的解法

此时，两组点云是已经匹配好的（通过下标一一对应），且数量相等（ $m=n$ ）。可通过最小化下式求解 $R,t$

$$E\left( {R,t} \right) =\frac{1}{n} \sum\limits_{i = 1}^n {{{\left\| {{x_i} - \left( {R{p_i} + t} \right)} \right\|}^2}} \ (2) \\$$

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

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

两组点云的质心分别为 ${\mu _x} = \frac{1}{n}\sum\limits_{i = 1}^n {{x_i}} ,\ {\mu _p} = \frac{1}{n}\sum\limits_{i = 1}^n {{p_i}}$ 。

两组点云去掉质心后的坐标为 $X' = \left\{ {{x_i} - {\mu _x}} \right\} = \left\{ {{{x'}_i}} \right\},P' = \left\{ {{p_i} - {\mu _p}} \right\} = \left\{ {{{p'}_i}} \right\}$

step 2. 计算

$$W = \sum\limits_{i = 1}^n {{{x'}_i}} {\left( {p'} \right)^T} \ (3)\\$$

step 3. 计算旋转矩阵R

对 $W$ 进行SVD分解，有 $W = U\Sigma {V^T}$ 。当 $W$ 满秩的时候，有唯一解

$$R=UV^T \\$$

step 4. 计算平移t

平移为

$$t=\mu_x -R\mu_p \\$$

1.3 ICP求解方法

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

step 1. 从两组点云中选择匹配的点对，形成匹配两组点云； step 2. 利用1.2节中的方法求解 ； step 3. 如果相对于上一次迭代误差 的减少量 || 超过最大迭代次数 || 结束迭代。否则以本次迭代为初值继续迭代。

第2、3步都比较简单，关键的是第一步，如何构建匹配点。最基本的做法是对于 $P$ 中的一个点 $p_i$ 选择 $X$ 中与其欧式距离最近的点最为匹配点（可以用KD-Tree加速）。如此，对于 $P$ 中的每一个点，都可以在 $X$ 中找到一个对应点。这样就形成了两组已知匹配的点云，并且点数相等。

还有一个问题，有一些匹配时错误的，这些错误的匹配的表现是匹配点之间的距离过大。一般会用一个N倍平均距离的阈值 $D_{th}=N\cdot D_{avg}$ 去掉那些错误匹配。这里的平均阈值可以使用上一次迭代的值。

2 实现

全部实现，请见

ydsf16/lidar_slam

我们利用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.