点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨流川峰

来源丨深蓝AI

介绍

点云配准（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算法流程

ICP算法

对于point-to-point ICP问题，最优变换是有解析解（closed-form solution）的，可以使用SVD 分解来计算。该解析解可以作为粗配准的结果。

SVD法

SVD法中求解旋转和平移是分开的。我们首先计算最优旋转，在根据最优结果计算最优平移。为了去除平移的影响，先将源点云和目标点云都转换到质心坐标下，即令

则点云配准问题的loss可以写为：

（1）

展开有：

（2）

因为点云是坐标是确定的，因此最小化loss转化为下式：

（3）

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

（4）

考虑到R为旋转矩阵的约束，最终解为：

（5）

自然地，最优平移可以得到：

（6）

非线性优化法（迭代法）

我们发现，点对点的ICP问题存在最优解的解析解。但是仅使用解析解计算一次匹配误差还是比较大，这里可以采用迭代的方法次计算。每一次迭代我们都会得到当前的最优变换参数  ，然后将该变换作用于当前源点云；“找最近对应点”和“求解最优变换”这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有：·

R,t的变化量小于一定值

loss 变化量小于一定值

达到最大迭代次数

PL（point-line）ICP算法（点到线）

点到点ICP算存在以下缺点：依赖初始值，初始值不好时，迭代次数增加；对于较大的初始误差，可能会出现错误的迭代结果；ICP是一阶收敛，收敛速度慢（为了弥补这一点，通常使用K-D树加快搜索）；会有离群点及噪声。为此改善上述缺点，有人提出了PLICP，顾名思义，这种方式使用源点云到目标点云直线的距离度量来估计变换。主要区别在于误差函数的构建上。ICP是找最近邻的一点，以点与点之间的距离作为误差，而PLICP是找到最近邻的两点，两点连线，是以点到线的距离作为误差，实际上，后者的误差度量方式更符合结构化场景中的雷达点云的实际情况。因此具有更小的误差（图2）。然而，它对非常大的初始位移误差的鲁棒性较差，因此需要比较精确的初始值。

图2 点到线度量比普通 ICP 中使用的点到点度量更接近表面的距离

点到线的误差函数可以写为：

       （7）

是目标点云中匹配到的最近两个点对应直线（）的法线。作者设计了六个仿真实验，初始位移误差增加（均匀分布；从实验 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算法示意图

最小化损失函数可以写为：

（8）

其中是源点，是对应的目的点，是处的单位法线向量。这种方法的优缺点也很明显：首先，点到平面成本函数允许平坦区域相互滑动；点到平面通常比点到点收敛得更快；迭代次数更少 ；点到平面在每次迭代中速度较慢，并且需要表面法线。

（8）式没有解析解，我们只能使用非线性最小二乘的方法求解，为了加快求解，有研究者发现当两个输入表面之间的相对方向较小时，可以使用近似的方法来有效地求解的线性最小二乘法近似非线性优化问题。

难以优化的原因是R 太复杂，无法优化。在每次迭代中，我们假设旋转的角度很小，即

（9）

那么旋转矩阵可以近似表示为：

（10）

重写损失函数（8）为：

（11）

其中：

（12）

这样非线性的损失函数近似为线性，每一步迭代的最优解x为：

（13）

多次迭代后直到算法收敛。

NICP

Normal Iterative Closest Point (NICP)在匹配两组点云时，将点云的局部特征（法向量，曲率）考虑在内，即在迭代求解过程中，误差函数不仅包含点云之间的法向量的投影距离（同point to plane ICP），还包含了法向量方向误差。相比于上述的方法，NICP更加鲁棒。NICP 算法的特点在于，其在匹配两组点云时并非考虑匹配点云之间的欧氏距离，而是将点云曲面的局部特征作为点对匹配以及计算变换的准则。具体来说主要可以分为以下几部分：计算点云中每个点的特征，即其表面的的法向量（normal）和曲面曲率(curvature)，以标记每个点；根据点的距离和特征找两组点云中的匹配点对；利用最小二乘法最小化，最小化目标函数，以求解点云变换矩阵。此处目标函数包括点面投影和法向量旋转误差。

令为带法线方向的三维点云，即T 为由旋转矩阵R 和平移向量t 参数化的变换矩阵。具有法线的点的⊕算子是

（14）

则可以构造一个六维的误差函数：

（15）

上述误差函数可以通过非线性优化的方法来求解，这里不再赘述，感兴趣的读者可以寻找参考文献[4].

参考

[1] https://yilingui.xyz/2019/11/20/191120_point_cloud_registration_icp/

[2] Censi A. An ICP variant using a point-to-line metric[C]//2008 IEEE International Conference on Robotics and Automation. Ieee, 2008: 19-25.

[3] Low K L . Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration[J]. Chapel Hill, 2004.

[4] 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.

[5] https://zhuanlan.zhihu.com/p/110428934

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区

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

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

15.国内首个3D缺陷检测教程：理论、源码与实战

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

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

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

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

▲长按加微信群或投稿

▲长按关注公众号

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

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

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

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