SLAM 前端匹配算法 PL－ICP

PL-ICP 实际就是point-to-line ICP,是点到线的ICP方法．相比于传统的ICP方法,即点到点的匹配方法,PL－ICP在算法方面实际上是对误差形式进行了改进,其他方面基本相同,但迭代速度以及匹配精度得到了很大的提升

1.基本思想

室内环境通常是结构化环境，即譬如墙壁等有众多规则的曲面，而激光数据实际是对实际环境中曲面的离散采样，因此，最好的误差尺度就是点到实际曲面的距离。我们目前通过传感器已经知晓激光点云数据，实际的重点就是对环境中曲面的恢复。PL-ICP采用的是用分段线性的方式近似替代曲面，从而获得激光点到实际曲面的距离。

图（a）上的折线就是采用分段线性方式对曲面（黄色曲线）的近似，其中黑色折线是近似折线的势场，淡黄色的折线是曲面的势场，黑色虚线是折线的梯度方向，可以看出沿着梯度的方向，中间两个点的势场与黄色曲线的势场逐渐重合，近似变成了直线。因此，用分段折线的方式近似，虽然存在一定的误差，但误差很小，能够接受。图b是点对点的势场。匹配点点到参考点的距离永远是圆的直径。而图c中,当P点位于同一势场上移动一段距离，但点到

线段的距离，仍等于p_i到该线段的距离。因此，与ICP相比，PL-ICP的误差形式更加的合理。

2.数学推导

ICP的目标函数为:
其中,
公式的作用就是把当前激光帧p根据初始位姿投影到参考点云的坐标系下

实际上,
这个式子表示的就是p在曲面上的投影点p_1.然后将两点做差就可以得到两点之间的向量,然后与直线的法向量n叉乘,就可以得到点p到直线的距离.

3.求解步骤

已知数据:

• 参考激光帧 y_t-1
• 当前激光帧 y_t
• 参考激光帧生成的曲面 S
• 初始解p_0

代求数据:

• 两帧激光之间的相对位姿关系

求解步骤:
1 把当前帧的数据根据初始位姿投影 到参考帧坐标系下。

因为两点确定一条直线

3. 计算直线误差，并去除误差过大的 点。

4. 最小化误差函数:

4. ICP对比

1.误差函数的形式不同，ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差； PL-ICP的误差形式更符合实际情况(因为激光雷达是不断旋转的,前一帧和后一帧扫描到同一个物体的可能性很低,但前后帧扫描到同一个物体\同一个曲面的可能性很大)。

2 收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛。

3 PL-ICP的求解精度高于ICP，特别是在结构化环境中。结构化环境实际就是人造环境,存在众多的规则平面.

4PL-ICP对初始值更敏感。不单独使用，与里程计、CSM等一起使用(Cartographer就使用CSM来优化ceres的初试位姿)