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

干货第一时间送达

作者丨paopaoslam

来源丨 泡泡机器人SLAM 、PCL源码

编辑丨玉玺、lionheart

摘要

NDT(Normal-Distributions Transform)算法与ICP算法一样是点云配准中常用的经典算法之一，本文对PCL中NDT代码的实现做一个简单的解析，希望对用到NDT的同学有所帮助。

NDT算法的基本原理在之前的文章中已经介绍过，在这里不再赘述，篇幅有限，本文没有贴出大量源码，仅对源码中重要的函数做相应解析。

图1 NDT 地图示意图

官网示例教程

PCL官网给出了NDT算法的示例教程（https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html#），例程中算法输入几个参数的含义：

1、ndt.setTransformationEpsilon (0.01);//设置NDT迭代收敛阈值，即迭代增量的大小，相当于牛顿法中的delata_x，阈值越小，迭代次数越多；

2、ndt.setStepSize (0.1);//Ndt使用了More-Thuente线性搜索[2]求解位姿增量，这里设置搜索的步长；

3、ndt.setResolution (1.0);//设置NDT网格的分辨率；

4、ndt.setMaximumIterations (35)；//设置最大迭代次数；

跑PCL例程时可能有的同学遇到过跑出来的结果匹配不好的情况，如图2所示，

图2 官网数据NDT匹配不佳效果示意图

后面会从代码实现上对这个现象做相应分析。

主要实现代码(PCL 1.12.0)

    1、创建NDT地图。

NDT的原理是把Target点云按照固定的分辨率划分成3d网格，并计算出每个3d网格点云的分布情况，计算Source点云在Target分布的概率来计算点云的匹配关系的，因此首先需要创建NDT地图。

在setInputTarget时便会先进入初始化函数init();

初始化的主要工作就是把Target点云按照输入的分辨率划分到3d网格中，并计算每个网格的均值与协方差，该过程实现在：

pcl/filters/impl/voxel_grid_covariance.hpp

的applyFilter函数中，整个计算过程比较容易懂，在看源码的过程中可能有问题的在于协方差的计算上，这里pcl协方差的计算在1.12.0版本之前有错误，最后协方差应该是乘以n/(n-1),但是实现写反了如图3所示。

图3 1.7版本NDT协方差计算实现代码截图

图4  1.12.0版本NDT协方差计算代码实现截图

这个问题在pcl1.12.0版本给出了修复，如图4所示。这里为了减小计算量，把协方差计算的公式展开了，简单推导如下（以二维协方差矩阵计算为例）：

    2、计算变换矩阵

计算Source点云与Target点云间的变换矩阵，主要在

pcl/registration/include/pcl/registration/impl/ndt.hpp

的computeTransformation的函数中。实现中有几个重要的部分：

（1）Source点与Target网格的关联。虽然可以根据匹配初值以及Target网格生成的相关信息（坐标原点，分辨率）直接计算出Source里的每个点落在哪个网格里，但是算法实现时仍然采用了kdtree索引的方式搜索每个Source点与Target网格（即网格中心）的关系。主要在computeDerivatives与computeHessian函数中radiusSearch来实现。

图5 ndt近邻搜索代码截图

之所以不直接计算索引而要使用kdtree查找近邻，猜测一个主要的原因是为了一定程度上避免初始误差较大时过多的source点落入空网格或错误网格而无法建立有效的约束，导致算法无法收敛到最优解附近的问题。

但是，图5中有一个细节，radiusSearch输入的搜索半径为resolution_，该变量为setResolution（）函数传入的ndt网格分辨率的变量，即搜索半径设定为一个网格分辨率的搜索半径，所以当初始误差大于网格分辨率时，同样可能会导致算法无法收敛到最优解附近，这也是导致官网例程没有收敛的原因（如图2）之一。

可以试下把初值的平移误差减小，设置为：（1.79387, 0, 0），如图6所示，

图6 修改官网例程初值示意图

修复初值后的收敛效果如图7所示，可见明显优于图2。

图7 修复初值后NDT官网例程配准效果

（2）代价值的计算。PCL中NDT实现的原理，参考的是《The Three-Dimensional Normal-Distributions Transform 》这篇论文。代价函数的计算在代码updateDerivatives（）函数中，如图8所示。

图8 PCL NDT代价值计算代码实现截图

可以看到，完全是论文中公式6.9的实现。

该公式即为单个激光点NDT匹配的代价函数计算公式。熟悉概率论的同学可以看出这个公式就是一个多维正态分布的极大似然函数，其实NDT匹配计算的本质就是一个多维正态分布的极大似然估计。

    3、迭代优化。

优化方法在实现上采用了牛顿法，但是为了增加鲁棒性，只采用了牛顿法计算出来的迭代方向作为步长搜索的初始方向，结合了线性搜索方法[2]计算得到最终的优化方向与迭代步长。该部分实现在computeStepLengthMT函数中，如图9所示。

图9 优化步长搜索算法代码截图

雅可比矩阵以及海森矩阵的计算分别在computeDerivatives以及computeHessian函数中实现，求导过程相对简单，在论文[1]中有详细的推导这里就不再赘述，感兴趣的同学可以去看。

总结

本文主要讲解了PCL中NDT匹配源码的主要实现部分，主要介绍了一些实现的细节：

1、NDT匹配关系的建立也是通过建立kdtree实现的，而没有使用创建体素网格的索引直接计算得到，因此实际使用中，如果要使用NDT做实时的定位匹配，需要控制kdtree搜索的次数以减小计算量。

2、在计算协方差矩阵时，1.12.0以前的版本有笔误，该笔误实际只是对协方差做了一个整体的缩放，因此在实际使用中影响并不大。

3、分析了官方例程数据匹配效果不佳的原因，主要是NDT的实现中搜索半径强制设为了NDT网格分辨率，最多只能搜索一个网格范围的地图网格形成约束，因此初始误差如果大于NDT分辨率，则容易导致NDT匹配不收敛。另外，Autoware的开源代码[3]中也对NDT做了实现，测试PCL官网数据也是可以匹配收敛的，感兴趣的也可以下载试用一下。

4、PCL 代码中关键的实现比如代价函数设计，一阶导二阶导的计算，优化步长的计算，代码都是参考论文[1][2]实现的，论文中有详细的推导，可以帮助读者更好的理解代码。

参考论文与链接

1.The Three-Dimensional Normal-Distributions Transform:https://www.divaportal.org/smash/get/diva2:276162/FULLTEXT02.pdf

2.Line Search Algorithms with Guaranteed Sufficient Decrease:http://www.ii.uib.no/~lennart/drgrad/More1994.pdf

3.https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/localization/ndt

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

3D视觉精品课程推荐：

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从入门到精通：理论与实战

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

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

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

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

▲长按加微信群或投稿

▲长按关注公众号

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

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

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

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