干货 | 三维点云配准：ICP 算法原理及推导

编者荐语

点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。

转载自 | 渡众机器人

1 问题描述

点云配准（Point Cloud Registration）指的是输入两幅点云  (source) 和  (target) ，输出一个变换  使得  和  （修改为Pt）的重合程度尽可能高。变换  可以是刚性的(rigid)，也可以不是，本文只考虑刚性变换，即变换只包括旋转、平移。

点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。

目前应用最广泛的点云精配准算法是迭代最近点算法（Iterative Closest Point, ICP）及各种变种 ICP 算法。

2 算法描述

对于  是刚性变换的情形，点云配准问题可以描述为：

这里  和  是源点云和目标点云中的对应点。

ICP 算法的直观想法如下：

• 如果我们知道两幅点云上点的对应关系，那么我们可以用 Least Squares 来求解 R, t 参数；

• 怎么知道点的对应关系呢？如果我们已经知道了一个大概靠谱的 R, t 参数，那么我们可以通过贪心的方式找两幅点云上点的对应关系（直接找距离最近的点作为对应点）。

ICP 算法实际上就是交替进行上述两个步骤，迭代进行计算，直到收敛。

ICP 一般算法流程为：

1. 点云预处理

- 滤波、清理数据等

2. 匹配

- 应用上一步求解出的变换，找最近点

3. 加权

- 调整一些对应点对的权重

4. 剔除不合理的对应点对

5. 计算 loss

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

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

整体上来看，ICP 把点云配准问题拆分成了两个子问题：

• 找最近点

• 找最优变换

2.1 找最近对应点（Find Closet Point）

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

如果直接进行比较找最近邻点，需要进行两重循环，计算复杂度为  ，这一步会比较耗时，常见的加速方法有：

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

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

2.2 求解最优变换（Find Best Transform）

对于 point-to-point ICP 问题，求最优变换是有闭形式解（closed-form solution）的，可以借助 SVD 分解来计算。

先给出结论，在已知点的对应关系的情况下，设  ，  分别表示源点云和目标点云的质心，令  ，  ，令  ，这是一个 3x3 矩阵，对 H 进行 SVD 分解得到  ，则 point-to-point ICP 问题最优旋转为：

最优平移为：

下面分别给出证明。

2.2.1 计算最优平移

令  ，loss 为  ，对其进行求导，则有：

令导数为 0 ，则有：

无论 R 取值如何，根据上式我们都可以求得最优的 t，使得 loss 最小。下面我们来推导最优旋转的计算公式。

2.2.2 计算最优旋转

经过最优平移的推导，我们知道无论旋转如何取值，都可以通过计算点云的质心来得到最优平移，为了下面计算上的简便，我们不妨不考虑平移的影响，先将源点云和目标点云都转换到质心坐标下，这也就是之前令  ，  的意义。

下面我们用  和  进行推导。

不考虑平移，则 loss 可以写成：

先化简  ：

这里利用到了  和  （标量的转置等于自身）的性质。

由于点的坐标是确定的(和 R 无关)，所以最小化原 loss 等价于求：

也即为求：

注意到  （由矩阵乘法及 trace 的定义可得）

则问题转化为：

根据 trace 的性质  ，（这里不要求 A, B 为方阵，只要 A*B 是方阵即可），有：

还记得前面定义的矩阵 H 和其 SVD 分解吗？带入上式得到：

注意这里  都是正交矩阵（orthogonal matrices），所以  也是正交矩阵。令  ，则有：

根据奇异值非负的性质和正交矩阵的性质（正交矩阵中的元素绝对值不大于 1），容易证得只有当 M 为单位阵时  最大，即：

所以有  。

最后还需要进行 Orientation rectification，即验证  是不是一个旋转矩阵（检查是否有  ），因为存在  的可能，此时 R 表示的不是旋转而是一个 reflection，所以我们还要给上述优化求解加上一个  的约束。

根据矩阵行列式的性质，以及 U, V 都是正交阵：

如果  ，则  ，  已经给出最优旋转；如果  ，则  ，我们需要求解此时的 R，也就是分析 M 应该具有何种形式。具体的讨论请参考这里，本文直接给出结论：当 |M|=−1 时，使得 trace(ΣM) 最大的 M 为：

综合考虑  和  两种情况，我们可以得到：

至此公式推导完了，简单总结一下求解最优变换的步骤：

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

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

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

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

5. 根据公式计算  。

2.3 迭代

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

•  的变化量小于一定值

• loss 变化量小于一定值

• 达到最大迭代次数

3 ICP 的优缺点及一些改进算法

ICP 优点：

• 简单，不必对点云进行分割和特征提取

• 初值较好情况下，精度和收敛性都不错

ICP 缺点：

• 找最近对应点的计算开销较大

• 只考虑了点与点距离，缺少对点云结构信息的利用

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

4 实际使用中的一些注意事项

ICP 比较依赖于变换初值，平移比较简单，直接用点云质心来估计；旋转初值的话可以手动调一个粗略值，或者沿每个轴的旋转进行采样、组合来尝试（不适合实时性应用）；

点太多的话可以先降采样；

找到一些 anchor 点对（比如先用特征点匹配），可以帮助加速收敛；

对应用场景引入一些合理假设，比如限制旋转、平移的范围，变换自由度数量等。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~