ICP 算法

ICP 算法是一种点云到点云的3D-3D配准方法。

在SLAM中通过空间点云的配准（可以通过相机或者3D激光雷达获取点云数据），可以估计相机运动（机器人运动，旋转矩阵R与平移向量t），累积配准，并不断回环检测，可以保证机器人定位的精度。

想象三维空间中两组点云PL（参考点云，固定不动） 以及 PR（目标点云，需要调整）：

1. 3D点匹配：在PL和PR中寻找一一对应的最近匹配点（对于稀疏点云的微小运动，寻找欧拉空间最近点；对于密集点云或者较大运动，可能需要寻找描述子之间二进制汉明距离的最近点）注意理解：这里最近点的意思是在各自点云坐标系中的坐标距离最近，而不是同一个坐标系下的空间距离！这一步，不一定需要配准所有的点；

2. 优化初值：通过初始配准的两个点集，计算各自点集质心的三维坐标 L0 与 R0, 通过这两个点的三维运动计算出相机运动初值；

　　// 或者寻找R，t，使得目标点集和参考点集之间距离的最小二乘（二范数）最小；

3. 迭代优化：由于初值匹配比较粗糙，通过初值变换获取的 PR‘ 与真实的 PR点集之间存在误差。迭代的目的就是减小这个最小二乘的误差，直到小于阈值或者达到一定迭代次数。

 

其目的是通过测量数据，获取机器人三维位姿变换的准确值。其中数学核心是奇异值分解，在常见的PCL库中有实现。并且ICP有不同的具体实现方法，例如利用kd树实现subset与subset之间的配准，提高效率。