NDT 公式推导及源码解析（1）

NDT 的论文在去年就看了，代码的话也零零散散看了一些，但直到最近才决定抽出时间把 NDT 的论文和代码重新整理记录一下，方便日后学习。本文主要参考的论文有 [1,2,3]，其中 [1] 是 NDT 被首次提出时发表的论文，如论文名所指出的——NDT 是一种 laser scan matching 方法。[2] 则是 Magnusson 等人将 NDT 从 2D 扩展到 3D 中的论文，[3] 是 Magnusson 的博士论文，论文的内容很多，我阅读的主要是介绍 NDT 的第六章，内容十分清晰详实。[4,5] 则是在 [3] 中的参考文献找到的论文，挑着 [4] 的高斯近似看了一下。

简单介绍

NDT 的直观介绍的话建议看 [1] 或 [7]，这里就不重复赘述了，本文主要关注的还是 NDT 的公式推导（不含 line search 部分的公式）和源码解析（不含 line search 部分的代码），公式主要来自于 [3] 第六章，代码主要来自于 Autoware[6] 的 ndt_cpu 库。

6.1 NDT for representing surfaces

直接使用点云的缺点是：1. 点云中没有直接包含平面的特征信息，如朝向、平滑性等；2. 直接使用点云有点 inefficient，需要大量存储空间。NDT 则使用局部 PDF(probability density function) 来描述点云的局部分布

$$p(\vec{x})=\frac{1}{(2\pi {)}^{D/2}\sqrt{|\Sigma |}}exp(-\frac{(\vec{x}-\vec{\mu }{)}^T{\Sigma }^{-1}(\vec{x}-\vec{\mu })}{2})\text{\hspace{1em}(6.1)}$$
$$\begin{gathered} \vec{\mu }=\frac{1}{m}\sum _{k-1}^m\overrightarrow{y_k}, \\ \Sigma =\frac{1}{m-1}\sum _{k-1}^m(\overrightarrow{y_k}-\vec{\mu })(\overrightarrow{y_k}-\vec{\mu }{)}^T \end{gathered}$$
其好处是：1. 正态分布局部是平滑的，具有连续的导数；2. 每个 PDF 可以认为是局部平面的近似，描述了平面的位置（均值 $\mu$）、朝向、形状和平滑性（协方差 $\Sigma$ 的特征向量和特征值，特征向量描述的点云分布的主成分（principal components））等特征。在 2D 场景中，1. 如果方差比较近似，局部点云的形状为 point；2. 如果一个方差远大于另一个方差，局部点云的形状为 line。在3D 场景中，1. 如果方差比较近似，局部点云形状为 point/sphere；2. 如果一个方差远大于另外两个方差，局部点云形状为 line；如果一个方差远小于另外两个方差，局部点云形状为 plane。如图 6.4。

6.2 NDT scan registration

NDT scan registration 的目标很简单，最大化下面似然函数：

$$\Psi =\prod _{k-1}^{n}p(T(\vec{p},\overrightarrow{x_k}))\text{\hspace{1em}(6.5)}$$

其中 χ = { x 1 ⃗ , . . . , x n ⃗ } \chi = \{\vec{x_1},...,\vec{x_n}\} χ={ x1​ ​,...,xn​ ​} 是要匹配的 source 点云，$T(\vec{p},\vec{x})$ 表示将点 $\vec{x}$ 通过位姿 $\vec{p}$ 旋转平移到 target 点云中。通过负对数将原问题转换为最小化问题：

$$-log\Psi =-\sum _{k=1}^{n}log(p(T(\vec{p},\overrightarrow{x_k})))\text{\hspace{1em}(6.6)}$$

虽然论文里说公式里的概率分布 PDF 并不局限与正态分布，只要能描述点云的局部特征并且对噪点鲁棒就行，但要想换成其他分布估计要花费挺大精力去做实验室