scan-match算法之NDT

NDT-Scan Alignment-Algorithm

作者QQ：845421543。NDT是scan-match中经典的方法之一。NDT算法相对于ICP算法速度更快，稳健性更强。
原文地址：The Normal Distributions Transform: A New Approach to Laser Scan Matching

输入：两帧点云数据 $S_1,S_2$

输出：位姿变换参数 $R,t$

1. 对 $S_1$ 计算正太分布变换(NDT)；
2. 初始化需要估计的参数 $R,t$，设置为0或者采用里程计数据；
3. 根据参数 $R,t$ 将 $S_2$ 中的所有样本点投影到对应的坐标系下；
4. 确定每一个映射点对应的正态分布即 $S_2$ 映射点属于 $S_1$ 中的哪一个cell；
5. 通过评估每个映射点的分布并对结果求和来确定参数的得分 $score$；
6. 通过尝试优化分数来计算新的参数估计。这是通过执行牛顿算法（非线性最小二乘中一种方法）的一个步骤来实现的；
7. 转到3，直到满足收敛条件。

详解：
$NDT$
将2D空间划分为具有固定大小的元胞(cell)，分别对每一个元胞进行如下操作：

• 1)确定cell中的点云 $x_{i=1...n}$
• 2)计算均值 $q=\frac{1}{n}{\sum }_{i=1}^n{x}_i$
• 3)计算协方差矩阵 $\Sigma =\frac{1}{n}{\sum }_{i=1}^n(x_i-q)(x_i-q{)}^t$

Cell中每一个点的概率服从正态分布 $N(q,\Sigma )$，概率可以表示为：
$p(x)\sim e^{-\frac{(x-q{)}^t{\Sigma }^{-1}(x-q)}{2}}$

$SCORE$
$score(p)={\sum }_{i=1}^n{e}^{-\frac{(x_i^{{}^{\prime }}-q_i{)}^t{\Sigma }_i^{-1}(x_i^{{}^{\prime }}-q_i)}{2}}$

其中 $x_i^{{}^{\prime }}$ 代表 $S_2$ 中的点 $x_i$ 映射到 $S_1$ 坐标系之后对应的点； $q_i$ 代表 $x_i^{{}^{\prime }}，{\Sigma }_i$ 在 $S_1$ 中所处的元胞cell的均值和协方差