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

ゝ一纸荒年。 2022-09-06 04:53 496阅读 0赞

概述：

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

4.1 ICP

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70][]

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

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1][]

证明：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 3][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 4][]

求出R 和t

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 5][]

ICP 存在的问题：

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

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

4.2 PL-ICP 匹配方法

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 6][]

PL\_ICP的基本思想：

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 7][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 8][] ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 9][]

论文中的步骤：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 10][]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 11][] 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源码][CSM]  
参考论文：An ICP variant using a point-to-line metric

4.3 NICP算法

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 12][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 13][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 14][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 15][]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 16][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 17][]

雅克比求解泰勒展开；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**收敛到了曲线上，符合真实情况，拟合结果稳定，误差尺度更好**；

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 18][]

具体措施：

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**激光：选择平面；

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 19][]

4） 曲面重建

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 20][] ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3p6cjEwMjQ_size_16_color_FFFFFF_t_70][]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3p6cjEwMjQ_size_16_color_FFFFFF_t_70 1][]

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

5）匹配求解

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 21][]

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

参考网址：  
[IMLS-SLAM 论文学习][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的理解][ICP_PL-ICP_NICP_IMLS-ICP]

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

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70]: /images/20220829/ba4be713c0dd4d15a1364f228ff97bc2.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1]: /images/20220829/89434edb41cb4c2cb9a84ffac8014d6a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2]: /images/20220829/c9ab92a16d9541d0be837c255242fd71.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 3]: /images/20220829/2b4740bbc5c94d9dbd03514a45f354b7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 4]: /images/20220829/7d06a6c86ea944d7bfe3a327d9370071.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 5]: /images/20220829/e49810debec942f293566404e5c9fcb9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 6]: /images/20220829/00189ff00cdb4227b033ad6b3a3d631c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 7]: /images/20220829/ead2cf65c3f946c88f33e5a39791b25c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 8]: https://img-blog.csdnimg.cn/20210819233847118.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU=,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 9]: /images/20220829/d0d27dd27548475d807af2d18d698721.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 10]: /images/20220829/56d3f912514c4a0188e3db2e137727a5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 11]: /images/20220829/24e4326a7899494082f34d83602d4bbc.png
[CSM]: https://github.com/AndreaCensi/csm
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 12]: /images/20220829/84a418e270e24959ae552f771491e2d6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 13]: /images/20220829/2ae6ad46ba6e4472aa13d3768a006404.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 14]: /images/20220829/05a24cbd246c4149881128ae5ab853f0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 15]: /images/20220829/9f561d6c4aad411db93cffa0c6c6bcd0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 16]: /images/20220829/99ee6d8e79c74fbd956d24a3dad642cc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 17]: /images/20220829/305bd194ae79439a8a422a7ef16e52d9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 18]: /images/20220829/8607ff010aad41e5bd49d714ce9f5468.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 19]: /images/20220829/0e3c854823b64d6e9a9916aa2ad9cde3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 20]: https://img-blog.csdnimg.cn/2021082000081988.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU=,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3p6cjEwMjQ_size_16_color_FFFFFF_t_70]: /images/20220829/1fc3df4e65744ea29740a8321e81b359.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3p6cjEwMjQ_size_16_color_FFFFFF_t_70 1]: /images/20220829/19146b0313c74435bd73b38609e92fff.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 21]: /images/20220829/e81c315b7b8348edad4c92ed3f18b0d6.png
[IMLS-SLAM]: https://blog.csdn.net/shuang_yu_/article/details/88978235
[ICP_PL-ICP_NICP_IMLS-ICP]: https://blog.csdn.net/shoufei403/article/details/102972842?utm_medium=distribute.pc_relevant_t0.none-task-blog-2~default~BlogCommendFromMachineLearnPai2~default-1.base&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2~default~BlogCommendFromMachineLearnPai2~default-1.base
[SLAM_SLAM]: https://blog.csdn.net/qq_21830903/article/details/104694431?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_title~default-0.base&spm=1001.2101.3001.4242