SLAM 前端匹配算法 NICP

ICP算法利用点云之间距离构建误差函数，PL-ICP利用点到线的距离构建误差函数，可以总结为都是利用欧式距离来进行匹配，而获得的数据中还包括法向量及曲率数据。实际上，以在点云匹配前，可以使用法向量和曲率数预先淘汰一部分质量不高的畸点，达到提高精确度和匹配速度的目的，NICP就是这个原理。

1.基本思想

• a.替换原始ICP对应点匹配的方法
• b. 充分的利用实际的曲面的一些特征来对错误的匹配点进行滤除，主要是法向量（车体坐标系下的法向量）及曲率。
• c.误差项除了对应点的欧氏距离外，还加入了对应点的法向量的角度差

b：曲率类似于语义信息，直角跟半圆、圆弧的曲率不相同，因此肯定不能匹配；而法向量的作用如图所示：当误差比较大时，ab表示的直角跟a’b’表示的直角是同一个物体，因此他们的法向量肯定是对应相等的，用a与a’的法向量进行匹配是可以的，但用长边b与短边a’进行匹配，肯定就是错的。

c.在误差函数中添加法向量角度误差，是因为对于轮式里程计来说，旋转比平移更重要，因为旋转会导致很大的平移误差，而平移则不会导致角度误差。

2.数学描述

Ω：表示信息矩阵，可以理解为权重，即看重欧式距离的误差，还是更看重法向量的角度差。

3.求解法向量和曲率

找到点p周围半径为R的范围内的所有点云
假设点云分布在一条直线上，这些点云服从高斯分布，则沿着直线的曲率比较大，垂直于曲线的曲率小。

计算点云的中心：

计算方差：

对方差进行特征值分解

则曲率可以定义为：

其中是较小的特征值

法向量：最小的特征值对应的特征向量，就是法向量。

4.点匹配的规则

• 没有 well define的点就拒绝，就是杂点拒绝

• 两点之间的距离大于阈值，拒绝

• 两点之间的曲率之差大于阈值，拒绝

• 两点之间的法向量之差大于阈值，拒绝
  

5.目标函数的求解

当迭代过程收敛，则得到最终解

6.算法流程

小节

• 由于在寻找点匹配的过程中考虑到了环境中的曲面的曲率和法向量，因此能排除很多明显是错误的点
• 误差函数除了考虑欧氏距离，还考虑到了法向量的角度差，因此角度更准确
• 开源领域最优的ICP匹配方法，开源的是3D的NICP