《 无人驾驶原理与实践》一3.3.3NDT算法的优点

3.3.3NDT算法的优点

       使用正态分布来表示原本离散的点云有诸多好处，这种通过一个个网格划分出来的光滑表面的表示是连续可导的，每一个概率密度函数可以被认为是一个局部表面的近似。图35为一个3D点云及其网格化效果。

       图35中的立方体边长为1m，其中明亮的位置表示概率高，以此来描述这个表面在空间中的位置。
       除了描述这个表面的位置，同时还包含这个表面的方向和光滑性等信息。图36为协方差矩阵特征值和表面形状之间的关系。

       以三维概率密度函数为例，如果三个特征值很接近，那么这个正态分布描述的表面是一个球面；如果一个特征值远大于另外两个特征值，那么这个正态分布描述的就是一条线；如果一个特征值远小于其他两个，则这个正态分布描述的就是一个平面。这里的概率密度函数其实并不要求一定是正态分布，任何能够反映扫描表面的结构信息并且对异常扫描点具有鲁棒性的概率密度函数都是可以的。

3.3.4NDT算法实例

对于如何使用正态分布（NDT）进行配准，上面简单介绍了一些原理。本小节借用实例介绍如何使用正态分布变换算法来确定两个大型点云（都超过100 000个点）之间的刚体变换，并通过PCL提供的正态分布变换函数来对两个点云图进行配准，点云数据分别保存在 build/cloud1pcd和build/cloud2pcd 文件中。

步骤1读取PCD文件中的点云信息。

代码清单3.6从main函数中分别读取点云

代码清单3.7从两个PCD文件中读取的点的数量

步骤2过滤输入点云。
       对数量特别多的点做优化是非常耗时的，本文使用voxel_filter对输入的点云进行过滤，这里只对input_cloud进行了滤波处理，减少其数据量到10%左右，而target_cloud不做滤波处理。

代码清单3.8过滤输入点云

代码清单3.9过滤后的扫描点数量只有原来的10%

代码清单310初始化NDT并且设置NDT参数

       其中ndtsetTransformationEpsilon()即设置变换的（两个连续变换之间允许的最大差值），它是判断优化过程是否已经收敛到最终解的阈值。ndtsetStepSize(01)设置牛顿法优化的最大步长。ndtsetResolution(10)设置网格化时立方体的边长，在NDT中网格大小设置非常重要，太大会导致精度不高，太小则会导致占用内存过高，并且只有在两幅点云相差不大的情况下才能匹配。ndtsetMaximumIterations(35)即优化的迭代次数，本文设置为35次，即当迭代次数达到35或者收敛到阈值时，停止优化。

       步骤3       初始化变换参数并开始优化。

       我们对变换参数p→进行初始化（给一个估计值），变换参数的初始化数据往往来自测量数据。

代码清单3.12保存配准以后的点云图，输出到文件 cloud3pcd

步骤4    将配准以后的点云图可视化。

       我们写一个函数用于可视化配准以后的点云，其中目标点云（即已有的高精度地图）用红点绘制，而输入点云用绿点绘制。

代码清单3.13将配准以后的点云图可视化

       实验的最终效果图如图37所示，其中红点表示目标点云，绿点表示输入点云，红绿叠加的部分就是目前的匹配效果。