《 无人驾驶原理与实践》一3.3正态分布变换

3.3正态分布变换

      正态分布变换（Normal Distribution Transform，NDT）算法是基于标准正态分布的配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以计算速度比其他方法快。
       这个算法耗时相对稳定，与给定的初值关系不大，初值误差大时也能很好地纠正过来。计算正态分布是在初始化时进行的一次性工作，不需要消耗大量代价来计算最近邻搜索匹配点，概率密度函数在两幅图像采集的间隔可以通过离线计算出来。接下来本文将对其进行详细介绍。

3.3.1NDT算法介绍

       由概率和统计知识可知，如果随机变量X满足正态分布（即X~N(μ,σ)），则它的概率密度函数（Probability Density Function，PDF）为：

       其中μ为正态分布的均值，σ2为方差，这是低维度的情况。对于多元正态分布，其概率密度函数可表示为：

       其中x→表示均值向量，D是维度，Σ表示协方差矩阵，其对角元素表示的是对应元素的方差，而非对应元素（行与列）的相关性。图34为服从二维正态分布的概率密度图。

       其概率密度函数为钟形曲面，它的概率密度函数的等高线是一个椭圆线，并且二元正态分布的两个边缘分布都是一元正态分布。
NDT算法的基本思想是先根据参考数据来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配得很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据匹配得最好。

3.3.2NDT算法的基本步骤

       （1）将参考点云网格化，并计算每个网格的多维正态分布参数

即将参考点云所占的空间划分成指定大小的网格。对于三维地图，就是用一个个小立方体将整个空间的扫描点划分为网格。然后对于每个网格，基于网格内的点计算其概率密度函数。其中yk→=1,…,m表示一个网格内所有的扫描点。
       均值： μ→=1m∑mk=1yk→
       协方差矩阵： Σ=1m∑mk=1(yk→-μ→)(yk→-μ→)T
       概率密度函数：f(x→)=1(2π)32Σe-(x→-μ→)TΣ-1(x→-μ→)2

       （2）初始化变换参数和求最大似然

       我们使用NDT配准的目标是找到当前扫描的姿态，使得当前扫描的点位于参考扫描表面上的可能性最大化。为了对当前的点云进行变换（平移、旋转等），我们用p→表示变换参数。当前扫描的结果为点云X={x1→, …, xn→}，给定扫描集合X，初始化变换参数p→，用空间转换函数T(p→, xk→)来表示使用姿态变换p→来移动点xk→。再结合之前的一组概率密度函数（每个网格都有一个PDF），那么最好的变换参数p→就应该是最大化似然函数的姿态变换：

求最大似然也就相当于求最小负对数似然-logθ

（3）优化参数

       这里的任务是使用优化算法来调整变换参数p→，令负对数似然最小化，在NDT算法中使用了牛顿法来进行，这里不展开介绍。