NDT算法原理

基本思想：

NDT 算法的基本思想是，先对待配准点云进行栅格化处理，将其划分为指定大小的网格，通过正态分布的方 式，构建每个网格的概率分布函数，之后优化求解出最优变换参数，使得源点云概率密度分布达到最大，以实现两个点云之间的最佳匹配。

整体流程：

1.将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网格或体素（Voxel）；并计算每个网格的多维正态分布参数；
2.计算网格的概率分布模型:
计算cell中的包含点的中心(各轴均值)，及协方差矩阵：
均值：
协方差矩阵：
点位于Cell中的概率模型：
3.变换要配准的点云（second/Target scan）到参考(reference)坐标系下(参考点云的坐标系，通常是前一帧)。
4.根据正态分布参数计算每个转换点落在对应cell中的概率
5.NDT配准得分（score）：计算对应点落在对应网格cell中的概率之和
6.根据牛顿优化算法对目标函数−score进行优化，即寻找变换参数 p 使得 score的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵：
令，则：根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
其中q对变换参数 pi的偏导数即为变换T的雅克比矩阵：
根据上面梯度的计算结果，继续求s关于变量、的二阶偏导：
根据变换方程，向量q对变换参数p的二阶导数的向量为：
7.跳转到第3步继续执行，直到达到收敛条件为止