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

cocapop

已于 2022-12-09 16:48:47 修改

阅读量610

点赞数

分类专栏： 6D姿态估计点云文章标签：计算机视觉

于 2022-12-09 15:11:34 首次发布

本文链接：https://blog.csdn.net/cocapop/article/details/128253518

版权

10 篇文章 0 订阅

订阅专栏

7 篇文章 4 订阅

订阅专栏

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

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

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

在没有误差的情况下，计算出旋转R和平移t，可以将 $P_{s}$ 转换到 $P_{t}$ ，转换公式如下: $p_{t}^{i}=R\cdot p_{s}^{i}\dotplus t$

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

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

ICP一般流程：

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

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

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

4、剔除不合理的对应点

5、计算loss

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

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

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

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

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

💛所以：加速方法：

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

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

求解最优变换的步骤：

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

原始的 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 的性能更好。