SLAM ICP介绍、详细推导及相关变种

1 ICP介绍

ICP即为迭代最近点。它在激光雷达应用频率很高，主要是在点云配准领域。当载有雷达（2D或3D）的机器人在移动时，由于机器人产生了一个平移（x，y方向）+一个旋转（机器人的角度），所以雷达扫描到的同一个物体得到的点云信息也相应的会出现一个变化，这个变化是一个经过了旋转和平移之后的位姿变化。ICP可以做的事情就是，求出这个旋转R和平移T。

其主要流程如下：

step1：预处理点云

step2：寻找对应点（最近点）

step3：根据对应点，计算R和t

step4：对点云进行转换，计算误差

step5：不断迭代，直至误差小于某一个值

图1-1 ICP流程图

2 ICP推导

2.1找到对应点

激光点云的传回数据是单一的，只有x和y的坐标值（当然也可以看作是距离和角度值）。那么激光环境下的ICP全部的依赖就是点的x和y，x与y的数量有很多。

如图2-1所示，雷达扫到了同一个物体，但是由于雷达本身有移动，所传回的物体的点云也有所移动，我们知道这些点云其实就是无数个x和y，那么我们要做的即通过大量的x和y来求得雷达的移动距离和偏转角度。

图2.1-1

首先第一步，就是要找对应点。

在雷达环境下，定义两坨点云Ai,Bi，以Ai点云中的A1点为例子，Bi中离A1最近的点，就认为是A1在Bi点云中的对应点，Ai中的其它点云也一样。

其中X和Y表示原始点云的点集，选取的是原始点云中能相互关联的那些点，即Nx=Ny

 

2.2 优化方程实现

2.2.1 优化目标

ICP的主要目的是求两个点云或是矩阵的R旋转和平移T的，由此可以设计出初步的优化方程,原始点云经过变换之后的点云和目标点云相减，这个残差越小，证明匹配的越好，即优化方程可简化为：

 

2.2.2 优化目标简化

求解该方程步骤如下：

 

其中ux与uy分别表示点集X与Y的质心

 

2.2.3 转化优化目标及简化

该化简的目的是使得R与t分开，方便进行后续求解。化简后原式即可看作对两个分式求最小值，即为

 

只要求得E1的最小值的相关取值，即可令E2为0，即该最优化问题转化为求E1的最小值。

接下来对于该式进行化简：

 

上式化简中XTX（第一个部分）与R无关，第二个部分为单位阵。

令

 

原式目的是使损失函数最小，即最小化E1，等价于最大化上式

2.2.4 相关简化及定理应用与证明

原式可转化为

 

而关于该问题，化简如下：

 

等式（1）：定理：常数的迹等于其自身

等式（2）：

 

等式（3）：恒等变换

 

最终问题转化为，找到合适的R，使得Trace（RH）达到最大值。

而对于Trace与正交矩阵，有如下定理：

 

 

2.2.5 最终优化

 

2.2.6 补充：柯西不等式证明

 

3 ICP存在问题

3.1 实现难点

经过上面的步骤，其实就可以得到R和T了，现在仍旧可能存在结果不够准确的问题。而在算法实现中，如果出现了求解值不准确的情况，一般的做法就是多次迭代。

1. 从B点云中一一找到A中点的对应距离最近点，构成最近点集C
2. 把C点集存入Eigen矩阵中，和A点云去中心化后，求SVD分解，得到R矩阵和T向量（一个旋转一个平移）
3. 开始迭代，通过R×A+T得到新的点云A1
4. 重新执行1到3步骤，这次是从B中找A1的最近点
5. 求得到的点云An和它的最近点集Cn的平均距离dst，当dst小于设定的阈值时，跳出循环

3.2 激光环境下ICP的难点

   在激光环境下，ICP很容易匹配出错，当ICP匹配出错时，说明它已经在进行匹配了，但是由于某些原因，它的迭代条件——也就是平均距离dst判断出错了，出现这种原因一般就是点云中出现了离散点，导致某两点的距离出现了异常，带动了整个dst判断出错。

解决方案如下：

1. 遍历A点找寻最近点，如果A中的某个点Ai和它的最近点距离大于某个阈值，则剔除，不参与接下来的计算。
2. 从B点云中一一找到A中点的对应距离最近点，构成最近点集C
3. 把C点集存入Eigen矩阵中，和A点云去中心化后，求SVD分解，得到R矩阵和T向量（一个旋转一个平移）
4. 开始迭代，通过R×A+T得到新的点云A1
5. 重新执行1到4，每次执行都要剔除一下离散点。
6. 求得到的点云An和它的最近点集Cn的平均距离dst，当dst小于设定的阈值时，跳出循环

4 ICP变种

4.1 PL-ICP

PL-ICP相对于ICP最大的区别是其改进了误差方程。ICP是点对点的距离作为误差而PL-ICP是采用点到其最近两个点连线的距离。

主要优势及不同如下：

1）误差函数的形式不同，ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差；PL-ICP的误差形式更符合实际情况。

2）收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛。

3）PL-ICP的求解精度高于ICP，特别是在结构化环境中。

4）PL-ICP对初始值更敏感。不单独使用。其容易陷入局部循环.

 

图4.1-1 PL-ICP流程图

4.2 N-ICP

NICP方法与ICP方法的主要流程和思想是一致的。但是它在误差项里考虑了更多的因素。它充分利用实际曲面的特征来对错误的点匹配进行滤除，主要的特征为法向量和曲率。在误差项里除了考虑对应点的欧氏距离之外， 同时还考虑对应点法向量的角度差。这也导致NICP方法在开源领域，目前是效果最好的ICP匹配方法。

主要的优势及不同如下：

1）当迭代过程收敛即得到需要的解。

2）由于在寻找点匹配的过程中，考虑了环境曲面的法向量和曲率，因此可以提前排除 一些明显是错误的匹配。

3）在误差定义中，除了考虑欧氏距离之外，还考虑了法向量之间的距离，因此具有更加准确的角度。

4）在开源领域，效果最好的ICP匹配方法。