– 第四章 激光的前端配准算法（ICP）
4.1、2、3、4的ICP方法，一个比一个精度高，计算量也大。
点云匹配算法是为了匹配两帧点云数据，从而得到传感器（激光雷达或摄像头）前后的位姿差，即****里程数据。匹配算法已经从最初的ICP方法发展出了多种改进的算法NDT。他们分别从配准点的寻找，误差方程等等方面进行了优化。下面分别介绍：
4.1 ICP匹配方法
点对点的icp，可以称PP-icp
- Iterative closest point：迭代最近点（最原始的icp算法）
- 简介：前端匹配（配准），后端优化；套路是固定的，即求解最小二乘解。基于图优化的slam如果做得精度高，那多半是前端配准做得棒，才可以拉开差距。
- 目地：配准两帧点云，求欧式变换（R、t变换）；
- 意义：两个安装方法、角度不同的雷达，对同一个实体看到的样子是不一样的（距离、姿态等），设为两个雷达探知到的同一个实体位姿为A、B，求A、B位姿的变换，（R、t变换、欧式变换问题）
- 两帧点云集合X、P：
– 4.1.1 已知对应解的求解方法–证明
- 一般是没办法不迭代就知道对应解的；
勘误：假设矩阵A为正定对称矩阵
– 4.1.2 未知对应点的求解方法
笔记的3.2.4同此；
迭代最近点求解流程（ICP通用方法）：
- 1、寻找对应点（需要初始输入的R、t），由R、t求‘对应点’；
- 2、根据对应点，再求解进一步的R、t，由‘对应点’求解R、t；
- 3、对点云进行转换，计算误差；
- 4、不断重复123点，迭代求解，直到误差小于某阈值；
将匹配的方法求解成EM算法思想；
4.2 PL-ICP匹配方法
后面的ICP的优化方法，都是基于这个优化思想。
- PL：point to line
- 示意图：
- a黄色表示实际曲线，蓝色表示激光采样的点；黑色表示和黄色近似的‘曲面’。
- b是点对点，势场是圆，往四周扩展递增，和实际不符合；
- c是点对线的势场模型，势场都是平行的，往外递增，比较符合实际。
– 4.2.1基本思想：
- 1、激光点是对实际环境的曲面的离散采样；
- 2、重要的不是激光点，而是隐藏在激光点中的曲面；
- 3、最好的误差尺度为当前的激光点到实际曲面的距离，关键的问题在于如何恢复曲面；
- 4、PL-ICP的解答思路：用分段线性的方法来对实际曲面进行近似，从而定义当前帧激光点到曲面的距离；
– 4.2.2 数学描述：
- 目标函数：表示点到曲面的距离，即点到直线的距离
– 4.2.3 求解方法：
- 流程：
1、把当前帧的数据根据初始位姿投影到参考帧坐标系下；
2、对于当前帧的点i，在参考帧中找到最近的两个点（j1，j2）****，因为两点确定一线。
3、计算直线误差，并去除误差过大的点。
4、代入最小化误差函数求解，点到直线的误差。
– 4.2.4 PL-ICP和ICP的区别
4.3 NICP匹配方法
- **N：normal **法向量
简介：原始的论文是运行在3D激光中的，不过变到2D是很容易的。
目前在开源领域最好的ICP方法；
– 4.3.1 基本思路：
1、替换了pp-icp的欧式最近距离特征。
2、充分利用曲面的特征来对错误的点匹配进行滤除，主要的特征为法向量和曲率。
3、误差项除了考虑对应点的欧式距离之外，同时还考虑对应点法向量的角度差。（有利于R旋转矩阵的求解，精度更高**；R比t的精度要求高**）
– 4.3.2 数学描述
– 4.3.3 法向量和曲率计算
高斯分布，描述点云的空间分布特性，即usi；
– 4.3.4 点匹配规则：
1、杂点不匹配
J的求解，泰勒展开；LM迭代求解收敛。
– 4.3.5 算法流程：
4.4 IMLS-ICP匹配方法
- IMLS：隐式的移动最小二乘；
- Implicit 隐式，由于实际的曲面的显式方程难以求出，实际是求点到曲线的距离，拟合一曲面。
- Moving 滑动窗口法：
- Least Square 最小二乘法求解；
理论上最好的算法，因为对实际环境的拟合是最真实的，精度最高。
目前没有开源的slam的ros算法包；
曲面重建约准确，对真实世界描述约准确，匹配的精度就越高。
步骤：
1、选取参考点；
2、曲面重构；
– 4.4.1 ICP和IMLS的对比：
1、ICP收敛到了一团点上，收敛结果不稳定；
2、IMLS-ICP收敛到了曲线上，符合真实情况，拟合结果稳定，误差尺度更好；
基本思想解释：
1、选择代表性点：选择信息量丰富的点，结构化的点。去除不好的杂点，因为激光常常会出现点分布不均匀的情况，可能会让最终匹配结果偏向特征点丰富的一边，令匹配失真；
– 4.4.2 代表点的选择策略：
- 2D激光：选择直线；
- 3D激光：选择平面；
解释：
1、选择结构化的点，具有良好的曲率和法向量，选择均衡，在曲面两侧数量基本平衡
2、角度不可观一般遇不到，除非是一个非常圆的环境，没有别的特征，难以分辨方位。
– 4.4.3 曲面重建方法
– 4.4.4 匹配求解
- 计算步骤：
- 当前帧中的一点xi到曲面的距离
；
- Pk中离点xi最近的点的法向量为ni；
- 则点xi在曲面上的投影yi为：yi=xi-Ipk(xi)*ni
- 点xi和点yi为对应的匹配点：
- 当前帧中的一点xi到曲面的距离
