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 \vec{y_k} ,\Sigma = \frac{1}{m-1} \sum_{k=1}^m (\vec{y_k} - \vec{\mu})(\vec{y_k} - \vec{\mu})^T
其中：\vec{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}。当前扫描表示为点云\chi = \lbrace x_1,…,x_n \rbrace。假设存在空间变换函数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},\vec{x_k}))
或者说，使得负对数似然最小的\Psi：
-\log{\Psi} = -\sum_{k=1}^n \log{(p(T(\vec{p},\vec{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_2p_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_3d_2 = -2\log ((-\log(c_1\exp(-1/2) + c_2) - d_3) / d_1)

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

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

给定一个点集\chi = \lbrace\vec{x_1},...,\vec{x_n}\rbrace，一个位姿\vec{p}，和一个变换函数T(\vec{p},\vec{x})，NDT在当前参数向量下的计分函数s(\vec{p})如下：
s(\vec{p}) = -\Sigma_{k=1}^n \tilde{p}(T(\vec{p},\vec{x_k}))

此方程对应于点\vec{x_k}在\vec{p}变换时位于基准扫描表面上的可能性。

似然函数要求协方差矩阵的逆: \Sigma^{-1}。在单元格中的点完全共面或共线的情况下，协方差矩阵是奇异的，不能反转。在3D情况下，由3点或更少对点计算出的协方差矩阵总是奇异的。故此PDF仅针对包含多于5个点的单元格计算。此外，为了防止数值问题，只要发现接近奇异，\Sigma就会稍微膨胀。如果的最大特征值\lambda_3大于\lambda_1或\lambda_2的100倍，则用\lambda_j ^\prime = \lambda_3/100代替较小的特征值\lambda_j。用矩阵\Sigma^\prime = V \Lambda^\prime V代替\Sigma，其中V包含\Sigma的特征向量，
\Lambda^\prime = \begin{bmatrix} \lambda_1^\prime & 0 & 0 \\ 0 & \lambda_2 ^\prime & 0 \\ 0 & 0 & \lambda_3 \\ \end{bmatrix}

牛顿算法可以用来寻找优化s(\vec{p}) 的参数\vec{p}。牛顿法迭代求解方程H\Delta \vec{p}=-\vec{g}，其中H和\vec{g}是s(\vec{p})的Hessian矩阵和梯度向量。在每次迭代中，将增量\Delta \vec{p}加到当前姿态估计中，使得\vec{p} = \vec{p} + \Delta \vec{p}。
为了简洁起见，让\vec{x^\prime_k}=T(\vec{p},\vec{x_k})-\vec{\mu_k}。换句话说，\vec{x^\prime_k}是由当前姿势参数转换的点\vec{x_k}，相对于其所属单元格的PDF中心。梯度向量\vec{g}的条目g_i可以被写为：
g_i=\frac{\delta s}{\delta p_i}=\sum^n_{k=1} d_1 d_2 \vec{x^\prime_k}^T \Sigma^{-1}_k \frac{\delta \vec{x^\prime_k}}{\delta p_i} \exp(\frac{-d_2}{2}\vec{x^\prime_k}^T\Sigma^{-1}_k \vec{x^\prime_k})

Hession矩阵H的条目H_{ij}为：
H_{ij}=\frac{\delta^2 s}{\delta p_i \delta p_j} = \sum^n_{k=1} d_1 d_2 \exp(\frac{-d_2}{2}\vec{x^\prime_k}^T\Sigma^{-1}_k\vec{x^\prime_k})(-d_2(\vec{x^\prime_k}^T\Sigma^{-1}_k \frac{\delta \vec{x^\prime_k}}{\delta p_i}) (\vec{x^\prime_k}^T\Sigma^{-1}_k \frac{\delta \vec{x^\prime_k}}{\delta p_j}) + \vec{x^\prime_k}^T \Sigma^{-1}_k \frac{\delta^2 \vec{x^\prime_k}}{\delta p_i \delta p_j} + \frac{\delta \vec{x^\prime_k}}{ \delta p_j} \Sigma^{-1}_k \frac{\delta \vec{x^\prime_k}}{\delta p_i})

NDT评分函数的梯度和Hessian以相同的方式表示，无论它是以二维还是三维（或任何其他维度）进行配准的。它们同样独立于正在使用的转换表示。上述两个方程中x^\prime的一阶和二阶偏导数确实取决于变换函数T。

---

6.2.1 2D-NDT

对于二维配准，有三个转换参数需要优化。设\vec{p}=[t_x，t_y，\phi]^T，其中t_x和t_y为转换参数，\phi为旋转角。使用逆时针旋转，二维转换函数为:
T_2(\vec{p},\vec{x}) = \begin{bmatrix} \cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \\ \end{bmatrix} \vec{x} + \begin{bmatrix} t_x \\ t_y \\ \end{bmatrix}

利用该二维变换函数，由雅可比矩阵第i列给出了用于计算方程6.12中梯度的一阶导数\delta \vec{x^\prime}/\delta p_i:
J_2 = \begin{bmatrix} 1 & 0 & -x_1\sin \phi - x_2 \cos \phi \\ 0 & 1 & x_1\cos \phi - x_2 \sin \phi \\ \end{bmatrix}

方程式6.13中使用的二阶导数是:
\frac{\delta^2 \vec{x^\prime}}{\delta p_i \delta p_j} = \begin{cases} \begin{bmatrix} -x_1 \cos \phi + x_2 \sin \phi \\ -x_1 \sin \phi - x_2 \cos \phi \\ \end{bmatrix}, & \text {if i = j = 3} \\ \begin{bmatrix} 0 \\ 0 \\ \end{bmatrix}, & \text{otherwise} \end{cases}

---

6.2.2 3D-NDT

NDT二维和三维配准的主要区别在于空间变换函数T(\vec{p},\vec{x})及其偏导数。在二维中，围绕原点的旋转角度用单个值表示，最明显的转换函数是上文中的转换函数。在三维情况下，有几种可能的方法来表示旋转，如第2.2节所述。

在我们之前的3D-NDT工作中，使用了轴/角旋转表示。但是这样做会给优化问题增加一个额外的变量，并且需要额外的约束，以便使旋转轴保持单位长度。牛顿优化方法是一种迭代方法，在每次牛顿迭代后，只需对旋转表示进行重新归一化，就可以简单地实施单位轴约束。然而，当牛顿更新方向偏离位姿参数空间的不可行区域时，该策略仍然会导致问题，这可能解释了先前结果中的一些不一致性。

在下面我们将使用三维欧拉角，尽管与此旋转表示相关的潜在问题。但是欧拉角的优点（如数值优化程序不需要约束，并且稍微不复杂的导数）被评估为优于万向节锁的风险，万向节锁只会在很大的角度上发生，本地配准很可能无论如何都会失败。使用欧拉角，有六个转换参数需要优化：三个用于转换，三个用于旋转。可以使用六维参数向量\vec{p_6} = [t_x, t_y, t_z, \phi_x, \phi_y, \phi_z]^T对姿势进行编码。

使用Euler序列z-y-x，3D转换函数为:
T_E(\vec{p_6}, \vec{x}) = R_x R_y R_z \vec{x} + \vec{t} = \begin{bmatrix} c_yc_z & -c_ys_z & s_y \\ s_xs_yc_z + c_xs_z & s_xs_ys_z - c_xc_z & -s_xc_y \\ c_xs_yc_z - s_xs_z & c_xs_ys_z + s_xc_z & c_xc_y \\ \end{bmatrix} \vec{x} + \begin{bmatrix} t_x \\ t_y \\ t_z \\ \end{bmatrix}
其中c_i = cos \phi_i , s_i = sin \phi_i，由雅可比矩阵第i列给出了用于计算方程6.17中梯度的一阶导数\delta /\delta p_i T_E(\vec{p_6}, \vec{x}) :
J_E = \begin{bmatrix} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \\ \end{bmatrix}
其中：
a = x_1(-s_xs_z + c_xs_yc_z) + x_2(-s_xc_z - c_xs_ys_z) + x_3(-c_xc_y),b = x_1(c_xs_z + s_xs_yc_z) + x_2(-s_xs_ys_z + c_xc_z) + x_3(-s_xc_y)c = x_1(-s_yc_z) + x_2(s_ys_z) + x_3(c_y)d = x_1(s_xc_yc_z ) + x_2(-s_xc_ys_z) + x_3(s_xs_y)e = x_1(-c_xc_yc_z ) + x_2(c_xc_ys_z) + x_3(-c_xc_y)f = x_1(-c_ys_z ) + x_2(-c_yc_z)g = x_1(c_xc_z - s_xs_ys_z) + x_2(-c_xs_z - s_xs_ys_z)h = x_1(s_xc_z + c_xs_ys_z) + x_2(c_xs_yc_z - s_xs_z)

二阶导数(\delta^2 / (\delta p_i \delta p_j)) T_E(\vec{p_6}, \vec{x})对应\vec{H_{ij}}对称矩阵块：
H_E = \begin{bmatrix} \vec{H_{11}} & \cdots & \vec{H_{16}} \\ \vdots & \ddots & \vdots \\ \vec{H_{61}} & \cdots & \vec{H_{66}} \\ \end{bmatrix} = \begin{bmatrix} \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} \\ \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} \\ \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} & \vec{0} \\ \vec{0} & \vec{0} & \vec{0} & \vec{a} & \vec{b} & \vec{c} \\ \vec{0} & \vec{0} & \vec{0} & \vec{b} & \vec{d} & \vec{e} \\ \vec{0} & \vec{0} & \vec{0} & \vec{c} & \vec{e} & \vec{f} \\ \end{bmatrix}

其中：
\vec{a} = \begin{bmatrix} 0 \\ x_1(-c_xs_z - s_xs_yc_z) + x_2(-c_xc_z + s_xs_ys_z) + x_3(s_xc_y) \\ x_1(s_xs_z + c_xs_yc_z) + x_2(-c_xs_ys_z - s_xc_z) + x_3(-c_xc_y) \\ \end{bmatrix}\vec{b} = \begin{bmatrix} 0 \\ x_1(c_xc_yc_z ) + x_2(-c_xc_ys_z) + x_3(c_xs_y) \\ x_1(s_xc_yc_z ) + x_2(-s_xc_ys_z) + x_3(s_xs_y)\\ \end{bmatrix}\vec{c} = \begin{bmatrix} 0 \\ x_1(-s_xc_z - c_xs_ys_z) + x_2(-s_xs_z - c_xs_yc_z) \\ x_1(c_xc_z - s_xs_ys_z) + x_2(-s_xs_ys_z - c_xs_z)\\ \end{bmatrix}\vec{d} = \begin{bmatrix} x_1(-c_yc_z) + x_2(c_ys_z) + x_3(-s_y) \\ x_1(-s_xs_yc_z ) + x_2(s_xs_ys_z) + x_3(s_xc_y) \\ x_1(c_xs_yc_z ) + x_2(-c_xc_ys_z) + x_3(-c_xc_y) \\ \end{bmatrix}\vec{e} = \begin{bmatrix} x_1(-c_ys_z ) + x_2(-c_yc_z) \\ x_1(-s_xc_ys_z ) + x_2(-s_xc_yc_z) \\ x_1(c_xc_ys_z ) + x_2(-c_xs_ys_z) \\ \end{bmatrix}\vec{f} = \begin{bmatrix} x_1(-c_yc_z ) + x_2(c_ys_z) \\ x_1(-c_xs_z - s_xs_yc_z ) + x_2(-c_xc_z - s_xs_ys_z) \\ x_1(-s_xs_z + c_xs_yc_z ) + x_2(-c_xs_ys_z - s_xc_z) \\ \end{bmatrix}

使用以下小角度的三角近似可以显著简化计算：
\sin \phi \approx \phi ,\cos \phi \approx 1,\phi^2 \approx 0.

对于小于10°的角，这些近似值可以被认为是精确的。对于正弦函数，近似误差在14°角时达到1%。对于余弦，相同的误差在8.2°角发生。当使用小角度近似法时，计算变换函数及其导数的速度更快，但在某些情况下，使用近似法进行配准的鲁棒性比使用方程式6.17的情况下要低。在时间临界的应用中，推荐近似公式，尽管差异非常小。

对于许多非线性优化的应用，通常使用Hessian矩阵的数值近似而不是分析Hessian矩阵，这可能是因为Hessian矩阵不能进行分析计算，或者计算上太昂贵。由于NDT评分函数的Hessian矩阵可以进行分析计算，且其大部分元素为零，因此最好不要使用近似值。

---

这一部分可以说是这篇论文的重中之重了，内容比较多，也比较复杂。NDT原理部分先写到这里，太多了实在写不动了……好累……