【图像配准】点云配准ICP算法介绍：基础流程、ICP算法的变种

ICP算法（Iterative Closest Points）

前言

ICP的目的：把不同坐标系中的点，通过最小化配准误差，变换到一个共同的坐标系中

配准：把匹配图像 配准到 参考图像的坐标系中

点云包括几何信息和非几何信息

点云数量非常巨大，并且含有噪声，所以需要滤波。滤波就是从带有噪声中提取有用的信息。

1）去除不能为匹配带来有用信息的点

2）点云进一步抽象，例如提取局部的法线信息或曲率

配准包括：粗配准、精配准

粗配准：大概配准，获得R和T的初值

精配准：不断迭代，计算最终的R和T。基本上使用ICP算法及其各种变种

拓展：

映射变换（三维）
$$H=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ v_x& v_y& v_z& s\end{array}\right]=\left[\begin{array}{cc}R& t\\ V& S\end{array}\right]$$
R旋转矩阵、t平移向量、V透视变换向量、S比例因子

刚性变换矩阵中涉及到六个未知数α、β、γ、 tx、ty、tz。

要唯一确定这六个未知参数，需要六个线性方程，即至少需要在待匹配点云重叠区域找到3组对应点对，且3组对应点对不能共线，才可以得到这几个未知数的值，进而完成刚性矩阵的参数估计。

通常情况下，人们会选择尽可能多的对应点对，进一步提高刚性变换矩阵的参数估计精度。

问题引入

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

理论上，两者内部的点应该是一一对应的，比如（Ps0，Pt0）对应同一个点。

并且在存储的时候，按照顺序存储下标为i的像素。

在没有误差的情况下，计算出旋转R和平移t，可以将$P_s$转换到$P_t$，转换公式为：
$$p_t^i=R\cdot p_s^i+t$$
但由于噪声存在，以及匹配错误存在，上面的公式不一定对于所有的点都成立，所以存在误差。

ICP具体算法

1）核心：最小化一个目标函数，即上面提到的误差
$$f(R,t)=\frac{1}{N_p}\sum _{i=1}^{N_p}|p_t^i-R\cdot p_s^i-t{|}^2$$

pt和ps为对应点，一共有Np对对应点，目标函数实际是对应点之间的欧氏距离求和

2）寻找对应点： 我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换，得到的一个变换后的点云。然后将这个变换后的点云与target cloud进行比较，只要两个点云中存在距离小于一定阈值，我们就认为这两个点就是对应点。称为最邻近点对

3）R、T优化： 有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵

4）迭代： 我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者邻近点对不再变化等。

算法大致流程就是上面这样。

ICP流程

1.预处理：滤波、清洗数据

2.匹配：利用粗配准的估计矩阵，找最邻近点

3.加权：调整一些对应点对的权重

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

5.计算Loss

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

7.重复执行2-6，迭代直到收敛

这里的优化过程是一个贪心的策略。

首先固定R跟T利用最邻近算法找到最优的点对，然后固定最优的点对来优化R和T，依次反复迭代进行。

ICP算法的参数主要有两个：一个是ICP的邻近距离，另外一个是迭代的终止条件。

邻近距离：最初使用较大的对应点距离参数，然后逐步减小到一个较小的值

整体上看，ICP也把配准分成两个子问题：找最邻近点、找最优变换

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

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

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

• 设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集；
  • 在目标点云中取点集$p_t^i属于P_t$，找出源点云中的对应点集$p_s^i属于P_s$，使得$||p_s^i-p_t^i||<=min$
  • 注意，此处的ps为源点云变换后的临时变换点云
• 使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-tree；KD-tree 建树的计算复杂度为 O(N log(N))，查找通常复杂度为 O(log(N))（最坏情况下 O(N)）。

KD-Tree

一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构，主要应用于多维空间关键数据的搜索

k-d树是每个节点都为k维点的二叉树

所有的非叶子节点，都可以看做是一个超平面，把空间分割为两个半空间，左子树为超平面左边的点，右子树为超平面右边的点。

BSPTree（Binary space partitioning tree，空间二分树）

黄色的点作为根节点，上面的点归左子树，下面的点归右子树，接下来再不断地划分，最后得到一棵树就是赫赫有名的BSPTree（binary space partitioning tree）。

KDTree就是超平面都垂直于轴的BSPTree

上面的数据集用KDTree划分后为：

黄色节点就是Root节点，下一层是红色，再下一层是绿色，再下一层是蓝色。

创建kd树

kd树的数据结构

Node-data - 数据矢量， 数据集中某个数据点，是n维矢量（这里也就是k维）
Range - 空间矢量， 该节点所代表的空间范围
split - 整数， 垂直于分割超平面的方向轴序号
Left - k-d树， 由位于该节点分割超平面左子空间内所有数据点所构成的k-d树
Right - k-d树， 由位于该节点分割超平面右子空间内所有数据点所构成的k-d树
parent - k-d树， 父节点

建立树最大的问题在于轴点（pivot）的选择，选择好轴点之后，树的建立就和BSTree差不多了。

建树必须遵循两个准则：

1.建立的树应当尽量平衡，树越平衡代表着分割得越平均，搜索的时间也就是越少。

2.最大化邻域搜索的剪枝机会。

轴点选取策略：（从垂直哪个轴的超平面切割、切割点在哪）

1.median of the most spread dimension pivoting strategy

对于所有描述子数据（特征矢量），统计他们在每个维度上的数据方差，挑选出方差中最大值，对应的维就是split域的值。数据方差大说明沿该坐标轴方向上数据点分散的比较开。这个方向上，进行数据分割可以获得最好的平衡。数据点集Data-Set按照第split维的值排序，位于正中间的那个数据点 被选为轴点。

这种方法容易形成：

2.纬度的选择的依据为数据范围最大的那一维作为分割纬度，之后也是选中这个纬度的中间节点作为轴点，然后进行分割，结果：

这样的策略对最近邻搜索会比较友好。

3.随着树的深度，轮流选择轴作为分割面（例如：在三维空间中根节点是 x 轴垂直分割面，其子节点皆为 y 轴垂直分割面，其孙节点皆为 z 轴垂直分割面，其曾孙节点则皆为 x 轴垂直分割面，依此类推。）也是选中某纬度的中间节点作为轴点

选择中间节点进行划分，会产生一个平衡的kd树，但是平衡的树不一定对每个应用都是最佳的

最近邻搜索

找出在树中与输入点最接近的点

过程：

1. 从根节点开始，递归的往下移。往左还是往右的决定方法与插入元素的方法一样(如果输入点在分区面的左边则进入左子节点，在右边则进入右子节点)。

2. 一旦移动到叶节点，将该节点当作"当前最佳点"。

3. 回溯搜索路径，并判断搜索路径上节点的其他子结点中是否可能有距离更近的点，如果有可能，就要对另一个子结点进行最近邻搜索

4. 如何判断？

5. 

6. 以查询点为圆心，以当前的最近距离为半径画圆，这个圆称为候选超球（candidate hypersphere），如果圆与回溯点的轴相交（也就是star点到红点切割轴的距离，小于star点和当前最佳点的距离），则认为其他子结点中可能有更近的点

7. 个人对相交的理解：与当前最佳点的距离，比距离划分轴的距离还要远，说明轴的另一边有可能存在更近的点。

8. 例子（不可能和可能两种情况）：

11. 当搜索路径为空时（向下搜索时，路过节点加入搜索路径；向上回溯时，路过节点从搜索路径中剔除），完成最邻近搜索。

求解最优变换（Find Best Transform）

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

给出结论，在已知点的对应关系的情况下，

设 ${\overline{p}}_s$，${\overline{p}}_t$ 分别表示源点云和目标点云的质心

令 ${\hat{p}}_s^i=p_s^i-{\overline{p}}_s$， ${\hat{p}}_t^i=p_t^i-{\overline{p}}_t$

令 $H={\sum }_{i=1}^{|P_s|}{\hat{p}}_s^i{{\hat{p}}_t^i}^T$，这是一个 3x3 矩阵，对 H 进行 SVD 分解得到 $H=U\sum V^T$，则 point-to-point ICP 问题最优旋转为：
$$R^{\ast }=V{U}^T$$
最优平移为：
$$t^{\ast }={\overline{p}}_t-R^{\ast }{\overline{p}}_s$$
下面分别给出证明。

计算最优平移

令$N=|P_s|$，设损失函数为$F(t)={\sum }_{i=1}^N||(R\cdot p_s^i+t)-p_t^i|{|}^2$，求导可得
$$\begin{gathered} \frac{\partial F}{\partial t}=\sum _{i=1}^{N}2(R\cdot p_s^i+t-p_t^i) \\ =2nt+2R\sum _{i=1}^N{p}_s^i-2\sum _{i=1}^N{p}_t^i \end{gathered}$$
当导数为0时，最优，损失最小
$$\begin{gathered} 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{gathered}$$

计算最优旋转

经过最优平移的推导，我们知道无论旋转如何取值，都可以通过计算点云的质心来得到最优平移

因此，为了计算方便，不考虑平移的影响，将源点云和目标点云都转换到质心坐标下，这就是之前为什么让 ${\hat{p}}_s^i=p_s^i-{\overline{p}}_s$， ${\hat{p}}_t^i=p_t^i-{\overline{p}}_t$。

转化之后，我们不考虑平移，损失函数为：
$$F(R)=\sum _{i=1}^N||R\cdot {\hat{p}}_s^i-{\hat{p}}_t^i|{|}^2$$
化简$||R\cdot {\hat{p}}_s^i-{\hat{p}}_t^i|{|}^2$:
$$\begin{gathered} ||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{gathered}$$

拓展：向量的二范数为每个元素平方求和，且||v|| = v^T*v

可以证明对于旋转矩阵R，$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$

由于点的坐标都是确定的，也就是$||{\hat{p}}_s^i|{|}^2和||{\hat{p}}_t^i|{|}^2$不变，所以最小化Loss就是最小化带R的那一部分，即：
$$R^{\ast }={\mathrm{argmin}}_R(-2\sum _{i=1}^N{{\hat{p}}_t^i}^{T}R{\hat{p}}_s^i)$$
即：
$$R^{\ast }={\mathrm{argmax}}_R(\sum _{i=1}^N{{\hat{p}}_t^i}^{T}R{\hat{p}}_s^i)$$
我们可以得到：
$$\sum _{i=1}^N{{\hat{p}}_t^i}^{T}R{\hat{p}}_s^i=trace(P_t^{T}R{P}_s)$$
其中$P_t^T和P_s$表示所有的点的${\hat{p}}_t或{\hat{p}}_s$，trace表示求矩阵的迹，trace中的三个矩阵为Nx3,3x3,3xN，相乘结果为NxN的矩阵，只有对角线上式对应某点的${{\hat{p}}_t^i}^{T}R{\hat{p}}_s^i$。

所以问题转化为（问题向量化？）：
$$R^{\ast }={\mathrm{argmax}}_{R}trace(P_t^{T}R{P}_s)$$
根据trace(AB)=trace(BA)：
$$trace(P_t^{T}R{P}_s)=trace(R{P}_s{P}_t^T)$$
之前令 $H={\sum }_{i=1}^{|P_s|}{\hat{p}}_s^i{{\hat{p}}_t^i}^T$，即$H=P_s{P}_t^T$，并对H进行SVD分解可以得到
$$\begin{gathered} trace(P_t^{T}R{P}_s)=trace(R{P}_s{P}_t^T) \\ =trace(RH) \\ =trace(RU\Sigma V^T) \\ =trace(\Sigma V^{T}RU) \end{gathered}$$
其中V、R、U都是正交矩阵，所以$V^{T}RU$也是正交矩阵。

令$M=V^{T}RU=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$，则：
$$\begin{gathered} trace(\Sigma V^{T}RU)=trace(\Sigma M) \\ ={\sigma }_1{m}_{11}+{\sigma }_2{m}_{22}+{\sigma }_1{m}_{33} \end{gathered}$$

SVD拓展：一个mxn的矩阵A，分解为$A=U\Sigma V^T$，

SVD分解之后，$\Sigma ={\left[\begin{array}{ccccc}{\sigma }_1& 0& 0& 0& 0\\ 0& {\sigma }_2& 0& 0& 0\\ 0& 0& \ddots & 0& 0\\ 0& 0& 0& \ddots & 0\end{array}\right]}_{m\times n}$

UV均为单位正交阵

根据奇异值非负的性质和正交矩阵的性质，容易证得只有当 M 为单位阵时 trace(ΣM) 最大，即：
$$\begin{gathered} V^{T}RU=I \\ R=V{U}^T \end{gathered}$$
所以最优的R为：
$$R^{\ast }=V{U}^T$$
最后还需要进行 Orientation rectification，即验证 R∗ 是不是一个旋转矩阵（检查是否有 |R|=1）

因为存在 |R|=−1 的可能，此时 R 表示的不是旋转而是一个 reflection，所以我们还要给上述优化求解加上一个 |R|=1 的约束。

在该约束下，M的行列式为：
$$\begin{gathered} |M|=|V^T||U||R| \\ =|V^T||U|=\pm 1 \end{gathered}$$
如果$|V{U}^T|=1$，则$|M|=1$，此时$R^{\ast }=VM{U}^T=VE{U}^T=V{U}^T$就是最优解。

如果$|V{U}^T|=-1$，则$|M|=-1$，我们需要求解此时的R，也就是求解$|M|=-1$时 什么样的M能让trace(ΣM)最大。 https://www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf（详细讨论），链接中的结论为：让trace(ΣM)最大的M为：
$$M=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]$$
将上面两种情况综合起来：
$$R^{\ast }=V\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& |V{U}^T|\end{array}\right]U^T$$
可以保证最优的R*行列式始终为1

总结：最优变换步骤

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

ICP算法关键点：

（1）原始点集的采集

均匀采样、随机采样和法矢采样

（2）确定对应点集

点到点、点到投影、点到面

（3）计算变化矩阵

四元数法、SVD奇异值分解法

迭代

每一次迭代都会得到当前的最优变换参数$R_k,t_k$

然后把这个变换作用于源点云，产生新的临时变换目标点云$P_{t\_k}$，继续找源点云和目标点云之间的最近对应点

“找最近对应点”和“求解最优变换”这两步不停迭代进行，

直到满足迭代终止条件（阈值判断），常用的终止条件有：

• $R_k,t_k$ 的变化量小于一定值
• loss 变化量小于一定值
• 达到最大迭代次数

优缺点和改进

ICP 优点：

• 简单，不必对点云进行分割和特征提取
• 初值较好情况下，精度和收敛性都不错

ICP 缺点：

• 找最近对应点的计算开销较大
• 只考虑了点与点距离，缺少对点云结构信息的利用

原始的 ICP 算法采用最小二乘估计变换矩阵，原理简单，精度较好，但是由于采用迭代计算，计算开销大，速度较慢，对初始变换敏感（依赖于初始的变换，初始变换找的好坏，会很大程度影响结果），容易陷入局部最优解。

原始的ICP可以看做为Point-to-Point 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 点对（比如先用特征点匹配），可以帮助加速收敛；
• 对应用场景引入一些合理假设，比如限制旋转、平移的范围，变换自由度数量等。

ICP变种

普通的ICP在计算点与点之间的误差、代价函数时，使用点对点的距离作为误差。

PL-ICP（Point-to-Line）：

相对于PP-ICP最大的区别就是改进了误差方程。

将点对点的距离 改成 点到其最近两个点连线的距离。精度更高。

Point-to-Plane ICP

误差方程中考虑的是源顶点到目标顶点所在面的距离，考虑目标点云的局部结构

Plane-to-Plane ICP，

point-to-plane 只考虑目标点云局部结构， plane-to-plane 顾名思义就是也考虑源点云的局部结构，计算面到面的距离；

GICP（Generalized ICP ）

综合考虑 point-to-point、point-to-plane 和 plane-to-plane 策略，精度、鲁棒性都有所提高；

NICP（Normal)

考虑在误差方程中加入法向量和曲率

粗配准

ICP算法的基本原理。它需要一个旋转平移矩阵的初值。这个初值如果不太正确，那么由于它的greedy优化的策略，会使其目标函数下降到某一个局部最优点（当然也是一个错误的旋转平移矩阵）。因此，我们需要找到一个比较准确的初值，这也就是粗配准需要做的。

粗配准目前来说还是一个难点。针对于不同的数据，有许多不同的方法被提出。

配准的评价标准：

比较通用的一个是LCP(Largetst Common Pointset)

给定两个点集P,Q，找到一个变换T§，使得变换后的P与Q的重叠度最大。

在变换后的P内任意一点，如果在容差范围内有另外一个Q的点，则认为该点是重合点。

重合点占所有点数量的比例就是重叠度。

解决上述LCP问题，最简单粗暴的方法就是遍历。

假设点集P,Q的大小分别为m,n。

而找到一个刚体变换需要3对对应点。那么brute force 搜索的需要$O(m^3{n}^3)$的复杂度。对于动辄几百万个点的点云，这种时间复杂度是不可接受的。

因此，许多搜索策略被提出。

搜索策略

比较容易想到的是RANSAC之类的搜索方法。而对于不同的场景特点，可以利用需配准点云的特定信息加快搜索。（例如知道点云是由特定形状的面构成的）这里先介绍一个适用于各种点云，不需要先验信息的搜索策略，称为4PC(4 Point Congruent)。

4PC(4 Point Congruent):

4PC搜索策略是在P,Q中找到四个共面的对应点。

四个共面的点相交于e

有两个比例在刚体变化下是不变的。$|ae|/|eb|=|a^{\prime }{e}^{\prime }|/|e^{\prime }{b}^{\prime }|$ 和 $|de|/|ec|=|d^{\prime }{e}^{\prime }|/|e^{\prime }{c}^{\prime }|$(实际上在仿射变换下也是不变的。)

4PC将对于三个点的搜索转换为对e,e’的搜索，从而将复杂度降低到了$O(n^2+k)$。这四个点的距离越远，计算得到的转换越稳健。但是这里的四个点的搜索依赖于两个点云的重叠度。

4PC算法通用性较好，但是对于重叠度较小、或是噪声较大的数据也会出现配准错误或是运行时间过长的问题。

具体的算法：4-Points Congruent Sets for Robust Pairwise Surface Registration

参考：

https://www.zhihu.com/question/34170804