ICP PL-ICP NICP IMLSICP

imlsicp:最小化  

这里用了point to plane的概念

xj是源点云中的点，yj是源点云的点投影到目标点云隐藏的曲面的投影点  

最小化 源点云的点到其投影点之间的距离在法向量上的投影，这个法向量与xj 最近的目标点云中点的法向量 

固定一个迭代次数和收敛精度。每求出一次R t ，将这个转换矩阵作用于源点云即新到点云更新，然后再进入循环进行又一次迭代计算，直到到达收敛精度退出或迭代次数退出

xj:源点云点 即新过来的一帧点云

yj:xj投影在目标点云隐藏曲面上的投影点

nj: 距离xj最近的目标点云中的那个点的法向量

参考：

《Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration 》

《IMLS-SLAM: scan-to-model matching based on 3D data》

PL-ICP

1.新来的激光帧点云经过一个R和t变换，得到点云 xj * R + t = xj'

2.在目标点云(指的是之前的上一帧或几帧的点云)中找到与xj'最近的两个点 j1, j2，这个可以用PCL库的kd-tree模块得到，参考：https://pcl.readthedocs.io/projects/tutorials/en/latest/kdtree_search.html#kdtree-search

3. pl-icp迭代公式，与ICP迭代公式比较下，就多了一个n这个方向量。

其原理是，连接转换后的点xi与最近的点中的一个，这个连线长度在这个法向量上的投影。

4.最小化这个J。J是点xj'到两个最近点连线的距离在连线法向量的投影

具体代码可以参考csm和scan_tools里的laser_scan_matcher

 论文《An ICP variant using a point-to-line metric》