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平面分成小的单元格。对于每一个单元格，我们分配一个正态分布，它局部地建模了测量一个点的概率。变换的结果是分段连续且可微的概率密度，我们可以使用牛顿算法将其和另一个扫描进行匹配。因此，不需要建立明确的对应关系。
方法
- 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网络或体素（Voxel）
- 计算网络的概率分布模型
计算cell中的所有点云的均值以及协方差矩阵：
均值 ： q = 1 n ∑ i x i q = \frac{1}{n}\sum_ix_i q=n1∑ixi
协方差矩阵 ： Σ = 1 n ∑ i ( x i − q ) ( x i − q ) T \Sigma = \frac{1}{n}\sum_i(x_i-q)(x_i-q)^T Σ=n1∑i(xi−q)(xi−q)T
则点云位于cell中的概率建模为正态分布 N ( q , Σ ) N(q,\Sigma) N(q,Σ)： p ( x ) ∼ e x p ( − ( x − q ) T Σ − 1 ( x − q ) 2 ) p(x) \sim exp(-\frac{(x-q)^T\Sigma^{-1}(x-q)}{2}) p(x)∼exp(−2(x−q)TΣ−1(x−q))
若随机变量X服从一个位置参数为 μ \mu μ、尺度参数为 σ \sigma σ的正态分布 N ( μ , σ 2 ) N(\mu,\sigma^2) N(μ,σ2) ，则其概率密度函数为 f ( x ) = 1 2 π σ e x p ( − ( x − μ ) 2 2 σ 2 ) f(x)=\frac{1}{\sqrt{2\pi}\sigma}exp(-\frac{(x-\mu)^2}{2\sigma^2}) f(x)=2πσ1exp(−2σ2(x−μ)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倍，如果不是，则将其设为该值
- 变换需要配准的点云到参考坐标系（参考点云的坐标系通常是前一帧）
( x ′ y ′ ) = ( c o s ϕ − s i n ϕ s i n ϕ c o s ϕ ) ( x y ) + ( t x t y ) \begin{pmatrix}x'\\y'\end{pmatrix} = \begin{pmatrix}cos\phi &&-sin\phi\\sin\phi&&cos\phi\end{pmatrix}\begin{pmatrix}x\\y\end{pmatrix}+\begin{pmatrix}t_x\\t_y\end{pmatrix} (x′y′)=(cosϕsinϕ−sinϕcosϕ)(xy)+(txty)
其中 t x , t y t_x,t_y tx,ty和 ϕ ϕ ϕ分别描述了两个坐标系之间的平移和旋转
-
根据正态分布参数计算每个转换点落在cell中的概率
p ( x ) ∼ e x p ( − ( x − q ) T Σ − 1 ( x − q ) 2 ) p(x) \sim exp(-\frac{(x-q)^T\Sigma^{-1}(x-q)}{2}) p(x)∼exp(−2(x−q)TΣ−1(x−q)) -
NDT配准得分（score）： 计算对应点落在对应网格cell中的概率之和：
s c o r e ( p ) = ∑ i e x p ( − ( x i ′ − q i ) T Σ − 1 ( x i ′ − q i ) 2 ) score(p)= \sum_i exp(-\frac{(x_i'-q_i)^T\Sigma^{-1}(x_i'-q_i)}{2}) score(p)=i∑exp(−2(xi′−qi)TΣ−1(xi′−qi)) -
根据牛顿优化算法对目标函数score进行优化，即寻找变换参数 p p p,使得score的值最大。优化的关键步骤是计算目标函数的梯度和Hessian矩阵：
由于一般的优化问题都是优化最小值，所以我们对score函数加上一个负号，这样优化-score的最小值就对应了score的最大值
令 q = x i ′ − q i q = x_i'-q_i q=xi′−qi,则有 s = − e x p ( − q T Σ − 1 q 2 ) s= -exp(-\frac{q^T\Sigma^{-1}q}{2}) s=−exp(−2qTΣ−1q)
根据链式求导法则以及向量、矩阵求导的公式，s的梯度方向为：
∂ s ∂ p i = ∂ s ∂ q ∂ q ∂ p i = q T Σ − 1 e x p ( − q T Σ − 1 q 2 ) ∂ q ∂ p i \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} ∂pi∂s=∂q∂s∂pi∂q=qTΣ−1exp(−2qTΣ−1q)∂pi∂q
协方差矩阵 Σ \Sigma Σ是对称矩阵，对称矩阵的形式为 Σ T = Σ \Sigma^T=\Sigma ΣT=Σ也即 Σ − T = Σ − 1 \Sigma^{-T}=\Sigma^{-1} Σ−T=Σ−1
∂ ( X 1 × n T A n × n X n × 1 ) ∂ x = A X + A T X \frac{\partial (X^T_{1\times n }A_{n\times n}X_{n\times 1})}{\partial x}=AX+A^TX ∂x∂(X1×nTAn×nXn×1)=AX+ATX = > − e x p ( − q T Σ − 1 q 2 ) ∂ q = ( Σ − T q + Σ − 1 q ) / 2 ∗ e x p ( − q T Σ − 1 q 2 ) = Σ − 1 q ⋅ e x p ( − q T Σ − 1 q 2 ) =>\frac{-exp(-\frac{q^T\Sigma^{-1}q}{2})}{\partial q}=(\Sigma^{-T}q +\Sigma^{-1}q)/2 *exp(-\frac{q^T\Sigma^{-1}q}{2})= \Sigma^{-1}q \cdot exp(-\frac{q^T\Sigma^{-1}q}{2}) =>∂q−exp(−2qTΣ−1q)=(Σ−Tq+Σ−1q)/2∗exp(−2qTΣ−1q)=Σ−1q⋅exp(−2qTΣ−1q)
由于 Σ − 1 q ⋅ e x p ( − q T Σ − 1 q 2 ) \Sigma^{-1}q \cdot exp(-\frac{q^T\Sigma^{-1}q}{2}) Σ−1q⋅exp(−2qTΣ−1q)是个标量，所以也可以等于 q T Σ − 1 e x p ( − q T Σ − 1 q 2 ) q^T\Sigma^{-1}exp(-\frac{q^T\Sigma^{-1}q}{2}) qTΣ−1exp(−2qTΣ−1q)
{标量 T = ^T= T=标量}，所以 f ( x ) = A T X = X T A f(x)=A^TX=X^TA f(x)=ATX=XTA,
其中 q q q对变换参数 p i p_i pi的偏导数 ∂ q ∂ p i \frac{\partial q}{\partial p_i} ∂pi∂q即为变换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=\begin{pmatrix}1 && 0 && -xsin\phi -ycos\phi \\ 0 && 1 && xcos\phi - ysin\phi\end{pmatrix} ∂pi∂q=JT=(1001−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 ]}=\begin{pmatrix}\frac{\partial cos\phi x-sin\phi y + t_x}{\partial t_x} &\frac{\partial sin\phi x + cos\phi y +t_x}{\partial t_x} \\ \frac{\partial cos\phi x-sin\phi y + t_x}{\partial t_y} &\frac{\partial sin\phi x + cos\phi y +t_x}{\partial t_y} \\\frac{\partial cos\phi x-sin\phi y + t_x}{\partial \phi} &\frac{\partial sin\phi x + cos\phi y +t_x}{\partial \phi} \end{pmatrix} ∂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 ϕ ) = \begin{pmatrix}1 & 0 \\ 0 & 1\\ -xsin\phi -ycos\phi & xcos\phi - ysin\phi\end{pmatrix} =⎝⎛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=\begin{pmatrix}1 && 0 && -xsin\phi -ycos\phi \\ 0 && 1 && xcos\phi - ysin\phi\end{pmatrix} 则有JT=(1001−xsinϕ−ycosϕxcosϕ−ysinϕ)
根据上面梯度的计算结果, 继续求 s \mathrm{s} s 关于变量 p i 、 p j \mathrm{p}_{\mathrm{i}} 、 \mathrm{p}_{\mathrm{j}} pi、pj 的二阶偏导:
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 ) ] \begin{aligned} H_{i j} &=\frac{\partial^2 s}{\partial p_i \partial p_j}=\frac{\partial\left(\frac{\partial s}{\partial p_i}\right)}{ \partial p_j}=\frac{\partial\left(\mathbf{q}^T \Sigma^{-1} \exp \left(\frac{-\mathbf{q}^T \Sigma^{-1} \mathbf{q}}{2}\right)\frac{\partial \mathbf{q}}{\partial p_i}\right) }{ \partial p_j} \\ &=\frac{\partial\left(\mathbf{q}^T \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_i}\right)} {\partial p_j} \exp \left(\frac{-\mathbf{q}^T \Sigma^{-1} \mathbf{q}}{2}\right)+\left(\mathbf{q}^T \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_i}\right) \frac{\partial \exp \left(\frac{-\mathbf{q}^T \Sigma^{-1} \mathbf{q}}{2}\right)} {\partial p_j}\\ &=-\exp \left(\frac{-\mathbf{q}^T \Sigma^{-1} \mathbf{q}}{2}\right)\left[\left(\mathbf{q}^T \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_i}\right)\left(\mathbf{q}^T \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_j}\right)-\left(\frac{\partial \mathbf{q}^T}{\partial p_j} \Sigma^{-1} \frac{\partial \mathbf{q}}{\partial p_i}\right)-\left(\mathbf{q}^T \Sigma^{-1} \frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j}\right)\right] \end{aligned} 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)]
其中，向量 q \mathrm{q} q 对变换参数 p \mathrm{p} 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}\right) & i=j=3 \\ \left(\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
- 跳转到第3步继续执行，指导达到收敛条件为止