The Three-Dimendional Normal-Distributions Transform论文学习（Chapter 6）

Chapter 6. The normal-distributions transform（一）

这一章详细讲解了NDT算法，以及NDT是怎样应用到扫描配准上的。

6.1 NDT for representing surface

用NDT算法来表示光滑表面，第一步是将扫描所占用的空间细分为网格单元（2D情况下为正方形，3D情况下为立方体）。根据单元内的点分布计算每个单元的PDF（probality distribution function 概率分布函数）。每个细胞中的PDF可以被解释为单元内表面点$\vec{x}$的"生成过程"。换句话说，假定$\vec{x}$的位置是通过从这个分布中提取而生成的。假设参考扫描表面点的位置是由D维正态随机过程产生的，则测量$\vec{x}$的似然函数为：
$$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})$$
其中$\mu$和$\Sigma$分别是扫描表面中由$\vec{x}$组成的网格中的均值向量和协方差矩阵：
$$\vec{\mu }=\frac{1}{m}\sum _{m=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$$
其中：$\overrightarrow{y_{k=1,...,m}}$，是网格单元中包含的参考扫描点的位置。正态分布给出了具有连续导数的点云的分段光滑表示。每个PDF可以看作局部表面的近似图像，描述表面的位置以及它的方向和平滑度。二维激光扫描及其相应的正态分布如图6.1所示。图6.2显示了矿井隧道扫描的3D正常分布。

在一维的情况下，在一维情况下，正态分布的随机变量$x$具有一定的期望值$\mu$，并且关于该值的不确定性用方差$\sigma$表示。方程为：
$$p(\vec{x})=\frac{1}{\sigma \sqrt{2\pi }}\exp (-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$
在多维情况下，均值和方差用均值向量$\vec{\mu }$和协方差矩阵$\Sigma$来描述。**协方差矩阵的对角元素表示每个变量的方差，非对角元素表示变量的协方差。**图6.3显示了一维、二维和三维中的正态分布。
在2D和3D情况下，表面取向和平滑度可以通过协方差矩阵的特征向量和特征值来评估。特征向量描述分布的主成分，即一组与变量协方差的主方向相对应的正交向量。取决于方差的比例，2D正态分布可以是点状（如果方差相似）或线状（如果一个比另一个大得多），或者介于两者之间。在图6.4所示的3D情况下，正态分布可以描述一个点或球（如果方差的幅度在所有方向上都相似）、一条线（如果一个方向的方差远大于其他两个方向的方差）或一个平面（如果一个方向的方差远小于另外两个方向的方差）。

---

6.2 NDT 扫描配准

当使用NDT进行扫描配准时，目标是找到当前扫描的姿态，使得当前扫描的点位于参考扫描表面上的可能性最大化。待优化的参数，即当前扫描的姿态估计的旋转和平移，可以编码为向量$\vec{p}$。当前扫描表示为点云 χ = { x 1 , … , x n } \chi = \lbrace x_1,…,x_n \rbrace χ={ x1​,…,xn​}。假设存在空间变换函数$T(\vec{p},\vec{x})$，该函数通过姿态$\vec{p}$在空间中移动点$\vec{x}$。给定扫描点(例如，等式6.1)的一些PDF $p(\vec{x})$，最佳姿态$\vec{p}$应该是使似然函数最大化的姿态$\vec{p}$
$$\Psi ={\Pi }_{k=1}^{n}p(T(\vec{p},\overrightarrow{x_k}))$$
或者说，使得负对数似然最小的$\Psi$：
$$-\log \Psi =-\sum _{k=1}^n\log (p(T(\vec{p},\overrightarrow{x_k})))$$

PDF不一定局限于正态分布。任何可以局部的描述表面点的结构并对离群值鲁棒的PDF都是合适的。正态分布的负对数似然对于远离平均值的点没有界限地增长。因此，扫描数据中的异常值可能对结果有很大的影响。在本工作中（如Biber、Fleck和Straer[8]的文章），同时使用了正态分布和均匀分布：
$$p(\vec{x})=c_1\exp (-\frac{(\vec{x}-\vec{\mu }{)}^T{\Sigma }^{-1}(\vec{x}-\vec{\mu })}{2})+c_2{p}_o$$
其中，$p_o$是异常值的预期比率。利用这个函数，离群值的影响是有界的。如图6.5所示。常数$c_1$和$c_2$可以通过要求$p(\vec{x})$的概率质量等于单元所跨越的空间中的一个来确定。(没看懂)

对第一种待优化的对数似然函数的求和由形式来说，它们没有简单的一阶和二阶导数。然而，由图我们可以看出对数似然函数反过来可以用高斯函数来近似。在$p(\vec{x})$中，令：$x=0$, $x=\sigma$和$x=\infty$：
$$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)$$

使用这种高斯近似，从当前扫描的一个点对NDT得分函数(score function)的影响是:
$$\tilde{p}(\overrightarrow{x_k})=-d_1\exp (-\frac{d_2}{2}(\overrightarrow{x_k}-\overrightarrow{{\mu }_k}{)}^T{\Sigma }_k^{-1}(\overrightarrow{x_k}-\overrightarrow{{\mu }_k}))$$

其中$\overrightarrow{{\mu }_k}$和${\Sigma }_k$是$\overrightarrow{x_k}$所处的NDT网格的平均值和协方差。这个NDT得分函数具有更简单的导数，但是当用于优化时仍然表现出相同的一般性质。注意，方程中省略了$d_3$项。当使用NDT进行扫描配准时，它不是必需的，因为它仅向得分函数添加常数偏移，并且不改变其形状或优化它的参数。

给定一个点集 χ = { x 1 ⃗ , . . . , x n ⃗ } \chi = \lbrace\vec{x_1},...,\vec{x_n}\rbrace χ={ x1​ ​,...,xn​ ​}，一个位姿$\vec{p}$，和一个变换函数 T ( p ⃗ , x ⃗