首页 > 编程学习 > NDT方法总结与公式推导

NDT方法总结与公式推导

发布时间:2022/11/12 1:46:20

目录

    • 参考
    • 摘要
    • 方法

参考

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 = 1 n ∑ i x i q = \frac{1}{n}\sum_ix_i q=n1ixi

协方差矩阵 : Σ = 1 n ∑ i ( x i − q ) ( x i − q ) T \Sigma = \frac{1}{n}\sum_i(x_i-q)(x_i-q)^T Σ=n1i(xiq)(xiq)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(xq)TΣ1(xq))

若随机变量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倍,如果不是,则将其设为该值

  1. 变换需要配准的点云到参考坐标系(参考点云的坐标系通常是前一帧)

( 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} (xy)=(cosϕsinϕsinϕcosϕ)(xy)+(txty)

其中 t x , t y t_x,t_y tx,ty ϕ ϕ ϕ分别描述了两个坐标系之间的平移和旋转

  1. 根据正态分布参数计算每个转换点落在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(xq)TΣ1(xq))

  2. 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)=iexp(2(xiqi)TΣ1(xiqi))

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

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

q = x i ′ − q i q = x_i'-q_i q=xiqi,则有 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} pis=qspiq=qTΣ1exp(2qTΣ1q)piq

协方差矩阵 Σ \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}) =>qexp(2qTΣ1q)=(ΣTq+Σ1q)/2exp(2qTΣ1q)=Σ1qexp(2qTΣ1q)
由于 Σ − 1 q ⋅ e x p ( − q T Σ − 1 q 2 ) \Sigma^{-1}q \cdot exp(-\frac{q^T\Sigma^{-1}q}{2}) Σ1qexp(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} piq即为变换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} piq=JT=(1001xsinϕ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} piq=J=[tx,ty,ϕ][cosϕxsinϕy+tx,sinϕx+cosϕy+tx]=txcosϕxsinϕy+txtycosϕxsinϕy+txϕcosϕxsinϕy+txtxsinϕx+cosϕy+txtysinϕ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} =10xsinϕ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=(1001xsinϕycosϕxcosϕysinϕ)

根据上面梯度的计算结果, 继续求 s \mathrm{s} s 关于变量 p i 、 p j \mathrm{p}_{\mathrm{i}} 、 \mathrm{p}_{\mathrm{j}} pipj 的二阶偏导:
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=pipj2s=pj(pis)=pj(qTΣ1exp(2qTΣ1q)piq)=pj(qTΣ1piq)exp(2qTΣ1q)+(qTΣ1piq)pjexp(2qTΣ1q)=exp(2qTΣ1q)[(qTΣ1piq)(qTΣ1pjq)(pjqTΣ1piq)(qTΣ1pipj2q)]
其中,向量 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. pipj2q=(xcosϕ+ysinϕxsinϕycosϕ)(00)i=j=3 otherwise 

  1. 跳转到第3步继续执行,指导达到收敛条件为止
Copyright © 2010-2022 dgrt.cn 版权所有 |关于我们| 联系方式