激光slam理论与实践 - 第四章 前端配准（ICP）

概述：

点云匹配算法是为了匹配两帧点云数据，从而得到传感器打的前后位置差。匹配算法从最初的ICP方法已经发展出了多种改进的算法，他们分别从配准点的需要，误差方程等方面进行了优化下面分别介绍：

4.1 ICP

也叫点对点PP-ICP，基本思想是：

如上图所示，求解旋转矩阵R 和平移矩阵t 使得误差最小。

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

证明：

 

 求出R 和t 

在实际工程中一般不知道两个点云是 如何配对的，只能通过迭代求解的方法缩小误差，理论上点云完全重刻即E (R,t)最小。

ICP 存在的问题：

两帧激光点云数据中不可能表示的是空间中相同的位置。所以用点到点的距离作为误差方程必然会引入随机误差。

更详细的资料可以 参考论文：Least-Squares Fitting of Two 3-D Point Sets 

4.2 PL-ICP 匹配方法

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

PL_ICP的基本思想：

1. 激光点是对实际环境中曲面的离散采样
2. 重要的不是激光点,而是隐藏在激光点中的曲面
3. 最好的误差尺度为当前激光点到实际曲面的距离;关键的问题在于如何恢复曲面
4. PL-ICP的答案:用分段线性的方法来对实际曲面进行近似,从而定义当前帧激光点到曲面的距离

 

论文中的步骤：

论文中关于 PL-ICP与 ICP的区别

 PL-ICP 容易陷入局部最小，所以通常是用里程计得到一个初始转换矩阵 q0 给到PL-ICP作为初始值。

csm源码包分析：
ros的csm包实现了ICP和PL-ICP算法。源码包和论文下载地址如下：
https://censi.science/software/csm/
csm包的作者给出了一个该功能包的操作说明文件（csm_manual.pdf）。里面详细描述了各项配置参数的含义。
其中sm/app文件夹中的sm0.c sm1.c sm2.c sm3.c 相当于是几个使用示例。
主要的算法实现是在csm/icp文件夹中的几个文件里。论文中的所有算法步骤完整的体现在了icp_loop.c文件中的icp_loop函数里。
CSM源码
参考论文：An ICP variant using a point-to-line metric

4.3 NICP算法

NICP方法与ICP方法的主要流程和思想是一致的。但是它在Trim outlier和误差项里考虑了更多的因素。这也是它效果更好的原因。它充分利用实际曲面的特征来对错误点进行了滤除，主要使用的特征为法向量和曲率。在误差项里除了考虑了点到对应点切面的距离，还考虑了对应点法向量的角度差。目前NICP方法开源的代码主要是针对3D点云的，其调用了Eigen库和OpenCV库。源码中显示的部分调用了QT5。既然NICP方法考虑了法向量和曲率，那么就涉及到了如何求解点的法向量和曲率。

 

点云匹配的规则就是杂点不匹配。

雅克比求解泰勒展开；LM迭代求解，直到收敛。

 算法流程总结：

1. 计算参考激光帧和当前激光帧中每一个点的法向量和曲率。（高斯拟合，然后用svd奇异值分解，然后求曲率，最小特征值对应的特征向量就是法向量）
2. 根据当前解,把当前激光帧的点转换到参考坐标系中,并且根据欧式距离、法向量、曲率等信息来选择匹配点(也有可能没有匹配点)。
3. 根据上面介绍的方法,用LM方法进行迭代求解,迭代收敛即可得到两帧激光数据之间的相对位姿。

 对NICP的总结：
1）由于在寻找点匹配的过程中，考虑了环境 曲面的法向量和曲率，因此可以提前排除 一些明显是错误的匹配。这样就减少了计算量并且提高了计算结果的精度。
2）在误差定义中，除了考虑欧式距离之外，还考虑了法向量之间的夹角，因此具有更加准确的求解角度。
3）用LM方法进行迭代求解目标误差方程，迭代收敛即可得到两帧激光数据之间的相对位姿。
源码地址
参考网址：http://jacoposerafin.com/nicp/

4.4 IMLS((Implicit Moving Least Square))ICP

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

IMLS：隐式的移动最小二乘；

• Implicit 隐式，由于实际的曲面的显式方程难以求出，实际是求点到曲线的距离，拟合一曲面。
• Moving 滑动窗口法：
• Least Square 最小二乘法求解；

理论上最好的算法，因为对实际环境的拟合是最真实的，精度最高。

目前没有开源的slam的ros算法包；

曲面重建越准确，对真实世界描述约准确，匹配的精度就越高。

ICP和IMLS的对比：

1、ICP收敛到了一团点上，收敛结果不稳定；

2、IMLS-ICP收敛到了曲线上，符合真实情况，拟合结果稳定，误差尺度更好；

具体措施：

1）scan egomotion
egomotion的定义如下：

进行激光数据的运动畸变去除。在雷达扫描一圈的过程中，车子是在运动的。这会造成一帧激光的时间内，每束激光测量时车子实际的位置是不同的。论文里假设两次连续的激光扫描间隔，车子的egomotion是相似的。所以采用上一次的相对位移来计算当前的实际的egomotion。
2）去除动态障碍物
在匹配scan和model之前，我们要去除scan中所有移动物体。我们采用去除所有可能会动的小物体的方法：首先，删去地面点云，聚类，并抛弃所有（bounding box的）长小于14m，宽小于14m，高小于4m的物体。剩下的结构足够我们匹配。最后添上去掉的地面点云。
3）特征点的选取
选取思路：

• 具有丰富特征的点，去即为结构化的点：具有良好的曲率和法向量的定义。去除不好的杂点。
• 曲率越小的点越好，因为曲率为0代表着直线，代表着最结构化的点，也代表着具有非常好的法向量定义，能够提供足够的约束。
• 选点的时候需要注意选取的激光点的均衡以保证可观性，（因为激光常常会出现点分布不均匀的情况，可能会让最终匹配结果偏向特征点丰富的一边，令匹配失真；）因为是平面匹配，不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的， 才能让结果不出现偏移。（角度不可观一般遇不到，除非是一个非常圆的环境，没有别的特征，难以分辨方位）

大概率上，随着角度的偏转，观测的点云是不一样的。所以角度一般是可观的。对于二维SLAM，只需保证X方向和Y方向上选取的点云数量接近就可以。而对于三维则需要考虑6个维度（X,Y,Z,roll,pitch,yaw）。

代表点的选择策略：

• 2D激光：选择直线；
• 3D激光：选择平面；

4） 曲面重建

 

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

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

5）匹配求解 

总结：
IMLS-ICP使用高斯拟合和最小二乘重建出一个隐含的曲面。找到空间点在隐含曲面的投影点。使用点到该曲面上投影点间的距离构建误差方程。

参考网址：
IMLS-SLAM 论文学习
IMLS-SLAM: scan-to-model matching based on 3D data,Jean-Emmanuel Deschaud

ICP方法初始值的设定方法
一个好的初始值可以减少icp迭代的次数，提高效率和效果。初始值的设定可以从下面三个角度来考虑：
1.、假设机器人静止，设置帧间位移为0。
2、假设机器人匀速运动，利用上一时刻机器人的速度和时间，计算当前时刻机器人位移间隔，将此作为初值传入icp
3、利用传感器如里程计、IMU等，计算前后两时刻的初始位移。实际使用时要注意坐标系的转换。因为icp方法实在激光坐标系下进行的。

提升ICP方法精度和速度的一些考虑
精度上：
1）使用其他传感器或方法（如里程计、IMU）提供初值（coarse to fine）。
2）Ransac框架，去除outlier。
效率上：
1）选取合适的采样点。IMLS-ICP的论文里就提到了一些去除误差点的方法。
2）使用合适的数据结构，提高程序的运行效率。

参考链接：

对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解

激光SLAM——第四节（激光SLAM的前端配准方法）