自动驾驶中点云配准常用方法

作者丨大兵小将@知乎

来源丨https://zhuanlan.zhihu.com/p/607917968

编辑丨3D视觉工坊

点击进入—>3D视觉工坊学习交流群

自动驾驶建图定位中常使用ICP、NDT等配准算法，本文主要介绍这两者的基本概念及区别联系。

一、ICP配准

1、基本概念

ICP通过迭代的方式，通过优化相对位姿，不断减小两帧点云之间点到点的距离，直到迭代收敛，最终计算出两帧点云的相对位姿。

2、ICP分类

ICP方法可分为点到点（PointToPoint）、点到线（PointTolane）和点到平面（PointToPlane）三类：
1）PointToPoint：计算P(t)和目标点云T的距离采用点到点之间的距离形式。
2）PointToPlane：计算P(t)中点到目标点云T的点所在直线的距离
3）PointToPlane：计算P(t)中点到目标点云T的点所在平面的距离

3、传统ICP及其变体的比较

1）由于传统经典ICP计算点云中点到点的距离，因此最小二乘期望的是两个点云的完全重合，然而现实环境中由于遮挡关系，两帧点云往往不是完全重合的，可能两帧点云只存在一部分的点是完全重合。由于点云实际上是现实几何环境的离散化表示，同时点云受到噪声的影响，即便在同一平面，离散化得到的点云也不可能完全相同，因此使用点到点的距离无法准确描述点云之间的配准问题。

2）变体ICP算法：点到面、点到线的距离解决了离散化问题。由于点云中的平面只提供了平面法向的约束，因此计算点到面的距离，只考虑平面法向约束，而避免了平面切向方向的影响。

二、NDT配准

1、基本概念

1）正态分布变换（NDT）算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配

2）因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。

3）NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。

4）因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好

2、算法过程

NDT 算法一般过程如下：

已知有两幅点云，分别为源点云 P 和目标点云 Q 。

1 ）将源点云 P 所在空间划分为一个单元一个单元的网格（即三维空间在二维空间上的投影）

2）根据所划分单元网格内点的分布情况，计算该单元网格的正太分布 PDF参数。

3）根据转移矩阵，将目标点云 Q 内的点进行变换；

4）统计源点云 P 所在空间划分网格内目标点云点的个数，根据点的分布情况计算对应的概率分布函数

5）求解所有点的最优值，也即求解目标点云与源点云之间的刚体变换。

3、算法原理

NDT算法原理：

NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布， 如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数 将匹配的最好。

NDT算法关键点：
1）将二维空间划分为固定大小网格，每个网格至少包括3个点（一般5个）

2）计算网格中点集的均值μ

3）计算网格中点集的协方差矩阵Σ

4）网格中的观测到点x的概率p(x)服从正态分布N( μ,Σ)。

三、ICP与NDT的区别与联系

1、ICP相比NDT，更容易受到初值误差的影响，当初始误差较大时，ICP更容易出现无法计算位姿的情况

2、NDT相比ICP，旋转误差更大。因此ICP对旋转的约束更强，NDT对初始误差的健壮性更强

3、NDT的匹配速度要比ICP块，NDT的时间复杂度为O(N)，其时间复杂度与点云的数量成正比，ICP的时间复杂度是O(NlogN)，即通过KD树进行查找的时间复杂度。

4、由于ICP是会进行点到点、点到线、点到面的特征匹配，特征匹配的效果最终决定了点云配准的效果，而NDT不需要进行特征匹配。而NDT是将点云转换为分段连续的正态分布函数，因此NDT对动态物体干扰的抵抗力更强

5、由于NDT对初始误差的敏感度较低，因此当速度较快时，运动方程无法提供准确的先验位姿，此时NDT的效果要由于ICP。

6、当环境中缺少垂直特征时，NDT相比ICP会提供更好的约束，原因是NDT使用栅格正态分布描述点云的局部性质，因此稀少的垂直特征可以体系在某些栅格的正态分布中，而ICP使用点云之间点到点的匹配，垂直点特征的影响会被大面积水平点特征减弱。

7、ICP要求点云的完全重合，这一假设会受到现实各种因素的影响。点云配准不利因素、初始值误差、点云的不重合度、动态物体

8、不同于 ICP 算法，NDT 算法在配准过程中需要删除距离不正确的点对。但是由于每一次的迭代过程计算代价较高，需要搜索所有的临近点

9、在配准时，NDT算法能够直接进行配准和计算，而不需要通过对应点的特征。所以在计算邻近搜索匹配点时不会消耗过多代价，概率密度函数计算较为简单，极大的提高了算法的效率 。因此从时间复杂度的角度上来看，NDT 算法优于 ICP 算法

10、 ICP是点到点之间的匹配，而NDT是利用了概率的特点，将点云划分成栅格，对应匹配栅格匹配服从高斯分布。因此ICP匹配条件更加严格（容易“过拟合”），但是通常两片待匹配的点云，由于采样、遮挡等因素，不可能每个点完全重合（只是一部分重合），因此ICP比NDT更容易受初值影响，也更容易受噪声的影响。鲁棒性NDT优于ICP。

11、ICP的精度要优于NDT。

12、NDT的时间复杂度为O(N)，ICP的时间复杂度为O(NlogN)。运算效率NDT优于ICP。

点云匹配的影响因素：1. 初始位姿；2. 噪声；3. 不重合度；4. 动态物体

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

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

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

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

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

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

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

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

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

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

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

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

17.机械臂抓取从入门到实战

重磅！粉丝学习交流群已成立

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

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

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

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

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

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