经典点云配准算法：迭代最近点算法ICP（Iterative Closest Point）

简介：

迭代最近点算法，又名为 ICP （ Iterative Closest Point ）算法 。该算法一般多用于三维空间内点云配准，不仅适用于平面场景，还适用于曲线和曲面等场景。当点云配准的结果未满足理想精度的要求，通过 ICP 算法，在不断地迭代中降低误差，从而实现理想的精度值。初始的粗略匹配为之后的精准匹配提供了较好的初始位置，而 ICP 算法的作用是把误差进一步的缩小，以无限接近理想精度值。

原理：

ICP 算法的核心在于不断地迭代，通过点与点之间的配准来进行旋转和平移，它的衡量标准基于最小二乘法，且点与点的距离要满足在一定的阈值范围内 。点云配准过程的原理是，假设现在有两个待配准点云 P和 Q ，在点云 P中任意选择一点，记为𝑝 𝑖 ，从点云 Q 中查找一个距离其欧氏距离最短的点（可使用kdtree进行临近查找），记为𝑞𝑖。𝑝 𝑖 和 𝑞 𝑖为对应点，得出变换矩阵，经过多次地迭代，最终获得最理想的变换矩阵，使两点云实现重合。此时还需要设置一个条件，让迭代的停止，公式如下：

其中， R 代表旋转变换矩阵， T 代表平移变换矩阵。有了迭代条件，最终就可以获得最优变换矩阵，使两点云重合。

以下是 ICP 算法的配准过程的步骤：

（ 1 ）从源点云𝑃中采样，然后找出同目标点云𝑄 相对应的点集，满足每个对应点之间的欧式距离都是最小的，最后得到两个新的点集，即去除无对应点的点和错误的点。

（2）由（ 1 ）中找到的点集，根据其坐标信息，计算对应点集的重心。

（3）求得重心，利用重心计算旋转矩阵 R 和平移向量 T ，计算误差函数𝐸(𝑅, 𝑇)， 使得其值最小；

（4）在源点云𝑃中的点，步骤（3）计算得到的旋转矩阵𝑅 和平移向量 T ，对其刚性变换，变换后获得的新的点集，命名为𝑃′；

（5）由新的点集𝑃′ 和目标点云𝑄内的全部点，求出两者之间对应点的平均距 离，记为𝑑̅；

（6）根据上式求得平均距离𝑑̅，并判断迭代是否结束，如果𝑑̅比预先设置的阈值 τ 大，则继续回到步骤（2）中计算到满足条件即可；如果𝑑̅比预先设置 的阈值 τ 小，或者满足设置的迭代次数 k ，则判断算法收敛，迭代终止。

 总结：

（1）ICP算法对初始值的依赖比较大，具体的配准过程中，如何选择初始值是一个问题，可以使用轮式里程计、imu预积分或其它先验的配准结果。

（2）ICP迭代次数过多，实际工程中需要考虑到其实时性的优化问题。

（3）迭代终止的阀值的设置也是需要考虑的点，可以考虑自适应阀值的方法。

（4）关于寻找最近点可以使用kdtree进行临近搜索。