自动驾驶（十）---------正态分布变换（NDT）

正态分布变换（NDT）

       在点云匹配中，ICP基于距离直接最优化变换矩阵的参数，由于是欠定方程且旋转矩阵的约束，使得结果很难优化，为此在新的维度优化变换矩阵的参数，被很好的提出：

       先将参考点云（目标点云）转换为多维变量的正态分布，匹配的点云如果采用某组变换参数后，使得新的点云和目标点云的正态分布参数匹配很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

多元正态分布

       我们知道，如果随机变量X满足正态分布X∼N(μ,σ),则其概率密度函数为：

                                                                        

      对于多元正态分布而言，其概率密度函数可以表示为：

                                                                    

NDT算法流程：

      将参考点云网格化，计算每个网格的概率密度函数：

                                                                    

      网格的概率密度函数则为：              

变换参数和最大似然：

      我们需要优化的参数就是对当前点云的坐标变换（旋转，平移等），转换函数表示使用姿态变换  来变换，结合之前的一组状态密度函数，那么最好的变换参数 应该是最大化似然函数的姿态变换：

                                                              

      那么最大化似然也就相当于最小化负对数似然 −logΘ;

                                                             

      就到了我们最熟悉的最优化的部分了。

      现在的任务就是使用优化算法来调整变换参数 来最小化这个负对数似然。NDT算法使用牛顿法进行参数优化。

注意：

      1. 经过变换后的点云，只需要和临近的网格进行匹配，就算和较远的网格匹配，也因为距离太远而概率很小，对最后的最大化结果没有什么帮助。