ICP 算法

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

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

想象三维空间中两组点云PL（参考点） 以及 PR（目标点）：

1. 在PL和PR中寻找最近点（对于稀疏点云的微小运动，寻找欧拉空间最近点；对于密集点云或者较大运动，可能需要寻找描述子之间距离的最近点），这一步，不一定需要配准所有的点；

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

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

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

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