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) 来描述点云的局部分布

(6.1)p(x⃗)=1(2π)D/2∣Σ∣exp(−(x⃗−μ⃗)TΣ−1(x⃗−μ⃗)2)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∣Σ∣1exp(−2(x−μ)TΣ−1(x−μ))(6.1)
μ⃗=1m∑k−1myk⃗,Σ=1m−1∑k−1m(yk⃗−μ⃗)(yk⃗−μ⃗)T\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 μ=m1k−1∑myk,Σ=m−11k−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 的目标很简单，最大化下面似然函数：

(6.5)Ψ=∏k−1np(T(p⃗,xk⃗))\Psi = \prod\limits_{k-1}^n p(T(\vec{p}, \vec{x_k})) \tag{6.5}Ψ=k−1∏np(T(p,xk))(6.5)

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

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

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

(6.7)p‾(x⃗)=c1exp(−(x⃗−μ⃗)TΣ−1(x⃗−μ⃗)2)+c2p0\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)=c1exp(−2(x−μ)TΣ−1(x−μ))+c2p0(6.7)

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

仔细观察 −log(p‾(x))=−log(c1exp(−x22σ2)+c2)-log(\overline p(x)) = -log(c_1 exp(-\frac{x^2}{2\sigma^2}) + c_2)−log(p(x))=−log(c1exp(−2σ2x2)+c2) （c1,c2c_1, c_2c1,c2 通过使 p⃗(x)\vec{p}(x)p(x) 求和为 1 计算得到，不过还是没搞懂 c1c_1c1 是怎么求的）的形状，我们可以用一个形如 p~(x)=d1exp(−d2x22σ2)+d3\widetilde p(x) = d_1 exp(-\frac{d_2 x^2}{2\sigma^2}) + d_3p(x)=d1exp(−2σ2d2x2)+d3 的高斯函数来近似。did_idi 通过使 p~(x)\widetilde p(x)p(x) 与 −log(p‾(x))-log(\overline{p}(x))−log(p(x)) 在 x=0,x=σ,x=∞x = 0, x = \sigma, x = \inftyx=0,x=σ,x=∞ 相等求解得到：

{−log(c1+c2)=d1+d3−log(c1e−12+c2)=d1e−d22+d3−log(c2)=d3\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(c1e−21+c2)=d1e−2d2+d3−log(c2)=d3

(6.8)d3=−log(c2),d1=−log(c1+c2)−d3,d2=−2log(−log(c1e−12+c2)−d3d1)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(c1e−21+c2)−d3)(6.8)

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

(6.9)p~(xk⃗)=−d1exp(−d22(xk⃗−μk⃗)TΣ−1(xk⃗−μk⃗)\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)=−d1exp(−2d2(xk−μk)TΣ−1(xk−μk)(6.9)

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

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

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

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

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

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

(6.12)gi=δsδpi=∑k=1md1d2x⃗k′TΣk−1δx⃗k′δpiexp(−d22x⃗k′TΣk−1x⃗k′)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∑md1d2xk′TΣk−1δpiδxk′exp(2−d2xk′TΣk−1xk′)(6.12)

(6.13)Hij=δ2sδpiδpj=∑k=1nd1d2exp(−d22x⃗k′TΣk−1x⃗k′)(−d2(x⃗k′TΣk−1δx⃗k′δpj)+x⃗k′TΣk−1δ2x⃗k′δpiδpj+δx⃗k′TδpjΣk−1δx⃗k′δpi)\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∑nd1d2exp(2−d2xk′TΣk−1xk′)(−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,Hg, \mathrm{H}g,H 的区别在于 x⃗k′\vec{x}_k'xk′ 关于参数 p⃗\vec{p}p 的导数。6.2.1, 6.2.2 分别介绍了 2D 和 3D 场景下的导数计算，之后的 scan registration 算法如表 2。

6.2.2 3D-NDT

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

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

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

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

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

简单的总结

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

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

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

论文和代码里的一些 trick：

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

Reference

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.
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.
Magnusson M. The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection[D]. Örebro universitet, 2009.
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.
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.
autowarefoundation/autoware
无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位