对ICP、PL-ICP、NICP、IMLS-ICP匹配算法的解析

1.ICP(Iterative Closest Point)

通过迭代最近点来实现数据的匹配，ICP方法是用来求解两个点云集合转换关系的最通用的方法

数学描述：

如下图所示，我们需要求解出R,t，使得两个点云集合匹配度达到大（理想效果是两个点云完全重合），即E(R,t)最小。

对于已知两个点云的对应点，一次迭代即可求出R,t。

但在实际中，对于两个点云，我们并不能确定它们的对应点。所以不能一步到位计算出Ｒ和ｔ，需要进行多次迭代的方法一步步缩小误差，最终得到使误差方程最小的旋转矩阵R和平移矩阵t。算法流程如下：

1)寻找对应点

通常使用编码盘的里程计数据得到位姿差，即当前机器人在上次机器人坐标系中的位姿。将此R和t作为ICP算法的first guess，帮助算法寻找点云对应点。

2)根据对应点，计算R和t。

3)对点云进行位姿变换，计算误差。

4)不断迭代，直至误差小于某一值。

迭代流程示意图如下：

这里指出ICP的一个明显缺陷：
由于激光点是对实际环境中曲面的离散采样，所以相邻两帧激光点云数据中的点不可能表示的是空间中相同的位置。所以用点到点的距离作为误差方程势必会引入随机误差。

2.PL-ICP(Point to Line-Iterative Closest Point)

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

PL-ICP方法示意图：图(a)中棕色的曲线表示实际场景的墙，蓝色是某时刻的激光扫描点，可以看出，激光点是对实际环境的离散采样。对于匹配来说，重要的不是激光点，而是隐藏在激光点中的曲面。最好的误差尺度为当前激光点到实际曲面的距离，关键的问题在于如何恢复曲面。PL-ICP的思想：用分段线性的方法来对实际曲面进行近似，用激光点到最近两点连线的距离来模拟实际激光点到曲面的距离。

数学描述如下：

算法流程如下：

总结PL-ICP与ICP的主要区别：
1）误差函数的形式不同，ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差；
PL-ICP的误差形式更符合实际情况。
2）收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛。

3）PL-ICP的求解精度高于ICP，特别是在结构化环境中。
4）PL-ICP对初始值更敏感。不单独使用，通常于里程计、CSM等一起使用。

3.NICP((Normal Closest Point)

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

算法流程总结：

计算参考激光帧和当前激光帧中每一个点的法向量和曲率。
根据当前解，把当前激光帧的点转换到参考坐标系中，并且根据欧氏距离、法向量、曲率等信息来选择匹配点(也有可能没有匹配点)。
根据上面介绍的方法，用LM方法进行迭代求解，迭代收敛即可得到两帧激光数据之间的相对位姿。

对NICP的总结：

当迭代过程收敛即得到需要的解。
由于在寻找点匹配的过程中，考虑了环境曲面的法向量和曲率，因此可以提前排除一些明显是错误的匹配。
在误差定义中，除了考虑欧氏距离之外，还考虑了法向量之间的距离，因此具有更加准确的角度。
在开源领域，效果最好的ICP匹配方法。

4.IMLS(Implicit Moving Least Square)-ICP匹配方法

IMLS-SLAM是一个仅依赖点云数据的低漂移（low-drift）SLAM算法。其依赖于一个scan-to-model的匹配框架。这里的model可以认为是对点云进行的局部曲面建模。

基本思想：

选择具有代表性的激光点来进行匹配，既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移。
点云中隐藏着真实的曲面，最好的做法就是能从参考帧点云中把曲面重建出来。
曲面重建的越准确，对真实世界描述越准确，匹配的精度就越高。

实现方法：

1)代表点(Informative Point)的选取

具有丰富特征的点，即为结构化的点：具有良好的曲率和法向量的定义。
曲率越小的点越好，因为曲率为0代表着直线，代表着最结构化的点，也代表着具有非常好的法向量定义，能够提供足够的约束。
选点的时候需要注意选取的激光点的均衡以保证可观性，因为是平面匹配，不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的，才能让结果不出现偏移。

2)曲面重建

我们认为激光点云是分布在真实曲面的附近，并可以用高斯分布描述。如下图

权重W在这的作用就是只取点x附近的一部分点使用上面的公式重建曲面，而在曲面上离点x比较远的就不需要考虑了,距离越远，权重W就越小。

3)匹配求解