（点、6）精配准Iterative Closest Point（ICP）

知乎ICP

ICP算法本文具体化

精配准：给定一个初始变换，进一步优化得到更精确的变换。ICP目的是把不同坐标系中的点，通过最小化配准误差，变换到一个共同的坐标系中。

为什么需要点云配准

通过RGBD相机获取两组点云，在待匹配的两组点云数据的重叠区域内，选取两组点云，一组点云
（源），相机经过位姿变换（旋转平移）之后拍摄第二组点云（平移后）。

理论上，两者内部的点应该是一一对应的,并且在存储的时候，按照顺序存储下标为i的像素。

在没有误差的情况下，计算出旋转R和平移t，可以将转换到 ，转换公式如下: 

但由于噪声存在，以及匹配错误存在，上面的公式不一定对于所有的点都成立，所以存在误差。

 ICP具体算法

1. 核心：最小化一个目标函数，即上面提到的误差。
2. 寻找对应点：我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对源点云进行变换，得到的一个变换后的点云。然后将这个变换后的点云与目标点云进行比较，只要两个点云中存在距离小于一定阈值，我们就认为这两个点就是对应点，称为最邻近点对。
3. R、T优化： 有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵。
4. 迭代： 我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者邻近点对不再变化等。

ICP一般流程：

1、点云预处理——滤波、数据清理

2、匹配——应用上一步求解出的变换，寻找最近点

3、加权——调整一些对应点的权重

4、剔除不合理的对应点

5、计算loss

6、最小化loss，求解当前最优变换

7、返回步骤2进行迭代，直到收敛

ICP算法的参数主要有两个：一个是ICP的邻近距离【邻近距离：最初使用较大的对应点距离参数，然后逐步减小到一个较小的值】，另外一个是迭代的终止条件。

寻找最近点

利用初始、或上一次迭代得到的,,对初始点云(源点云)进行变换，得到一个临时的变换点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点。

如果直接比较寻找最近邻点需要两重循环，耗时。

所以：加速方法：

 

• 设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集；

• 使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-Tree.KD-tree 建树的计算复杂度为 O(N log(N))，查找通常复杂度为 O(log(N))（最坏情况下 O(N)）

最优变换

这里的优化过程是一个贪心的策略。首先固定R跟T利用最邻近算法找到最优的点对，然后固定最优的点对来优化R和T，依次反复迭代进行。

求解最优变换的步骤：

1. 计算源点云和目标点云质心；

2. 将源点云和目标点云进行转换到质心坐标系；

3. 计算矩阵 H（形式类似“协方差矩阵”）；

4. 对 H 求 SVD 分解，根据公式求得；

5. 根据公式计算最优变换。

迭代

每一次迭代我们都会得到当前的最优变换参数  ，然后将该变换作用于当前源点云；“找最近对应点”和“求解最优变换”这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有：

•  的变化量小于一定值

• loss 变化量小于一定值

• 达到最大迭代次数

原始的 ICP 算法计算开销大，对初始变换敏感，容易陷入局部最优解。
自 ICP 提出以来，有相当多的 ICP 改进算法，简要列举一些：

• Point-to-Plane ICP，原始 ICP 算法的代价函数中使用的 point-to-point 距离，point-to-plane 则是考虑源顶点到目标顶点所在面的距离，比起直接计算点到点距离，考虑了点云的局部结构，精度更高，不容易陷入局部最优；但要注意 point-to-plane 的优化是一个非线性问题，速度比较慢，一般使用其线性化近似；

• Plane-to-Plane ICP，point-to-plane 只考虑目标点云局部结构， plane-to-plane 顾名思义就是也考虑源点云的局部结构，计算面到面的距离；

• Generalized ICP (GICP)，综合考虑 point-to-point、point-to-plane 和 plane-to-plane 策略，精度、鲁棒性都有所提高；

• Normal Iterative Closest Point (NICP)，考虑法向量和局部曲率，更进一步利用了点云的局部结构信息，其论文中实验结果比 GICP 的性能更好。