hdl_graph_slam源码解析（三）

3. scan_matching_odometry

Hello，热爱SLAM的小伙伴们，大家好！在经历了惊魂体检、春节土嗨以及爆肝论文后，越来越懒的魏小新继续给大家带来鸽了快半年的源码解析（三），期间也是十分惊讶原来自己写的东西真的有人看有人催更，这更加坚定了我和大家一起分享、解读优秀开源SLAM代码的想法，希望自己可以一直这样永葆热情。废话不多说，let’s do it！

3.3 ndt

NDT，即是normal distribution transformation的缩写，翻译成中文就是正太分布变换了，哦不对，是正态分布变换！该算法的核心思想是首先将空间离散为方格，若是二维空间，则离散为栅格，若是三维空间则离散划分为立方体，没错就像切豆腐一样。然后这样做有什么好处呢，笔者觉得这样就可以将采样的点云划分到不同的网格中，这样可以很方便的描述点云的局部特性，例如点云局部的形状（直线、平面or球体）、方向（平面法向、直线方向等）。例如现在我们可以利用统计的方法分析每一块豆腐的特性。

所谓正态分布变换，当然是采用正态分布的形式来拟合分析点云的局部特性。至于为什么用正态分布来描述呢？那还不是因为正态分布足够优秀！首先啊，相信大家都对正态分布的公式很熟悉，这种分布呢不仅是一种广泛存在的数据分布形式，同时又具有良好的数学性质，均值和方差（高维情况：均值向量和协方差矩阵），因此总是能在各种SLAM中的建模中看到它。其次，更有趣的是，高维情况下的协方差矩阵还能反映点云的局部特性。假设现在已知有$N$个点落在了一个格子里面，为了简化问题就说是二维情况吧，然后我们可以很容易的求出这$N$个点的均值$\mu$和协方差矩阵$\Sigma$，显然对于二维情况，该协方差矩阵为$2\times 2$的矩阵。若这几个点在一条直线上，则容易知道该协方差矩阵的秩为1，因此对应的特征值其中一个为0；而当这几个点近似在一条直线上时，则协方差矩阵的一个特征值会明显大于另外一个。当大致均匀分布时，两个特征值会大小类似。因此，正态分布不仅具有良好的数学性质来描述分布，还能够反映点云的局部形态。

既然正态分布可以反映点云的局部形态，自然可以利用正态分布来表示局部点云，这样就可以利用数学公式来表示一组离散的点云。

用公式表示就是：
p ( x ) = 1 2 π ∣ Σ ∣ exp ⁡ { − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) } p(x)=\frac{1}{\sqrt{2\pi|\Sigma|}}\exp\{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\} p(x)=2π∣Σ∣ ​1​exp{ −21​(x−μ)TΣ−1(x−μ)}

3.4 ndt matching

上一节中我们说可以用正态分布来拟合表示点云，这种方法可以很好的描述点云的局部形态。既然本节的主题是scan matching odometry，那么肯定要想怎么把这种表示方法应用到点云配准，进而用到激光里程计中呢？如下图所示，图中左侧是用正态分布拟合给定参考点云（上一时刻的激光扫描）后的示意图，图中红色椭圆是高斯分布对应的协方差椭圆。图中右侧当前时刻的激光扫描（目标点云），怎样利用ndt方法确定两组点云对应的位姿变换矩阵$T$呢？

对于目标点云中的一点，我们可以根据当前假设的位姿变换矩阵$T$计算其在参考点云坐标系内的坐标，并根据该坐标确定其在参考点云中对应的正态分布，则可以根据正态分布的概率计算方法，计算该点满足对应正态分布的概率$p_{\mu ,\Sigma }(x)$。则计算完目标点云中所有的点后，可以得到这样一个联合概率：
$$p=\prod _{i=1}^N{p}_i(Tx{)}_{{\mu }_i,{\Sigma }_i}$$
由极大似然估计，可以得到对位姿变换矩阵 T T </