点云配准算法ICP及其各种变体
作者：流川峰介绍
点云配准（Point Cloud Registration）算法指的是输入两幅点云 Ps (source) 和 Pt (target)，输出一个变换T（即旋转R和平移t）使得 T(Ps)和Pt的重合程度尽可能高。常用的有NDT、ICP。本文主要介绍ICP（Iterative Closest Point）算法及其各种变体。
点云配准首先要知道两组点云的匹配关系，对于视觉三维点来说，可以通过视觉特征匹配来获取，对于雷达点云，可以通过最近邻匹配来获取，关于匹配本文不深入介绍。知道点云的匹配关系后，通过粗配准（Coarse Registration）和精配准（Fine Registration）两步来计算变换矩阵。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。其中，粗配准存在解析解，精配准通过非线性优化的方式进一步优化结果。
图1 ICP算法流程
CP算法
对于point-to-point ICP问题，最优变换是有解析解（closed-form solution）的，可以使用SVD 分解来计算。该解析解可以作为粗配准的结果。
SVD法
SVD法中求解旋转和平移是分开的。我们首先计算最优旋转，在根据最优结果计算最优平移。为了去除平移的影响，先将源点云和目标点云都转换到质心坐标下，即令 https://www.zhihu.com/equation?tex=%5C%5B%5Chat+p_s%5Ei+%3D+p_s%5Ei+-+%7B%5Cbar+p_%7Bs%2C%7D%7D%5Cquad+%5Chat+p_t%5Ei+%3D+p_t%5Ei+-+%7B%5Cbar+p_t%7D%5C%5D 则点云配准问题的loss可以写为： https://www.zhihu.com/equation?tex=%5C%5BF%28R%29+%3D+%5Csum%5Climits_%7Bi+%3D+1%7D%5EN+%7B%7B%7B%5Cleft%5C%7C+%7BR+%5Ccdot+%5Chat+p_s%5Ei+-+%5Chat+p_t%5Ei%7D+%5Cright%5C%7C%7D%5E2%7D%7D+%5C%5D （1）
展开有：
https://www.zhihu.com/equation?tex=%5C%5B%5Cbegin%7Barray%7D%7Bl%7D+%7B%5Cleft%5C%7C+%7BR+%5Ccdot+%5Chat+p_s%5Ei+-+%5Chat+p_t%5Ei%7D+%5Cright%5C%7C%5E2%7D%5C%5C++%3D+%7B%5Cleft%28+%7BR+%5Ccdot+%5Chat+p_s%5Ei+-+%5Chat+p_t%5Ei%7D+%5Cright%29%5ET%7D%5Cleft%28+%7BR+%5Ccdot+%5Chat+p_s%5Ei+-+%5Chat+p_t%5Ei%7D+%5Cright%29%5C%5C++%3D+%5Cleft%28+%7B%5Chat+p%7B%7B_s%5Ei%7D%5ET%7D%7BR%5ET%7D+-+%5Chat+p%7B%7B_t%5Ei%7D%5ET%7D%7D+%5Cright%29%5Cleft%28+%7BR+%5Ccdot+%5Chat+p_s%5Ei+-+%5Chat+p_t%5Ei%7D+%5Cright%29%5C%5C++%3D+%5Chat+p_s%5Ei%7BR%5ET%7DR%5Chat+p_s%5Ei+-+%5Chat+p_t%5EiR%5Chat+p_s%5Ei+-+%5Chat+p_s%5Ei%7BR%5ET%7D%5Chat+p_t%5Ei+%2B+%5Chat+p_t%5Ei%5Chat+p_t%5Ei%5C%5C++%3D+%7B%5Cleft%5C%7C+%7B%5Chat+p_s%5Ei%7D+%5Cright%5C%7C%5E2%7D+%2B+%7B%5Cleft%5C%7C+%7B%5Chat+p_t%5Ei%7D+%5Cright%5C%7C%5E2%7D+-+%5Chat+p_t%5EiR%5Chat+p_s%5Ei+-+%5Chat+p_s%5E%7B%7Bi%5ET%7D%7D%7BR%5ET%7D%5Chat+p_t%5Ei%5C%5C++%3D+%7B%5Cleft%5C%7C+%7B%5Chat+p_s%5Ei%7D+%5Cright%5C%7C%5E2%7D+%2B+%7B%5Cleft%5C%7C+%7B%5Chat+p_t%5Ei%7D+%5Cright%5C%7C%5E2%7D+-+2%5Chat+p_t%5E%7B%7Bi%5ET%7D%7DR%5Chat+p_s%5Ei+%5Cend%7Barray%7D%5C%5D （2）
因为点云是坐标是确定的，因此最小化loss转化为下式：
https://www.zhihu.com/equation?tex=%5C%5B%5Cbegin%7Barray%7D%7Bl%7D+%7BR%5E%2A%7D+%3D+%5Cmathop+%7B%5Carg+%5Cmin+%7D%5Climits_R+%5Cleft%28+%7B+-+2%5Csum%5Climits_%7Bi+%3D+1%7D%5EN+%7B%5Chat+p_t%5Ei%7D+R%5Chat+p_s%5Ei%7D+%5Cright%29+%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%5Cleft%28+%7B%5Csum%5Climits_%7Bi+%3D+1%7D%5EN+%7B%5Chat+p_t%5Ei%7D+R%5Chat+p_s%5Ei%7D+%5Cright%29%5C%5C++%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%5Cleft%28+%7BP_t%5ETR%7BP_s%7D%7D+%5Cright%29+%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%5Cleft%28+%7BR%7BP_s%7DP_t%5ET%7D+%5Cright%29%5C%5C++%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%5Cleft%28+%7BRH%7D+%5Cright%29+%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%5Cleft%28+%7BRU%5CSigma+%7BV%5ET%7D%7D+%5Cright%29%5C%5C++%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%5Cleft%28+%7B%5CSigma+%7BV%5ET%7DRU%7D+%5Cright%29+%3D+%5Cmathop+%7B%5Carg+%5Cmax+%7D%5Climits_R+%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%28%5CSigma+M%29+%5Cend%7Barray%7D%5C%5D
根据奇异值非负的性质和正交矩阵的性质（正交矩阵中的元素绝对值不大于 1），容易证得只有当M为单位阵时 https://www.zhihu.com/equation?tex=%5C%5B%7B%5Cmathop%7B%5Crm+trace%7D%5Cnolimits%7D+%28%5CSigma+M%29%5C%5D 最大，
即： https://www.zhihu.com/equation?tex=%5C%5B%5Cbegin%7Barray%7D%7B%2A%7B20%7D%7Br%7D%7D+%7B%7BV%5ET%7DRU+%3D+I%7D%5C%5C+%7BR+%3D+V%7BU%5ET%7D%7D+%5Cend%7Barray%7D%5C%5D
考虑到R为旋转矩阵的约束，最终解为： https://www.zhihu.com/equation?tex=%5C%5BR+%3D+V%7BU%5ET%7D%28%7CR%7C+%3D+1%29%5C%5D 自然地，最优平移可以得到： https://www.zhihu.com/equation?tex=%5C%5Bt%7B%5Crm%7B+%7D%7D+%3D+%5Cfrac%7B1%7D%7BN%7D%5Csum%5Climits_%7Bi+%3D+1%7D%5EN+%7Bp_t%5Ei%7D++-+R%5Cfrac%7B1%7D%7BN%7D%5Csum%5Climits_%7Bi+%3D+1%7D%5EN+%7Bp_s%5Ei%7D++%3D+%7B%5Cbar+p_t%7D+-+R%7B%5Cbar+p_s%7D%5C%5D
非线性优化法（迭代法）
我们发现，点对点的ICP问题存在最优解的解析解。但是仅使用解析解计算一次匹配误差还是比较大，这里可以采用迭代的方法次计算。每一次迭代我们都会得到当前的最优变换参数，然后将该变换作用于当前源点云；“找最近对应点”和“求解最优变换”这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有：
·的变化量小于一定值
· loss 变化量小于一定值
· 达到最大迭代次数
PL（point-line）ICP算法（点到线）
点到点ICP算存在以下缺点：依赖初始值，初始值不好时，迭代次数增加；对于较大的初始误差，可能会出现错误的迭代结果；ICP是一阶收敛，收敛速度慢（为了弥补这一点，通常使用K-D树加快搜索）；会有离群点及噪声。为此改善上述缺点，有人提出了PLICP，顾名思义，这种方式使用源点云到目标点云直线的距离度量来估计变换。主要区别在于误差函数的构建上。ICP是找最近邻的一点，以点与点之间的距离作为误差，而PLICP是找到最近邻的两点，两点连线，是以点到线的距离作为误差，实际上，后者的误差度量方式更符合结构化场景中的雷达点云的实际情况。因此具有更小的误差（图2）。然而，它对非常大的初始位移误差的鲁棒性较差，因此需要比较精确的初始值。
图2 点到线度量比普通 ICP 中使用的点到点度量更接近表面的距离
点到线的误差函数可以写为： https://www.zhihu.com/equation?tex=%5C%5BJ+%3D+%5Csum%5Climits_s+%7B%7B%7B%5Cleft%28+%7B%7B%5Cbf%7Bn%7D%7D_s%5E%7B%5Crm%7BT%7D%7D%5Cleft%5B+%7B%7B%5Cbf%7BR%7D%7D%7B%7B%5Cbf%7Bp%7D%7D_s%7D+%2B+%7B%5Cbf%7Bt%7D%7D+-+%7B%7B%5Cbf%7Bp%7D%7D_t%7D%7D+%5Cright%5D%7D+%5Cright%29%7D%5E2%7D%7D+%5C%5D
https://www.zhihu.com/equation?tex=%5C%5B%7B%5Cbf%7Bn%7D%7D_s%5E%7B%7D%5C%5D 是目标点云中匹配到的最近两个点对应直线（ https://www.zhihu.com/equation?tex=%5C%5B%7B%7B%5Cbf%7Bp%7D%7D_%7Bt2%7D%7D+-+%7B%7B%5Cbf%7Bp%7D%7D_%7Bt1%7D%7D%5C%5D ）的法线。作者设计了六个仿真实验，初始位移误差增加（均匀分布；从实验 1 中的 [±0.05m，±0.05m，±2°] 到实验 6 中的 [±0.2m，±0.2m，±45°]）。对于每个实验，上述过程对 778 次扫描中的每一次重复 100 次。通过非线性优化的方式来求解点云的转换。这里给出算法的仿真结果：
图3 PLICP与其他点云配准算法的仿真结果对比
Point-plane ICP
使用点到平面（point-plane）误差度量的迭代最近点 (ICP) 算法已被证明比使用点到点（point-point）误差度量的算法收敛得更快。在 ICP 算法的每次迭代中，产生最小点到平面误差的相对位姿变化通常使用标准的非线性最小二乘法来解决。例如 Levenberg-Marquardt 方法。当使用点到平面误差度量时，最小化的对象是每个源点与其对应目标点的切平面之间的平方距离之和（参见图 4）。
图4 point-to-plane ICP算法示意图
最小化损失函数可以写为： https://www.zhihu.com/equation?tex=%5C%5B%7B%5Cbf%7BJ%7D%7D+%3D+%5Carg+%7B%5Cmin+_%7B%5Cbf%7BT%7D%7D%7D%5Csum%5Climits_s+%7B%7B%7B%5Cleft%28+%7B%5Cleft%28+%7B%7B%5Cbf%7BT%7D%7D+%5Ccdot+%7Bp_s%7D+-+%7Bp_t%7D%7D+%5Cright%29+%5Ccdot+%7B%7B%5Cbf%7Bn%7D%7D_s%7D%7D+%5Cright%29%7D%5E2%7D%7D+%5C%5D
其中 https://www.zhihu.com/equation?tex=%5C%5B%7Bp_s%7D%5C%5D 是源点，是对应的目的点， https://www.zhihu.com/equation?tex=%5C%5B%7B%7B%5Cbf%7Bn%7D%7D_s%7D%5C%5D 是处的单位法线向量。这种方法的优缺点也很明显：首先，点到平面成本函数允许平坦区域相互滑动；点到平面通常比点到点收敛得更快；迭代次数更少 ；点到平面在每次迭代中速度较慢，并且需要表面法线。
（8）式没有解析解，我们只能使用非线性最小二乘的方法求解，为了加快求解，有研究者发现当两个输入表面之间的相对方向较小时，可以使用近似的方法来有效地求解的线性最小二乘法近似非线性优化问题。
难以优化的原因是R 太复杂，无法优化。在每次迭代中，我们假设旋转的角度很小，即
那么旋转矩阵可以近似表示为：
重写损失函数（8）为：
其中：
这样非线性的损失函数近似为线性，每一步迭代的最优解x为 https://www.zhihu.com/equation?tex=%5C%5B%5Chat+x+%3D+%7B%5Cleft%28+%7B%7BA%5ET%7DA%7D+%5Cright%29%5E%7B+-+1%7D%7D%7BA%5ET%7Db%5C%5D 多次迭代后直到算法收敛。
NICP
Normal Iterative Closest Point (NICP)在匹配两组点云时，将点云的局部特征（法向量，曲率）考虑在内，即在迭代求解过程中，误差函数不仅包含点云之间的法向量的投影距离（同point to plane ICP），还包含了法向量方向误差。相比于上述的方法，NICP更加鲁棒。NICP 算法的特点在于，其在匹配两组点云时并非考虑匹配点云之间的欧氏距离，而是将点云曲面的局部特征作为点对匹配以及计算变换的准则。具体来说主要可以分为以下几部分：计算点云中每个点的特征，即其表面的的法向量（normal）和曲面曲率(curvature)，以标记每个点；根据点的距离和特征找两组点云中的匹配点对；利用最小二乘法最小化，最小化目标函数，以求解点云变换矩阵。此处目标函数包括点面投影和法向量旋转误差。
令 https://www.zhihu.com/equation?tex=%5C%5B%5Cwidetilde+%7B%5Cbf%7Bp%7D%7D%5C%5D 为带法线方向的三维点云，即 https://www.zhihu.com/equation?tex=%5C%5B%5Cwidetilde+%7B%5Cbf%7Bp%7D%7D+%3D+%7B%5Cleft%28+%7B%5Cbegin%7Barray%7D%7B%2A%7B20%7D%7Bl%7D%7D+%7B%7B%7B%5Cbf%7Bp%7D%7D%5ET%7D%7D%26%7B%7B%7B%5Cbf%7Bn%7D%7D%5ET%7D%7D+%5Cend%7Barray%7D%7D+%5Cright%29%5ET%7D%5C%5D ，T 为由旋转矩阵R 和平移向量t 参数化的变换矩阵。具有法线的点的⊕算子是
则可以构造一个六维的误差函数：
上述误差函数可以通过非线性优化的方法来求解，这里不再赘述，感兴趣的读者可以寻找参考文献.
为了让大家对三维视觉各研究方向有更加深入、更加全面地了解，深蓝学院组织了公益的『三维视觉系列公开课』，邀请了三维视觉各细分方向国内外知名的一线研究者为大家分享研究思路与进展，同时，配套建立了专业的技术交流群。
本系列公开课涵盖的方向包含3D环境感知、3D物体检测、3D点云分割、3D姿态估计等。除了公益的公开课外，还建立了多个高质量的技术交流群，但入群名额所剩无几。点击卡片即可领取公开课合集并加入交流群~
参考
https://yilingui.xyz/2019/11/20/191120_point_cloud_registration_icp/
Censi A. An ICP variant using a point-to-line metric//2008 IEEE International Conference on Robotics and Automation. Ieee, 2008: 19-25.
Low K L . Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration. Chapel Hill, 2004.
Serafin, J., & Grisetti, G. (2015, September). NICP: Dense normal based point cloud registration. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 742-749). IEEE.
https://zhuanlan.zhihu.com/p/110428934
页:
[1]