点击上方“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）计算转换矩阵的两个步骤。粗准确性是指在两点云之间的变化完全未知的情况下进行更粗糙的准确性，主要目的是为精准确性提供更好的初始变化值；精准准则是给出初始变化，进一步优化更准确的变化。其中，通过非线性优化进一步优化结果，粗配准存在分析解决方案。
ICP算法流程
对于point-to-point ICP问题，最佳变换是分析（closed-form solution）是的，可以用SVD 分解计算。这种分析可以作为粗配准的结果。
SVD旋转和平移是分开的。我们首先计算最佳旋转，并根据最佳结果计算最佳平移。为了消除平移的影响，首先将源点云和目标点云转换为纹理坐标
点云配准问题loss可以写为：
（1）
展开有：
（2）
因为点云是坐标确定的，所以最小化loss转化为下：
（3）
根据异常值和正交矩阵的非负性质（正交矩阵中的元素绝对值不大于） 1)很容易证明只有M是单位阵时最大的，即:
（4）
考虑到对于旋转矩阵的约束，最终解为：
（5）
最优平移自然可以得到：
（6）
我们发现点对点ICP问题有最优解的解析。但只使用分析解计算一次匹配误差还是比较大的，这里可以采用迭代法次计算。每次迭代，我们都会得到当前的最佳变换参数，然后将此转换作用于当前源点云；在满足迭代终止条件之前，不断迭代寻找最新对应点和解决最佳转换两个步骤。常用的终止条件包括：·
变化小于一定值
loss 变化小于一定值
达到最大迭代次数
点到点ICP有以下缺点：依靠初始值，初始值不好，迭代次数增加；对于较大的初始误差，可能会出现错误的迭代结果；ICP是一阶收敛，收敛速度慢(为了弥补这一点，通常使用K-D树木加速搜索)；会有离群点和噪音。为了改善上述缺点，有人提出PLICP，顾名思义，这种方法利用源点云到目标点云直线的距离度量来估计变化。主要区别在于误差函数的构建。ICP以点与点之间的距离为误差，寻找最近的邻居PLICP是找到最近邻的两点，两点连线，是以点到线的距离作为误差，实际上，后者的误差度量方式更符合结构化场景中的雷达点云的实际情况。因此误差较小（）。然而，它对初始位移误差较大的鲁棒性较差，因此需要更准确的初始值。
图2点到线量比普通ICP 点到点测量更接近表面的距离
点到线的误差函数可以写：
（7）
是目标点云中匹配的最近两点对应直线()的法线。作者设计了六个模拟实验，增加了初始位移误差个模拟实验 1 中的 [±0.05m，±0.05m，±2°] 到实验 6 中的 [±0.2m，±0.2m，±45°]）。上述过程适用于每个实验 778 每次重复扫描中的每一次重复 100 次。点云的转换是通过非线性优化来解决的。这里给出算法的模拟结果：
图3PLICP与其他点云匹配算法的模拟结果进行比较
使用点到平面（point-plane）最近的点是误差测量的迭代(ICP) 该算法已被证明比使用点到点（point-point）误差测量算法收敛得更快。在 ICP 在算法的每次迭代中，通常使用标准的非线性最小二乘法来解决从最小点到平面误差的相对位置变化。例如 Levenberg-Marquardt 方法。当用点测量到平面误差时，最小化的对象是每个源点与相应目标点的切割平面之间的平方距离之和(见图 4）。
图4point-to-plane ICP算法示意图
最小损失函数可以写为：
（8）
其中是源点，是相应的目的点，是单位法线向量。该方法的优缺点也很明显：首先，点到平面成本函数允许平面区域相互滑动；点到平面通常比点到点收敛更快；迭代次数较少 ；每次迭代中点到平面速度较慢，需要表面法线。
（8）类型没有分析，我们只能使用非线性最小二乘法来解决。为了加快解决方案，一些研究人员发现，当两个输入表面之间的相对方向较小时，可以使用类似的方法来有效地解决线性最小二乘法的近似非线性优化问题。
难以优化的原因是R 它太复杂了，无法优化。假设旋转角度很小，即每次迭代都很小
（9）
旋转矩阵可以近似表示为：
（10）
重写损失函数(8)为:
（11）
其中：
（12）
非线性损失函数类似于线性，每一步迭代的最佳解x为：
（13）
算法在多次迭代后收敛。
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
作者也是我们特邀嘉宾：
本文仅做学术分享，如有侵权，请联系删文。
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缺陷检测教程：理论、源码与实战
扫码添加小助手微信，可
也可申请加入我们的细分方向交流群，目前主要有、、、、、等微信群。
一定要备注：，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。也请联系。
▲长按加微信群或投稿
▲长按关注公众号
：针对3D视觉领域的五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、答疑解惑、助你高效解决问题