PCL中NDT（Normal Distributions Transform）论文及源码解析

PCL中NDT（Normal Distributions Transform）论文及源码解析   PCL库中NDT模块代码结构如下 分成两部分，其中ndt.hpp中实现了ndt算法的配准过程：计算点所在概率函数及其梯度，并实现高斯牛顿法的最优化求解过程。另一部分voxel_grid_covariance.hpp实现了一个基于体素率波器的正交分布变换，实现了将离散的点云转换为连续的地图。这一过程也被成为NDT（Normal Distributions Transform），其效果如下： 上图是对2D激光点云进行NDT后生成的概率图中的一部分，其中蓝色的点代表采集到的点云，红黄的代表对应生成的高斯概率分布。NDT将2维空间分割为网格（每个网格中点的数量大于5或其他），计算网格中点的均值以及协方差。使用计算出来的均值以及协方差生成对应的高斯分布概率密度，代表网格中对应位置存在点在真实世界中的可能性。 上图中展示了3D点云数据经过NDT的结果，其中左侧是原始点云数据，右侧是经过NDT变换后生成的概率密度地图。   NDT配准通过最优化的方法优化两幅3D点云数据（目标点云，输入点云）间的旋转矩阵（从输入点云转换到目标点云之间的旋转矩阵），使的输入点云进行空间变换后新生成的点云所在位置存在点的概率密度之和最大。即其优化的目标函数形式如下： 其中T(P, xk)代表输入点云中某一个点xk，经过旋转矩阵p的转移后所在的空间坐标。其中p()代表根据目标点云计算出来的点的概率分布密度。 整个NDT算法的流程如下： 【初始化】 构建网格结构B 对目标点云中的所有点进行如下操作     找到该点所对应的网格，并将该点存储到该网格之中。   对网格结构B当中的某一个网格bi计算：     网格中所有点的均值以及协方差，具体计算公式如下：        【配准】 While 拟合成功 do     Score = 0    g = 0     H = 0     对输入点云中的每一个点：         寻找经过T转换后的点所在的网格位置                  更新g和H          计算并更新p   NDT的扩展： 固定尺度离散化 Oct尺度离散化 迭代的离散化 聚类离散化 不同的离散化对NDT影响很大：较大的网格无法用高斯模型来表示真实世界的曲面形状。网格太小，可能高斯模型完全被噪声决定。 Linked cell ：使用kDtree存储网格，加速查找邻居速度 三次线性插值：用附近点和自己概率函数的三次线性插值值代替自己的概率，为了克服离散化带来的影响 图中左侧为原来的概率分布，右边为经过三次线性插值后的概率分布，使得概率比密度分布更加平滑。减少导数不存在的情况。