PLICP

介绍

PLICP相比较于普通ICP算法，使用点线之间的距离作为度量，最终找到一个最小化该度量的闭式解（解析解）。 最优结果以平方的速度收敛。相比较于ICP，IDC，MBICP。PLICP更加准确，且需要更少的迭代次数。缺点是对于大的初始化误差无法给出很好的结果。

对于传统的ICP算法，其原理见公式(1)，即求解一个位姿使得当前帧通过该位姿变换到参考帧与参考帧间的误差最小化。普通算法无法找到闭式解，由于投影面的任意性以及旋转平移变换的非线性。而迭代之后投影点的计算可以使用上一帧的位姿，最小化当前帧重投影误差。在每一步都可以找到一个闭式解，具体见公式(2)。ICP算法的缺点在于无法解决初始化太差的问题、收敛太慢、外点频繁、不适合概率框架。

该文章使用点线距离作为度量，(n_i)为投影点所在平面的法向量，点线度量公式：

公式3

具体算法

1. 通过当前估计的位姿(初始化为(q_0)，可以从里程计中获取)，计算第二帧点在第一帧下映射的位置。公式(4)。
2. 对于每一个映射过去的点，在第一帧中找到两个最近的点，这两个点可以构成一条线段。
3. 使用修剪程序除去外点（文献9）
4. 构建误差函数，最小化点到线段之间的距离，优化出新的位姿（公式5）

关于二阶收敛速度：

文献10中已经证明了ICP算法是一阶收敛的（公式6）：误差一直降低，存在局部最小值。PLICP的收敛和高斯牛顿法相似（公式7）。

剩下的部分就是一些证明了。

A

既然是闭式解，为什么是迭代优化的方法，这不是数值解？

找到最近点对应的线段后，怎么到法向量(n_1)。