ICP算法总结

ICP（迭代最近点）是一种点云精配准算法，用于求解两组点云间的旋转矩阵R和平移向量t。其核心流程为：1)对源点云Q中的每个点，在目标点云P中寻找最近邻作为对应点；2)通过SVD或非线性优化最小化对应点距离平方和，计算最优R、t；3)用新变换更新Q并迭代至收敛。该算法依赖粗配准提供初始值，且计算效率受最近邻搜索策略影响。ICP广泛应用于三维重建、SLAM等领域，需注意噪声和误匹配可能导致局部最优解。

不会编程的刘同学

889人浏览 · 2025-08-19 21:59:42

不会编程的刘同学 · 2025-08-19 21:59:42 发布

ICP算法原理 ：给定一个参考点集 $P$ 和一个数据点集 $Q$ （在给定的初始估计 $R$ ， $t$ ），算法为 $Q$ 中的每个点寻找 $P$ 中对应的最近点，形成匹配点对。！！！！（这就是匹配点对的来源）。然后，将所有匹配点对的欧氏距离之和作为待求解的目标函数，利用奇异值分解求出 $R$ 和 $t$ 以使目标函数最小，根据 $R$ , $t$ 转换得到新的 $Q$ ，并再次找到对应的点对，如此迭代。
ICP的基本原理数学表达
设：

参考点集 P （固定不动）
数据点集 Q （要移动）
初始估计（旋转 R，平移 t）
ICP 的步骤：

最近邻搜索：对于 Q 中的每个点 q，找到 P 中距离最近的点 p。这个 (q, p) 就当作一对“对应点”。
刚体变换估计：在这些对应点对的基础上，计算最佳的旋转矩阵 R 和平移向量 t（在粗配准计算出的对应关系上做），使得 $_i∥Rqi+t−pi∥^2$ 最小。
更新 Q：把数据点集 Q 按新的 R、t 变换。
迭代：重复 1-3，直到收敛（误差变化很小，或达到最大迭代次数）。

👉 这就是“Iterative Closest Point”的来源：迭代最近点。

什么是ICP算法

由于三维扫描仪设备受到测量方式和被测物体形状的条件限制，一次扫描往往只能获取到局部的点云信息，进而需要进行多次扫描，然后每次扫描时得到的点云都有独立的坐标系，不可以直接进行拼接。在逆向工程、计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需要对多个局部点云进行配准。为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是 点云数据的配准 。

点云配准步骤上可以分为 粗配准 （Coarse Registration）和 精配准 （Fine Registration）两个阶段。

粗配准 是指在点云 相对位姿完全未知 的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。

精配准 是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的 初始变换估计 以及在迭代过程中 对应关系的确立 。所以需要各种粗配准技术为ICP算法提供较好的位置，在迭代过程中确立正确对应点集能避免迭代陷入局部极值，决定了算法的收敛速度和最终的配准精度。

总结：ICP算法是一种精配准方法，需要粗配准来提供初始变换估计

ICP算法的数学模型

假设我们通过 RGB-D 相机得到了第一组点云 $\{p_1, p_2, p_3, \cdots, p_n\}$ ，相机经过位姿变换（旋转加平移）后又拍摄了第二组点云 $\{q_1, q_2, q_3, \cdots, q_n\}$ ，注意这里的 $P$ 和 $Q$ 的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为相机光心，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得 $P$ 和 $Q$ 中的点一一对应，如 $P_{99}, Q_{99})$ 在三维空间中对应同一个点。

现在我们要解决的问题是：计算相机的旋转 $R$ 和平移 $t$ ，在没有误差的情况下，从 $P$ 坐标系转换到 $Q$ 的公式为：

$q_i = R p_i + t$

由于噪声及错误匹配（如 $P_{99}, Q_{99})$ 其实并不对应空间中同一点，但特征匹配算法错误地认为二者是同一点）的存在，上式不总是成立，所以我们要最小化的目标函数为：

$\frac{1}{n} \sum_{i=1}^{n} \left\| q_i - R p_i - t \right\|^2$
ICP的核心思想就是，求解旋转矩阵R和平移向量t,使得上式最小。

常用的求解 $R$ 和 $t$ 的方法有（常见的 ICP 解法）：

估计对应点（Correspondences estimation）

在前面数学模型中，我们假设了P和Q中的点云一一对应，这个在实际应用中很难求得（总不能给每一个点提取描述子，计算量太大）。在实际应用中估计对应点的做法为：
设：

参考点集 P （固定不动）
数据点集 Q （要移动）
初始估计（旋转 R，平移 t）
最近邻搜索：对于 Q 中的每个点 q，找到 P 中距离最近的点 p。这个 (q, p) 就当作一对“对应点”。

对 Source 点云中的一点，求解其与 Target 点云中距离最近的那个点，作为其对应点。当然，这样操作的时间复杂度很高，为了加速计算，我们不需要计算 Target 点云中每个点到 Source 点云中一点的距离。可以设定一个阈值，当距离小于阈值时，就将其作为对应点。还有一些其他加速求解对应点的方法，如 ANN(Approximate Nearest Neighbor)。

ICP的计算方法

已知上述关系构建起数学模型之后，应该如何去求解呢？
ICP的求解方法分为两种：

利用线性代数的求解（主要是SVD）
以及利用非线性优化的求解（类似于BA）
具体可参考视觉SLAM十四讲第二版7.9节，这里就不赘述

注意！：SVD 、非线性优化（类似于 BA 优化）这些只是ICP的解法

参考链接
https://blog.csdn.net/qq_32761549/article/details/122538257
https://zhuanlan.zhihu.com/p/107218828
https://blog.csdn.net/sinat_52032317/article/details/130441840