NDT方法总结与公式推导

参考

NDT论文下载：https://www.researchgate.net/publication/4045903_The_Normal_Distributions_Transform_A_New_Approach_to_Laser_Scan_Matching
NDT论文翻译:https://blog.csdn.net/sunqin_csdn/article/details/106183711
NDT算法七个步骤：https://blog.csdn.net/weixin_41469272/article/details/105622447?
代码实现： https://zhuanlan.zhihu.com/p/503522458
三维NDT：
https://blog.csdn.net/u013794793/article/details/89306901
https://blog.csdn.net/adamshan/article/details/79230612

摘要

大多数扫描匹配算法需要找到使用的，如点或线等特征之间的对应。我们为范围扫描提供了一个可选的表达，即正态分布变换。与占用栅格类似，我们将2D平面分成小的单元格。对于每一个单元格，我们分配一个正态分布，它局部地建模了测量一个点的概率。变换的结果是分段连续且可微的概率密度，我们可以使用牛顿算法将其和另一个扫描进行匹配。因此，不需要建立明确的对应关系。

方法

1. 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网络或体素（Voxel）
2. 计算网络的概率分布模型

计算cell中的所有点云的均值以及协方差矩阵：
均值 ： $q=\frac{1}{n}{\sum }_i{x}_i$

协方差矩阵 ： $\Sigma =\frac{1}{n}{\sum }_i(x_i-q)(x_i-q{)}^T$

则点云位于cell中的概率建模为正态分布$N(q,\Sigma )$： $$p(x)\sim exp(-\frac{(x-q{)}^T{\Sigma }^{-1}(x-q)}{2})$$

若随机变量X服从一个位置参数为$\mu$、尺度参数为$\sigma$的正态分布$N(\mu ,{\sigma }^2)$ ，则其概率密度函数为$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }exp(-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$

实现细节1：为了较少离散化的影响，使用四个的栅格（将一个栅格分成四个栅格）That is, one grid with side length l of a single cell is placed first, then a second one, shifted by l/2 horizontally, a third one, shifted by l/2 vertically and finally a fourth one, shifted by l/2 horizontally and vertically.保证每个点云落在四个栅格的区域，计算四个概率密度的和。

实现细节2：为了防止协方差矩阵奇异且不可逆，我们检查$\Sigma$的较小特征值是否至少是较大特征值的0.001倍，如果不是，则将其设为该值

3. 变换需要配准的点云到参考坐标系（参考点云的坐标系通常是前一帧）

( x ′ y ′ ) = ( c o s ϕ − s i n ϕ s i n ϕ c o s ϕ ) ( x y ) + ( t x t y )

= + (x′y′​)=(cosϕsinϕ​​−sinϕcosϕ​)(xy​)+(tx​ty​​)

其中 $t_x,t_y$和$\varphi$分别描述了两个坐标系之间的平移和旋转

4. 根据正态分布参数计算每个转换点落在cell中的概率
   $$p(x)\sim exp(-\frac{(x-q{)}^T{\Sigma }^{-1}(x-q)}{2})$$

5. NDT配准得分（score）： 计算对应点落在对应网格cell中的概率之和：
   $$score(p)=\sum _{i}exp(-\frac{(x_i^{\prime }-q_i{)}^T{\Sigma }^{-1}(x_i^{\prime }-q_i)}{2})$$

6. 根据牛顿优化算法对目标函数score进行优化，即寻找变换参数$p$,使得score的值最大。优化的关键步骤是计算目标函数的梯度和Hessian矩阵：

由于一般的优化问题都是优化最小值，所以我们对score函数加上一个负号，这样优化-score的最小值就对应了score的最大值

令$q=x_i^{\prime }-q_i$,则有$s=-exp(-\frac{q^T{\Sigma }^{-1}q}{2})$

根据链式求导法则以及向量、矩阵求导的公式，s的梯度方向为：
$$\frac{\partial s}{\partial p_i}=\frac{\partial s}{\partial q}\frac{\partial q}{\partial p_i}=q^T{\Sigma }^{-1}exp(-\frac{q^T{\Sigma }^{-1}q}{2})\frac{\partial q}{\partial p_i}$$

协方差矩阵 $\Sigma$是对称矩阵，对称矩阵的形式为${\Sigma }^T=\Sigma$也即${\Sigma }^{-T}={\Sigma }^{-1}$
$$\frac{\partial (X_{1\times n}^T{A}_{n\times n}{X}_{n\times 1})}{\partial x}=AX+A^{T}X$$ $$=>\frac{-exp(-\frac{q^T{\Sigma }^{-1}q}{2})}{\partial q}=({\Sigma }^{-T}q+{\Sigma }^{-1}q)/2\ast exp(-\frac{q^T{\Sigma }^{-1}q}{2})={\Sigma }^{-1}q\cdot exp(-\frac{q^T{\Sigma }^{-1}q}{2})$$
由于${\Sigma }^{-1}q\cdot exp(-\frac{q^T{\Sigma }^{-1}q}{2})$是个标量，所以也可以等于$q^T{\Sigma }^{-1}exp(-\frac{q^T{\Sigma }^{-1}q}{2})$

{标量${}^T=$标量}，所以$f(x)=A^{T}X=X^{T}A$,

其中$q$对变换参数$p_i$的偏导数$\frac{\partial q}{\partial p_i}$即为变换T的雅克比矩阵:
∂ q ∂ p i = J T = ( 1 0 − x s i n ϕ − y c o s ϕ 0 1 x c o s ϕ − y s i n ϕ ) \frac{\partial q}{\partial p_i}=J_T=

∂pi​∂q​=JT​=(10​​01​​−xsinϕ−ycosϕxcosϕ−ysinϕ​)

∂ q ∂ p i = J = ∂ [ c o s ϕ x − s i n ϕ y + t x , s i n ϕ x + c o s ϕ y + t x ] ∂ [ t x , t y , ϕ ] = ( ∂ c o s ϕ x − s i n ϕ y + t x ∂ t x ∂ s i n ϕ x + c o s ϕ y + t x ∂ t x ∂ c o s ϕ x − s i n ϕ y + t x ∂ t y ∂ s i n ϕ x + c o s ϕ y + t x ∂ t y ∂ c o s ϕ x − s i n ϕ y + t x ∂ ϕ ∂ s i n ϕ x + c o s ϕ y + t x ∂ ϕ ) \frac{\partial q}{\partial p_i}=J = \frac{\partial [cos\phi x-sin\phi y + t_x, sin\phi x + cos\phi y +t_x]}{\partial [t_x,t_y,\phi ]}=

(∂cosϕx−sinϕy+tx∂tx∂sinϕx+cosϕy+tx∂tx∂cosϕx−sinϕy+tx∂ty∂sinϕx+cosϕy+tx∂ty∂cosϕx−sinϕy+tx∂ϕ∂sinϕx+cosϕy+tx∂ϕ)" role="presentation" style="position: relative;">⎛⎝⎜⎜⎜⎜∂cosϕx−sinϕy+tx∂tx∂cosϕx−sinϕy+tx∂ty∂cosϕx−sinϕy+tx∂ϕ∂sinϕx+cosϕy+tx∂tx∂sinϕx+cosϕy+tx∂ty∂sinϕx+cosϕy+tx∂ϕ⎞⎠⎟⎟⎟⎟$$\left(\begin{array}{cc}\frac{\mathrm{\partial }cos\varphi x-sin\varphi y+t_x}{\mathrm{\partial }{t}_x}& \frac{\mathrm{\partial }sin\varphi x+cos\varphi y+t_x}{\mathrm{\partial }{t}_x}\\ \frac{\mathrm{\partial }cos\varphi x-sin\varphi y+t_x}{\mathrm{\partial }{t}_y}& \frac{\mathrm{\partial }sin\varphi x+cos\varphi y+t_x}{\mathrm{\partial }{t}_y}\\ \frac{\mathrm{\partial }cos\varphi x-sin\varphi y+t_x}{\mathrm{\partial }\varphi }& \frac{\mathrm{\partial }sin\varphi x+cos\varphi y+t_x}{\mathrm{\partial }\varphi }\end{array}\right)$$

∂pi​∂q​=J=∂[tx​,ty​,ϕ]∂[cosϕx−sinϕy+tx​,sinϕx+cosϕy+tx​]​=⎝⎜⎛​∂tx​∂cosϕx−sinϕy+tx​​∂ty​∂cosϕx−sinϕy+tx​​∂ϕ∂cosϕx−sinϕy+tx​​​∂tx​∂sinϕx+cosϕy+tx​​∂ty​∂sinϕx+cosϕy+tx​​∂ϕ∂sinϕx+cosϕy+tx​​​⎠⎟⎞​
= ( 1 0 0 1 − x s i n ϕ − y c o s ϕ x c o s ϕ − y s i n ϕ ) =

(1001−xsinϕ−ycosϕxcosϕ−ysinϕ)" role="presentation" style="position: relative;">⎛⎝⎜10−xsinϕ−ycosϕ01xcosϕ−ysinϕ⎞⎠⎟$$\left(\begin{array}{cc}1& 0\\ 0& 1\\ -xsin\varphi -ycos\varphi & xcos\varphi -ysin\varphi \end{array}\right)$$

=⎝⎛​10−xsinϕ−ycosϕ​01xcosϕ−ysinϕ​⎠⎞​
则 有 J T = ( 1 0 − x s i n ϕ − y c o s ϕ 0 1 x c o s ϕ − y s i n ϕ ) 则有J^T=

(10−xsinϕ−ycosϕ01xcosϕ−ysinϕ)" role="presentation" style="position: relative;">(1001−xsinϕ−ycosϕxcosϕ−ysinϕ)$$\left(\begin{array}{ccccc}1& & 0& & -xsin\varphi -ycos\varphi \\ 0& & 1& & xcos\varphi -ysin\varphi \end{array}\right)$$

则有JT=(10​​01​​−xsinϕ−ycosϕxcosϕ−ysinϕ​)

根据上面梯度的计算结果, 继续求 $\mathrm{s}$ 关于变量 ${\mathrm{p}}_{\mathrm{i}}、{\mathrm{p}}_{\mathrm{j}}$ 的二阶偏导:
H i j = ∂ 2 s ∂ p i ∂ p j = ∂ ( ∂ s ∂ p i ) ∂ p j = ∂ ( q T Σ − 1 exp ⁡ ( − q T Σ − 1 q 2 ) ∂ q ∂ p i ) ∂ p j = ∂ ( q T Σ − 1 ∂ q ∂ p i ) ∂ p j exp ⁡ ( − q T Σ − 1 q 2 ) + ( q T Σ − 1 ∂ q ∂ p i ) ∂ exp ⁡ ( − q T Σ − 1 q 2 ) ∂ p j = − exp ⁡ ( − q T Σ − 1 q 2 ) [ ( q T Σ − 1 ∂ q ∂ p i ) ( q T Σ − 1 ∂ q ∂ p j ) − ( ∂ q T ∂ p j Σ − 1 ∂ q ∂ p i ) − ( q T Σ − 1 ∂ 2 q ∂ p i ∂ p j ) ]

Hij​​=∂pi​∂pj​∂2s​=∂pj​∂(∂pi​∂s​)​=∂pj​∂(qTΣ−1exp(2−qTΣ−1q​)∂pi​∂q​)​=∂pj​∂(qTΣ−1∂pi​∂q​)​exp(2−qTΣ−1q​)+(qTΣ−1∂pi​∂q​)∂pj​∂exp(2−qTΣ−1q​)​=−exp(2−qTΣ−1q​)[(qTΣ−1∂pi​∂q​)(qTΣ−1∂pj​∂q​)−(∂pj​∂qT​Σ−1∂pi​∂q​)−(qTΣ−1∂pi​∂pj​∂2q​)]​
其中，向量 $\mathrm{q}$ 对变换参数 $\mathrm{p}$ 的二阶导数的向量为:
∂ 2 q ∂ p i ∂ p j = { ( − x cos ⁡ ϕ + y sin ⁡ ϕ − x sin ⁡ ϕ − y cos ⁡ ϕ ) i = j = 3 ( 0 0 )  otherwise  \frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j}=\left\{

\begin{array}{cc} \left(\begin{array}{c} -x \cos \phi+y \sin \phi \\ -x \sin \phi-y \cos \phi \end{array}" role="presentation" style="position: relative;">\begin{array}{cc} \left(\begin{array}{c} -x \cos \phi+y \sin \phi \\ -x \sin \phi-y \cos \phi \end{array}$$\text{begin{array}{cc} left(begin{array}{c} -x cos phi+y sin phi -x sin phi-y cos phi end{array}}$$

\right) & i=j=3 \\ \left(

00" role="presentation" style="position: relative;">00$$\begin{array}{l}0\\ 0\end{array}$$

\right) & \text { otherwise } \end{array}\right. ∂pi​∂pj​∂2q​=⎩⎪⎪⎨⎪⎪⎧​(−xcosϕ+ysinϕ−xsinϕ−ycosϕ​)(00​)​i=j=3 otherwise ​

7. 跳转到第3步继续执行，指导达到收敛条件为止