基于最小化三维NDT距离的快速精确点云配准

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

干货第一时间送达

作者丨paopaoslam

来源丨 泡泡机器人SLAM 

标题：Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations

作者：Todor Stoyanov, Martin Magnusson, Henrik Andreasson and Achim J Lilienthal

来源：IJRR 2012

编译：徐奕聪

审核：阮建源 王志勇

摘要

距离传感器的量测配准在移动机器人领域中获得了很多的关注。研究者们提出了若干迭代优化的方案来匹配三维点集。随着高帧率扫描仪的广泛使用，移动机器人的应用场景日益广泛，现时的算法已经难以满足业界需求，需要更快更精确的配准方法。本文提出了一套全新的点云配准算法，将业界先进算法的性能提高了一个数量级。算法性能获得提高主要得益于使用了一种紧凑的空间表示方式：三维正态分布变换（3D-NDT）。另外，一种基于3D-NDT的快速全局描述因子被用来可靠地计算迭代算法中的初始姿态。最后，我们推导了该方法协方差的闭合解。我们在两个标准点云数据集上测试了该算法，稳定得到了与业界先进算法相当甚至更优的性能。该算法用开源包ROS进行了实现。

主要贡献

本文延续了前人在3D-NDT匹配方面的工作。在以前的文献中Magnusson已经证明了3D-NDT是一种紧凑的空间表示形式。我们过去的研究中已经提出了一套最小化3D-NDT模型之间L₂距离的算法并且证明了其在点云匹配问题中的有效性。本文进一步论述了分布到分布(Distribution-to-Distribution, D2D) 的3D-NDT匹配算法，并且用若干标准数据集进行了验证。另外，本文还进一步补充了另外两方面的讨论：其一，对于优化迭代算法的初始估计选择；其二，协方差的估计，用于预估算法给出结果的误差。

方法概述

类似于高斯混合模型GMM，3D-NDT模型M_NDT使用一组高斯分布的集合构建。该模型使用一组点集P来得到一组高斯分布集合：

n_P是正态分布的个数，通常远远小于P中的点的数目。该模型的概率密度函数可以表示为若干高斯分布的加权和：

令F和M分别表示固定点集和移动点集。两个集合部分重叠。对准任务的目的是估计变换函数的参数𝚯，使得变换函数T(M, 𝚯)与固定点集F能一致地对准。一个点x是M_NDT(F)产生的概率为：

整个点集M被该模型产生的概率为：

本文所使用的方法不再计算离散点集与模型之间的最大似然，而是直接计算两个3D-NDT模型M_NDT(F)和M_NDT(M)之间的距离。3D-NDT模型代表了观测到空间中一个点对于坐标的概率密度函数，这些点来自于某个物体的表面。这种解释方式认为GMM中的各个成分权值相同并且彼此间可认为没有交集。

我们采用了最小化概率密度函数的正则L₂距离的方法，该距离定义为：

进一步展开为：

任何M_NDT经过变换T(M_NDT, 𝚯)以后依然是一个M_NDT模型。前两项相对于𝚯为不变量，所以可以忽略。同时有等式：

所以我们可以得到：

变换函数T(., 𝚯)可以用不同的旋转表示来参数化。我们用向量p=(t_x, t_y, t_z, r_x, r_y, r_z)来表示。t=(t_x, t_y, t_z)表示位移向量，r_x, r_y, r_z表示对于坐标轴的欧拉角。

最终的D2D对准算法的目标函数为：

u_ij为关于p的函数：

该目标函数的梯度向量：

其中p_a是向量p中的一项，并且：

海森矩阵：

其中：

这些方法使用牛顿法例行的优化流程并且使用More–Thuente方法控制步长即可。

早先提到过该模型中的各成分可认为没有交集，该算法只考虑来自两个3D-NDT模型中距离最近的一对成分用于计算。这大大加快了计算速度并且并不失为对目标函数一种有效的近似。

本文剩余部分详细介绍了旋转运算的表示以及本算法与ICP算法的关系。并且介绍了作为一个迭代方法，如何使用3D-NDT直方图法（Magnusson 2009年提出）来给迭代算法一个接近全局最优结果的一个初始估计。并且计算了估计匹配的协方差。限于篇幅本处不再赘述，详见原文。

主要结果

本节展示了评估3D-NDT D2D匹配算法的结果，包括使用直方图法得出的初始状态猜测和协方差估计。限于篇幅，此处只展示算法本身在两个数据集上运行的结果，及其与ICP算法的对比。

表1. 所用数据集属性

图1. 在Hannover数据集上运行匹配算法的结果，922对扫描点云。(a) 归一化的位移误差。(b) 方向误差。(c) 相对运行时间

图2. 在AASS数据集上运行匹配算法的结果，922个扫描对。(a) 归一化的位移误差。(b) 方向误差。(c) 相对运行时间

图3: 所提出匹配方法在五个扫描对上的结果:

(a)P2D 3D-NDT考虑和不考虑平移误差的成功率。

(b)D2D成功率

(c)ICP成功率

(d)平均位移

(e)成功匹配扫描的旋转误差

(f)三个算法的运行时间分布

总结

本文提出的最小化3D-NDT模型来进行点云匹配的方法在若干标准数据集上得到了广泛的验证，其计算性能快于P2D和标准的ICP实践。本文还展示了两种针对该模型的改进：使用3D-NDT的全局描述子来估计匹配的初始姿态，以及估计算法的协方差。提出了基于3D-NDT产生的直方图的一种快速对准两个该模型的方法。Censi提出的一种方法也被应用于本文模型的目标函数中，得到了一个便于计算的闭环方法来估计匹配协方差。

尽管提出的算法展现出了优越的性能，其局限性不可忽视。作为一种迭代方法，目标函数容易陷入局部最优。因此，用来寻找初始姿态的直方图法若能继续提高，将会进一步提高算法的稳定性。另一个可能的改进方向是在不同传感器的数据集上进一步测试算法，特别是噪声较大、视角较窄的传感器所产生的数据。最后，我们将致力于评估一个完整的基于3D-NDT D2D算法的建图系统。本文所提出的三个部分将和一个闭环检测系统和全局姿态图优化框架结合，我们将建立一个快速、准确、稳定的3D-NDT地图探测系统。

Abstract

Registration of range sensor measurements is an important task in mobile robotics and has received a lot of attention.Several iterative optimization schemes have been proposed in order to align three-dimensional (3D) point scans. With themore widespread use of high-frame-rate 3D sensors and increasingly more challenging application scenarios for mobilerobots, there is a need for fast and accurate registration methods that current state-of-the-art algorithms cannot alwaysmeet. This work proposes a novel algorithm that achieves accurate point cloud registration an order of a magnitudefaster than the current state of the art. The speedup is achieved through the use of a compact spatial representation:the Three-Dimensional Normal Distributions Transform (3D-NDT). In addition, a fast, global-descriptor based on the3D-NDT is defined and used to achieve reliable initial poses for the iterative algorithm. Finally, a closed-form expressionfor the covariance of the proposed method is also derived. The proposed algorithms are evaluated on two standard pointcloud data sets, resulting in stable performance on a par with or better than the state of the art. The implementation isavailable as an open-source package for the Robot Operating System (ROS).

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

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天内无条件退款

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

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