NDT算法原理

NDT算法原理

算法提出原文：The Normal Distributions Transform: A New Approach to Laser Scan Matching(From IEEE)

参考：https://www.cnblogs.com/21207-iHome/p/8039741.html

NDT是基于栅格化地图的算法，主要算法思路：基于2D场景

将参考帧点云地图分为一个个小块(cell)，计算下一时刻的目标帧得到的点云，变换到参考坐标，落到对应参考帧的cell中的概率。就是同一个环境点，不同时刻检测结果，变换到同一坐标，落在同一个cell。通过这种关系得到评分指标，最优化得到R，t。

详细步骤：

1. 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网格或体素（Voxel）；并计算每个网格的多维正态分布参数：

2. 计算网格的概率分布模型：

计算cell中的包含点的中心(各轴均值)，及协方差矩阵：
均值：

协方差矩阵：

点位于Cell中的概率模型：

3. 变换要配准的点云（second/Target scan）到参考(reference)坐标系下(参考点云的坐标系，通常是前一帧)。
   
4. 根据正态分布参数计算每个转换点落在对应cell中的概率
   
5. NDT配准得分（score）：计算对应点落在对应网格cell中的概率之和
   
6. 根据牛顿优化算法对目标函数$-score$进行优化，即寻找变换参数$p$使得$score$的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵：
   令$q=x{\prime }_i-q_i$，则：
   
   根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
   
   其中q对变换参数$p_i$的偏导数$\frac{\partial q}{{\partial p}_i}$即为变换T的雅克比矩阵：
   
   根据上面梯度的计算结果，继续求$s$关于变量$p_i$、$p_j$的二阶偏导：
   
   根据变换方程，向量$q$对变换参数$p$的二阶导数的向量为：
   
7. 跳转到第3步继续执行，直到达到收敛条件为止