激光SLAM学习笔记：ICP、PL-ICP、NICP和IMLS-ICP的理解

激光SLAM学习笔记：ICP、PL-ICP、NICP和IMLS-ICP的理解

点云匹配算法是为了匹配两帧点云数据，从而得到传感器（激光雷达或摄像头）前后的位姿差，即里程数据

---

---

前言

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、ICP是什么？

1.数学描述

主要分为已知对应点的求解和未知对应点的求解，两种问题。

• 已知对应点，求解如下：
  

2.明显缺陷

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

二、PL-ICP

1.基本思想

用分段线性的方法来对实际曲面进行近似，用激光点到最近两点的距离来模拟实际激光点到曲面的距离
最好的误差：当前激光点到曲面的距离，关键在于如何恢复曲面。

2.方法

1）给定一个初始的转换矩阵 q 0 q_{0} q0​，将当前激光帧的数据转换到参考帧坐标系下。初始的转换矩阵 q 0 q_{0} q0​一般通过里程计来获得。后面迭代计算所需的 q k q_{k} qk​由上一次算法迭代计算得到。
2）为当前激光帧中的每一个点，找到其最近的两个点j1和j2。
3）去除误差过大的点。
4）构建最小化误差方程。
5）求解出位姿转换矩阵 q k + 1 \boldsymbol{q}_{k+1} qk+1​。然后将其用于下次迭代计算。

3.主要缺点

三、NICP

1.基本思想

替换原始ICP方法中对应点匹配的方法，充分利用曲面的特性法向量和曲率，对错误的匹配进行滤除。
考虑了欧式距离以及点向量的角度差。

2.方法

算法流程：

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

四、IMLS-ICP

1.基本思想

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