论文翻译—3D NDT算法论文（节选6.1-6.2）

第6章 正态分布变换

本章详细描述了正态分布变换以及它是如何被应用到扫描配准中的。

6.1 用于表面表达的NDT

在第3章讨论的距离传感器都是输出的点云：来自表面的空间采样点的集合。而且，在第5章的工作中提到的许多相关的算法都与点云打交道。但是，使用点云来表达表面有一系列的限制。例如，点云没有包含如方向、平滑度或者洞（holes）等表面属性的明确信息。根据传感器的配置，点云也可能低效的，需要不必要的大量存储。为了在远离传感器的位置得到足够的采样分辨率，通常需要对传感器进行配置，使得从传感器附近的表面产生大量冗余数据。

    正态分布变换可以被描述为一种紧凑的表示表面的方法。它首先是由 Biber 和 StraBer 在2003年【7】作为一个2D扫描配准方法提出的。 Biber 和 StraBer 后来在与 Sven Fleck【8】 的联合论文中详细阐述了该方法（在扫描配准和建图的背景下）。变换将点云映射成一个光滑的平面表达，该表达被描述为一组局部概率密度函数，每个函数都描述了表面截面的形状。

    算法的第一步是将扫描占用的空间细分为一个单元栅格（在2D空间中是正方形，在3D空间中是立方体）。根据在单元里面的点的分布计算每个单元的概率密度函数。每个单元格中的概率密度函数可以被解释为单元格内表面点 $\vec{x}$ 的生成过程。换句话说，假设 $\vec{x}$ 的位置是通过从这个分布中绘制生成的。假设参考扫描面点的位置是由一个D维正态随机过程产生的，已经测量的 $\vec{x}$ 的似然为：

$$p(\vec{x})=\frac{1}{(2\pi {)}^{\frac{D}{2}}\sqrt{|\Sigma |}}\exp (-\frac{(\vec{x}-\vec{\mu }{)}^T{\Sigma }^{-1}(\vec{x}-\vec{\mu })}{2}),\text{\hspace{1em}(6.1)}$$

其中，$\vec{\mu }$ 和 $\Sigma$ 分别表示 $\vec{x}$ 所在的单元内的参考扫描表面点的均值向量和协方差矩阵。因子 $((2\pi {)}^{\frac{D}{2}}\sqrt{|\Sigma |}{)}^{-1}$ 对函数缩放，使其积分为1。 出于实用目的，用一个常数 $c_0$ 来代替它。均值和协方差的计算公式如下：

$$\vec{\mu }=\frac{1}{m}\sum _{k=1}^m{\vec{y}}_k,\text{\hspace{1em}(6.2)}$$

$$\vec{\mu }=\frac{1}{m-1}\sum _{k=1}^m({\vec{y}}_k-\vec{\mu })({\vec{y}}_k-\vec{\mu }{)}^T,\text{\hspace{1em}(6.3)}$$

其中 ${\vec{y}}_{k=1,...,m}$ 是包含在该单元中的参考扫描点的位置。

    正态分布给出了点云的具有连续导数的分段平滑表示。每个概率密度函数可以看作是局部表面的近似，描述了表面的位置，方向以及光滑程度。图6.1显示了一个2D激光扫描及其对应的正态分布。图6.2展示了一个矿井隧道扫描的3D正态分布。

图6.1：来自矿井隧道（用点表示的）的2D激光扫描及其描述了表面形状的概率密度函数。在这个例子中，每个单元是一个边长为2米的正方形。越亮的区域表示概率越高。只有单元中超过5个点才会计算概率密度函数。

图6.2：（如上面所示）隧道部分的3D-NDT表面表达，越亮，更密集的部分代表更高的概率。单元的边长为1米。

    因为当前的工作是如此地专注于正态分布，所以让我们更仔细地观察单变量和多变量正态分布的特性。在一维的例子中，正态分布的随机变量 $x$ 有一个确定的期望值 $\mu$ 并且关于该值的不确定性用方差 $\sigma$ 表示。

$$p(x)=\frac{1}{\sigma \sqrt{2\pi }}\exp (-\frac{(x-\mu {)}^2}{2{\sigma }^2})\text{\hspace{1em}(6.4)}$$

公式（6.1）中的多元概率函数 $p(\vec{x})$ 简化为上述一维情况下的 $p(x)(D=1)$。在多维的情况下，均值和方差变为均值向量 $\vec{\mu }$ 和协方差矩阵 $\Sigma$。协方差矩阵的对角元素表示每个变量的方差，且非对角元素表示变量间的方差。图6.3展示了在一，二，三维情况下的正态分布。

在 2D 和 3D 情况下，可以从协方差矩阵的特征向量和特征值评估表面的方向和平滑度。特征向量描述了分布的主成分；即，与变量的协方差的主方向对应的一系列正交向量。根据方差的比例，一个2D的正态分布可以是点状（如果方差相似）或者线状（如果其中一个远大于另一个）或者任意在这两者之间的形状。在3D情况下（如图6.4所示），一个正态分布可以被描述为点或者球（如果方差的大小在所有方向上都相似），线（如果一个方向的方差远大于其他两个），或者平面（如果一个方向的方差远小于其他两个）。【注：这里提到的“在一个方向上的方差”，这里的“方向”应该是由协方差矩阵的特征向量表示，而在这个”方向上的方差“则应该是指的协方差矩阵的特征值】

图6.3：在一，二，三维情况下的正态分布概率密度函数。

图6.4：依赖于 Σ 的特征值间关系的3D正态分布的不同形状。箭头表示分布的特征向量，由对应的特征值进行缩放。

6.2 NDT 扫描配准

当使用NDT进行扫描配准时，目标是找到当前扫描的位姿，使当前扫描的点位于参考扫描表面上的可能性最大化。要优化的参数是当前扫描的位姿估计的旋转和平移，它们可以被编码成一个向量 $\vec{p}$。当前扫描用点云 X = { x ⃗ 1 , . . . , x ⃗ n } \mathcal{X} = \{\vec{x}_1, ... , \vec{x}_n\} X={ x 1​,...,x n​} 来表示。假设有一个空间变换函数 $T(\vec{p},\vec{x})$ 通过位姿 $\vec{p}$ 移动空间中的一个点 $\vec{x}$ 。给定扫描点的概率密度函数(如等式6.1所示) $p(\vec{x})$ ，最好的位姿 $\vec{p}$ 应该使似然函数最大化

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

或者，等价地，最小化 $\Psi$ 的负对数：

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

    概率密度函数并不限于正态分布。任意能够局部地捕捉表面点的结构并且对异常值是鲁棒的概率密度函数都是合适的。对于远离均值的点，正态分布的负对数似然增长没有限制。因此，扫描数据中的异常值可能对结果有很大的影响。在这项工作中（正如Biber, Fleck, 和 StraBer【8】论文中），使用正态分布和均匀分布的混合：

$$\overline{p}(\vec{x})=c_1\exp (-\frac{(\vec{x}-\vec{\mu }{)}^T{\Sigma }^{-1}(\vec{x}-\vec{\mu })}{2})+c_2{p}_o,\text{\hspace{1em}(6.7)}$$

其中 $p_o$ 是异常值的期望比率。使用这个函数，异常值的影响是受限的。图6.5对此有说明。常数 $c_1$ 和 $c_2$ 可以通过求 $\overline{p}(\vec{x})$ 的概率质量在单元格生成的空间内等于 1 来确定（【注：即是概率密度函数的积分为1来确定】）。

图6.5：比较一个正态分布 $p(x)$ 和 混合模型 $\overline{p}(x)$。负对数似然是进行DNT扫描配准时的目标函数。它的导数表征了特定测量对解的偏差。大的 $x$ 的对 $p(x)$ 的影响的增长没有边界，而对 $\overline{p}(x)$ 的影响则有界。

    要优化的对数似然能量函数的和由以下形式的项组成 $-\log \left(c_1\exp \right(-((\vec{x}-\vec{\mu }{)}^T{\Sigma }^{-1}(\vec{x}-\vec{\mu }))/2)+c_2)$。它们没有简单的一阶和二阶导数。然而，图6.5(b)显示，对数似然函数可以，反过来，用高斯来近似。如下形式的函数 $\overline{p}(x)=-\log (c_1\exp (-x^2/(2{\sigma }^2))+c_2)$ 或许可以被高斯 $\tilde{p}(x)=d_1\exp (-d_2{x}^2/(2{\sigma }^2))+d_3$ 来近似。通过要求 $\tilde{p}(x)$ 应该和 $p(x)$ 在 $x=0,\sigma ,\infty$ 处的行为相似来求拟合参数 $d_i$：

$$\begin{array}{rl}{d}_3=& -\log (c_2),\\ d_1=& -\log (c_1+c_2)-d_3,\\ d_2=& -2\log ((-\log (c_1\exp (-1/2)+c_2)-d_3)/d_1).\end{array}\text{\hspace{1em}(6.8)}$$

【注：
当 $x=\infty ,\tilde{p}(x)=\overline{p}(x)\to d_3=-\log (c_2)$ 求出 $d_3$；
当 x = 0 , p ~ ( x ) = p ‾ ( x ) → d 1 + d 3 =