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}) \tag{6.1}$p(x)=(2π)D/2∣Σ∣​1​exp(−2(x−μ​)TΣ−1(x−μ​)​)(6.1)
$\vec{\mu} = \frac{1}{m}\sum\limits_{k-1}^m \vec{y_k}, \\ \Sigma = \frac{1}{m-1}\sum\limits_{k-1}^m (\vec{y_k} - \vec{\mu})(\vec{y_k} - \vec{\mu})^T$μ​=m1​k−1∑m​yk​​,Σ=m−11​k−1∑m​(yk​​−μ​)(yk​​−μ​)T
其好处是：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\limits_{k-1}^n p(T(\vec{p}, \vec{x_k})) \tag{6.5}$Ψ=k−1∏n​p(T(p​,xk​​))(6.5)

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

$-log \Psi = -\sum\limits_{k=1}^n log(p(T(\vec{p}, \vec{x_k}))) \tag{6.6}$−logΨ=−k=1∑n​log(p(T(p​,xk​​)))(6.6)

虽然论文里说公式里的概率分布 PDF 并不局限与正态分布，只要能描述点云的局部特征并且对噪点鲁棒就行，但要想换成其他分布估计要花费挺大精力去做实验室验证的。由于当 point 距离 $\mu$μ 太远时（可以认为是离群点），负对数的值很大，结果就是这些离群点对结果有很大的影响，所以我们需要减小离群点对优化函数的影响。[4] 中使用正态分布和均值分布的混合模型来降低离群点的影响：

$\overline p(\vec{x}) = c_1 exp(-\frac{(\vec{x} - \vec{\mu})^T\Sigma^{-1}(\vec{x} - \vec{\mu})}{2}) + c_2 p_0 \tag{6.7}$p​(x)=c1​exp(−2(x−μ​)TΣ−1(x−μ​)​)+c2​p0​(6.7)

这些 PDF 与其对应的负对数的形状如图 6.5 所示

仔细观察 $-log(\overline p(x)) = -log(c_1 exp(-\frac{x^2}{2\sigma^2}) + c_2)$−log(p​(x))=−log(c1​exp(−2σ2x2​)+c2​) （$c_1, c_2$c1​,c2​ 通过使 $\vec{p}(x)$p​(x) 求和为 1 计算得到，不过还是没搞懂 $c_1$c1​ 是怎么求的）的形状，我们可以用一个形如 $\widetilde p(x) = d_1 exp(-\frac{d_2 x^2}{2\sigma^2}) + d_3$p​(x)=d1​exp(−2σ2d2​x2​)+d3​ 的高斯函数来近似。$d_i$di​ 通过使 $\widetilde p(x)$p​(x) 与 $-log(\overline{p}(x))$−log(p​(x)) 在 $x = 0, x = \sigma, x = \infty$x=0,x=σ,x=∞ 相等求解得到：

$\begin{cases} -log(c_1 + c_2) = d_1 + d_3\\ -log(c_1 e^{-\frac{1}{2}} + c_2) = d_1 e^{-\frac{d_2}{2}} + d_3\\ -log(c_2) = d_3 \end{cases}$⎩⎪⎨⎪⎧​−log(c1​+c2​)=d1​+d3​−log(c1​e−21​+c2​)=d1​e−2d2​​+d3​−log(c2​)=d3​​

$d_3 = -log(c_2),\\ d_1 = -log(c_1+c_2)-d_3,\\ d_2 = -2log(\frac{-log(c_1 e^{-\frac{1}{2}}+c_2)-d_3}{d_1}) \tag{6.8}$d3​=−log(c2​),d1​=−log(c1​+c2​)−d3​,d2​=−2log(d1​−log(c1​e−21​+c2​)−d3​​)(6.8)

最终的每个 source point 对 score function(cost function) 的 contribution 即为（省略了常数项 $d_3$d3​）:

$\widetilde{p}(\vec{x_k}) = -d_1 exp(-\frac{d_2}{2}(\vec{x_k} - \vec{\mu_k})^T\Sigma^{-1}(\vec{x_k} - \vec{\mu_k}) \tag{6.9}$p​(xk​​)=−d1​exp(−2d2​​(xk​​−μk​​)TΣ−1(xk​​−μk​​)(6.9)

使用这个高斯近似的好处是求导更加方便，同时不失优化时的通用特性（最小二乘法？不太知道论文里说的 same general properties 是什么）。因此给定一个 source point cloud $\chi = \{\vec{x_1},...,\vec{x_n}\}$χ={x1​​,...,xn​​}，一个位姿 transformation $\vec{p}$p​，最终的 score function(cost function) 为：

$s(\vec{p}) = -\sum\limits_{k=1}^m \widetilde{p}(T(\vec{p}, \vec{x_k})) \tag{6.10}$s(p​)=−k=1∑m​p​(T(p​,xk​​))(6.10)

在式 6.9 中，我们需要计算 $\Sigma^{-1}$Σ−1，在 3D 的场景下，如果 voxel 中的点小于等于 3，$\Sigma$Σ 肯定是奇异的，如果点恰好都共面或共线的话，$\Sigma$Σ 也会是奇异的。出于这些原因，3D-NDT 只对点数大于 5（min_points_per_voxel_ = 6）的 voxel 计算 PDF。同时为了预防数值问题（numerical problem），如果 $\Sigma$Σ 近似奇异，我们会对其进行一个轻微的 inflate。即：如果最大的特征值 $\lambda_3$λ3​ 比 $\lambda_1$λ1​ 或 $\lambda_2$λ2​ 大 100 倍（[1] 中是 1000 倍）的话，就使用 $\lambda_j' = \frac{\lambda_3}{100}$λj′​=100λ3​​ 代替小的特征值，并使用 $\Sigma' = \mathrm{V}\Lambda'\mathrm{V}$Σ′=VΛ′V 代替 $\Sigma$Σ，$\mathrm{V}$V 为 $\Sigma$Σ 的特征向量矩阵，

$\Lambda' = \left[\begin{matrix} \lambda_1' & 0 & 0\\ 0 & \lambda_2' & 0\\ 0 & 0 & \lambda_3 \end{matrix}\right] \tag{6.11}$Λ′=⎣⎡​λ1′​00​0λ2′​0​00λ3​​⎦⎤​(6.11)

接下来的内容就顺理成章了，使用 Newton 法来迭代求解 $\mathrm{H}\Delta\vec{p} = -\vec{g}$HΔp​=−g​ 进而最小化 $s(\vec{p})$s(p​)。

接下的公式主要就是推导海森矩阵和雅克比矩阵了，论文的步骤已经十分详细了（其实上面的公式推导也很详细，一路看下来十分清爽），这里就搬运一下。为了简洁起见，记 $\vec{x}_k' \equiv T(\vec{p}, \vec{x}_k) - \vec{\mu}_k$xk′​≡T(p​,xk​)−μ​k​，$\vec{x}_k'$xk′​ 的实际意义也比较直观，就是 $\vec{x}_k$xk​ 通过位姿 $\vec{p}$p​ 变换后相对于对应 voxel 的 PDF 中心的位置。所以

$g_i = \frac{\delta s}{\delta p_i} = \sum\limits_{k=1}{m} d_1 d_2 \vec{x}_k'^T \Sigma_k^{-1} \frac{\delta \vec{x}_k'}{\delta p_i} exp(\frac{-d_2}{2} \vec{x}_k'^T \Sigma_k^{-1} \vec{x}_k') \tag{6.12}$gi​=δpi​δs​=k=1∑​md1​d2​xk′T​Σk−1​δpi​δxk′​​exp(2−d2​​xk′T​Σk−1​xk′​)(6.12)

$\begin{aligned} H_{ij} = \frac{\delta^2 s}{\delta p_i \delta p_j} = \sum\limits_{k=1}^n d_1 d_2 exp(\frac{-d_2}{2} \vec{x}_k'^T \Sigma_k^{-1} \vec{x}_k')(-d_2(\vec{x}_k'^T\Sigma_k^{-1}\frac{\delta \vec{x}_k'}{\delta p_j}) + \\ \vec{x}_k'^T\Sigma_k^{-1}\frac{\delta^2 \vec{x}_k'}{\delta p_i \delta p_j} + \frac{\delta \vec{x}_k'^T}{\delta p_j}\Sigma_k^{-1}\frac{\delta \vec{x}_k'}{\delta p_i}) \tag{6.13} \end{aligned}$Hij​=δpi​δpj​δ2s​=k=1∑n​d1​d2​exp(2−d2​​xk′T​Σk−1​xk′​)(−d2​(xk′T​Σk−1​δpj​δxk′​​)+xk′T​Σk−1​δpi​δpj​δ2xk′​​+δpj​δxk′T​​Σk−1​δpi​δxk′​​)​(6.13)

2D 和 3D 场景的 $g, \mathrm{H}$g,H 的区别在于 $\vec{x}_k'$xk′​ 关于参数 $\vec{p}$p​ 的导数。6.2.1, 6.2.2 分别介绍了 2D 和 3D 场景下的导数计算，之后的 scan registration 算法如表 2。

6.2.2 3D-NDT

3D 跟 2D 并没有太大区别，这里只介绍 3D（实在不想打公式了，还是截图方便。。。）场景下 $T_E(\vec{p}_6, \vec{x})$TE​(p​6​,x) 关于 $\vec{p}_6 = [t_x, t_y, t_z, \phi_x, \phi_y, \phi_z]$p​6​=[tx​,ty​,tz​,ϕx​,ϕy​,ϕz​] 的导数。

其中 $c_i = cos(\phi_i), s_i = sin(\phi_i)$ci​=cos(ϕi​),si​=sin(ϕi​)。

实际计算时对于小角度会进行 trigonometric approximation 以减小计算量加速求解：

需要注意的是，这里是对导数计算中的 $cos, sin$cos,sin 进行近似，而不是在式 6.17 中就进行近似，原因在附录 B.1 中解释了，因为如果在 6.17 中就近似的话会导致导数中的大部分值为 0，与实际的导数结果相差较大。代码里直接将小于 10e-5 弧度的 sin 近似为 0，cos 近似为 1。

这一节也简单解释了为什么 NDT 里使用最直接的牛顿法（也就是直接计算 $\mathrm{H}$H）求解，因为 NDT 的 $\mathrm{H}$H 比较好计算（能计算出解析式），并且足够稀疏，直接的牛顿法求解更加鲁棒（文中有与伪牛顿法的实验对比）。最后就是通过 More-Thuente 的 line search 计算更新步长来更新位姿了。

简单的总结

voxel 的 leaf size 设置影响很大，至少需要包含 6 个点防止共面或共线，以计算 $\Sigma^{-1}$Σ−1，但又不能太大，否则高斯平滑过度（高斯分布的单峰特性导致的）反而丧失了局部特征，可是小了的话需要的内存更多，具体多大取决于输入数据（超参数嘛，调就完了），6.3 节介绍了一些构造 cell structure 的方法，包括：

1. fixed discretisation（voxel）
2. octree discretisation（octree）
3. iterative discretisation（一组 NDT，先 coarse，后 finer，有点像 Cartographer 里面的分层计算）
4. adaptive clustering（使用 k-means 聚类，对每个类使用不同的 cell size）
5. linked cells（对于没有落在 voxel 中的 source point(radiusSearch，ndt_cpu 的方案)，不将其丢弃，而是寻找距离其最近(nearstSearch，论文里的方案)的 voxel 的 PDF 进行计算）
6. trilinear interpolation（对于 cell 边缘不连续的问题，通过交错 cell 使其相互 overlap，最后加权 overlapped cell 的 PDF，理论上计算时间是不使用 trilinear interpolation 的 8 倍，实际使用时大部分在 4 倍左右，因为大部分点云还是分布在平面上的）。

后面的实验分析就不介绍了，之后有兴趣的可以仔细研究下论文里的参数和 trick 对结果的影响。

论文和代码里的一些 trick：

• 使用高斯和均值的混合分布降低离群点影响，使用高斯函数对混合分布的负对数进行近似，方便计算 $\mathrm{J,H}$J,H
• 进行三角近似减小计算量加速求解（弧度小于 10e-5）
• 只对点数大于 5 的 voxel 计算 PDF，防止奇异问题，为预防数值问题还会对原协方差 $\Sigma$Σ 进行一个 inflate
• 协方差的计算有点技巧（展开求解）

Reference

1. Biber P, Straßer W. The normal distributions transform: A new approach to laser scan matching[C]//Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453). IEEE, 2003, 3: 2743-2748.
2. Magnusson M, Lilienthal A, Duckett T. Scan registration for autonomous mining vehicles using 3D‐NDT[J]. Journal of Field Robotics, 2007, 24(10): 803-827.
3. Magnusson M. The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection[D]. Örebro universitet, 2009.
4. Biber P, Fleck S, Strasser W. A probabilistic framework for robust and accurate matching of point clouds[C]//Joint Pattern Recognition Symposium. Springer, Berlin, Heidelberg, 2004: 480-487.
5. Moré J J, Thuente D J. Line search algorithms with guaranteed sufficient decrease[J]. ACM Transactions on Mathematical Software (TOMS), 1994, 20(3): 286-307.
6. autowarefoundation/autoware
7. 无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位