三维点云配准之ICP算法

问题描述

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

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

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

算法描述

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

$\begin{matrix} (1) & R^{*}, t^{*} = \underset{R, t}{\arg min} \frac{1}{| P_{s} |} \sum_{i = 1}^{| P_{s} |} | | p_{t}^{i} - (R \cdot p_{s}^{i} + t) | |^{2} \end{matrix}$

这里 $p_{s}$ 和 $p_{t}$ 是源点云和目标点云中的一一对应点。

ICP 算法的直观想法如下：

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

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

ICP 一般算法流程为：

点云预处理

滤波、清理数据等

匹配

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

加权

调整一些对应点对的权重

剔除不合理的对应点对
计算 loss
最小化 loss，求解当前最优变换
回到步骤 2. 进行迭代，直到收敛

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

找最近点
找最优变换

找最近对应点（Find Closet Point）

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

如果直接进行比较找最近邻点，需要进行两重循环，计算复杂度为 O(| $P_{s}$ |⋅| $P_{t}$ |) ，这一步会比较耗时，常见的加速方法有：

设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集；
使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-tree；

求解最优变换（Find Best Transform）

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

先给出结论，在已知点的对应关系的情况下，设 $\bar{p_{s}}$ ， $\bar{p_{t}}$ 分别表示源点云和目标点云的质心，令：

$\begin{aligned} {\hat{p}}_{s}^{i} & = p_{s}^{i} - \bar{p_{s}} \\ {\hat{p}}_{t}^{i} & = p_{t}^{i} - \bar{p_{t}} \\ H & = \sum_{i = 1}^{| P_{s} |} {\hat{p}}_{s}^{i} {\hat{p}}_{t}^{i}^{T} \end{aligned}$

H是一个 3x3 矩阵，对 H 进行 SVD 分解得到 $H = U Σ V^{T}$ ， point-to-point ICP 问题最优旋转为：

$\begin{matrix} (2) & R^{*} = V U^{t} \end{matrix}$

下面分别给出证明。

计算最优平移

令 $N = | P_{s} |$ ，设 $F (t) = \sum_{i = 1}^{N} | | (R \cdot p_{s}^{i} + t) - p_{t}^{i} | |^{2}$ ，对其进行求导，则有：

$\begin{matrix} (3) & \begin{aligned} \frac{\partial F}{\partial t} & = \sum_{i = 1}^{N} 2 (R \cdot p_{s}^{i} + t - p_{t}^{i}) \\ = 2 n t + 2 R \sum_{i = 1}^{N} p_{s}^{i} - 2 \sum_{i = 1}^{N} p_{t}^{i} \end{aligned} \end{matrix}$

令导数为0，则有：

$\begin{matrix} (4) & \begin{aligned} t & = \frac{1}{N} \sum_{i = 1}^{N} p_{t}^{i} - R \frac{1}{N} \sum_{i = 1}^{N} p_{s}^{i} \\ = {\bar{p}}_{t} - R {\bar{p}}_{s} \end{aligned} \end{matrix}$

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

计算最优旋转

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

下面我们用 ${\hat{p}}_{s}^{i}$ 和 ${\hat{p}}_{t}^{i}$ 进行推导。

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

$\begin{matrix} (5) & F (R) = \sum_{i = 1}^{N} | | R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i} | |^{2} \end{matrix}$

先简化 $| | R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i} | |^{2}$ ：

$\begin{matrix} (6) & \begin{aligned} | | R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i} | |^{2} & = (R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i})^{T} (R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i}) \\ = ({\hat{p}}_{s}^{i}^{T} R^{T} - {\hat{p}}_{t}^{i}^{T}) (R \cdot {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i}) \\ = {\hat{p}}_{s}^{i}^{T} R^{T} R {\hat{p}}_{s}^{i} - {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i} - {\hat{p}}_{s}^{i}^{T} R^{T} {\hat{p}}_{t}^{i} + {\hat{p}}_{t}^{i}^{T} {\hat{p}}_{t}^{i} \\ = | | {\hat{p}}_{s}^{i} | |^{2} + | | {\hat{p}}_{t}^{i} | |^{2} - {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i} - {\hat{p}}_{s}^{i}^{T} R^{T} {\hat{p}}_{t}^{i} \\ = | | {\hat{p}}_{s}^{i} | |^{2} + | | {\hat{p}}_{t}^{i} | |^{2} - 2 {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i} \end{aligned} \end{matrix}$

这里利用到了 $R^{T} R = I$ 和 ${\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i} = {\hat{p}}_{s}^{i}^{T} R^{T} {\hat{p}}_{t}^{i}$ （标量的转置等于自身）的性质。

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

$\begin{matrix} (7) & R^{*} = \underset{R}{\arg min} (- 2 \sum_{i = 1}^{N} {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i}) \end{matrix}$

也即为求：

$\begin{matrix} (8) & R^{*} = \underset{R}{\arg max} (\sum_{i = 1}^{N} {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i}) \end{matrix}$

注意到 $\sum_{i = 1}^{N} {\hat{p}}_{t}^{i}^{T} R {\hat{p}}_{s}^{i} = t r a c e (P_{t}^{T} R P_{s})$ 由矩阵乘法及 trace 的定义可得）

则问题转化为：

$\begin{matrix} (9) & R^{*} = \underset{R}{\arg max} t r a c e (P_{t}^{T} R P_{s}) \end{matrix}$

根据trace的性质 $t r a c e (A B) = t r a c e (B A)$ ，（这里不要求 A, B 为方阵，只要 A*B 是方阵即可），有：

$\begin{matrix} (10) & t r a c e (P_{t}^{T} R P_{s}) = t r a c e (R P_{s} P_{t}^{T}) \end{matrix}$

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

$\begin{matrix} (11) & \begin{aligned} t r a c e (P_{t}^{T} R P_{s}) & = t r a c e (R P_{s} P_{t}^{T}) \\ = t r a c e (R H) \\ = t r a c e (R U Σ V^{T}) \\ = t r a c e (Σ V^{T} R U) \end{aligned} \end{matrix}$

注意这里 $V, U, R$ 都是正交矩阵（orthogonal matrices），所以 $V^{T} R U$ 也是正交矩阵。令

$M = V^{T} R U = [\begin{array}{cccc} m_{11} & m_{12} & m_{13} \\ m_{21} & m_{22} & m_{23} \\ m_{31} & m_{32} & m_{33} \end{array}]$

则有：

$\begin{matrix} (12) & \begin{aligned} t r a c e (Σ V^{T} R U) & = t r a c e (Σ M) \\ = σ_{1} m_{11} + σ_{2} m_{22} + σ_{1} m_{33} \end{aligned} \end{matrix}$

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

$\begin{matrix} (13) & \begin{array}{r} V^{T} R U = I \\ R = V U^{T} \end{array} \end{matrix}$

所以有 $R^{*} = V U^{T}$ 。

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

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

$\begin{matrix} (14) & \begin{aligned} | M | & = | V^{T} | | U | | R | \\ = | V^{T} | | U | = \pm 1 \end{aligned} \end{matrix}$

如果 $| V U T | = 1$ ，则 $| M | = 1$ ， $R^{*} = V U^{T}$ 已经给出最优旋转；如果 $| V U^{T} | = - 1$ ，则 $| M | = - 1$ ，我们需要求解此时的 $R$ ，也就是分析 $M$ 应该具有何种形式。具体的讨论请参考这里，本文直接给出结论：当 $| M | = - 1$ 时，使得 $t r a c e (Σ M)$ 最大的 $M$ 为：

$\begin{matrix} (15) & M = [\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & - 1 \end{array}] \end{matrix}$

综合考虑 $| M | = 1$ 和 $| M | = - 1$ 两种情况，我们可以得到：

$\begin{matrix} (16) & R^{*} = V [\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & | V U^{T} | \end{array}] U^{T} \end{matrix}$

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

1.计算源点云和目标点云质心；
2.将源点云和目标点云转换到质心坐标系；
3.计算矩阵 H（形式类似“协方差矩阵”）；
4.对 H 求 SVD 分解，根据公式求得 $R^{*}$ ；
5.根据公式计算 $t^{*}$ 。

迭代

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

$R_{k}, t_{k}$ 的变化量小于一定值
loss 变化量小于一定值
达到最大迭代次数

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

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

ICP 比较依赖于变换初值，平移比较简单，直接用点云质心来估计；旋转初值的话可以手动调一个粗略值，或者沿每个轴的旋转进行采样、组合来尝试（不适合实时性应用）；
点太多的话可以先降采样；
找到一些 anchor 点对（比如先用特征点匹配），可以帮助加速收敛；
对应用场景引入一些合理假设，比如限制旋转、平移的范围，变换自由度数量等。

参考

旋转矩阵(Rotate Matrix)的性质分析

KD(k-dimensional)树

奇异值分解SVD