3D激光里程计其二：NDT
3D激光里程计其二：NDT
- 1. 经典NDT
- 2. 计算方式
-
- 2.1 2D场景求解:
- 2.2 3D场景求解：
- 3. 其他 NDT
Reference:
- 深蓝学院-多传感器融合
1. 经典NDT
NDT 核心思想：基于概率的匹配。目标是将点集 Y 匹配到固定的点集 X 中。这里的联合概率说的是将 X 划分成栅格(如上图)，每个栅格里面都有很多点，这些点可以计算一个 均值/协方差，这是一个概率的概念。而联合概率是：Y 往 X 上旋转的时候，它落在栅格中的哪个格子里知道的。根据落在格子里的点，本身 X 格子已经有了一个 均值/协方差，而 Y 的这些落在格子里的点可以形成一个联合概率。这个联合概率会作为有没匹配好的一个指标。
点集：
X = { x 1 , x 2 , ⋯ , x N x } Y = { y 1 , y 2 , ⋯ , y N y } \begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned} X={x1,x2,⋯,xNx}Y={y1,y2,⋯,yNy}
目标: max Ψ = max ∏ i = 1 N y f ( X , T ( p , y i ) ) \max \Psi=\max \prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) maxΨ=max∏i=1Nyf(X,T(p,yi))(与ICP这里有些区别，ICP的是点到点的距离，这里变成了联合概率)
2D模型: p = p 3 = [ t x t y ϕ z ] T \quad p=p_3=\left[\begin{array}{lll}t_x & t_y & \phi_z\end{array}\right]^{\mathrm{T}} p=p3=[txtyϕz]T
3D模型: p = p 6 = [ t x t y t z ϕ x ϕ y ϕ z ] T \quad p=p_6=\left[\begin{array}{llllll}t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}\right]^{\mathrm{T}} p=p6=[txtytzϕxϕyϕz]T
2. 计算方式
均值和协方差：
μ = 1 N x ∑ i = 1 N x x i ， Σ = 1 N x − 1 ∑ i = 1 N x ( x i − μ ) ( x i − μ ) T \mu=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i， \boldsymbol{\Sigma}=\frac{1}{N_x-1} \sum_{i=1}^{N_x}\left(x_i-\mu\right)\left(x_i-\mu\right)^{\mathrm{T}} μ=Nx1i=1∑Nxxi，Σ=Nx−11i=1∑Nx(xi−μ)(xi−μ)T
根据预测的位姿，对点进行旋转和平移(这里的旋转和平移是一个初始预测值)：
y i ′ = T ( p , y i ) = R y i + t y_i^{\prime}=T\left(p, y_i\right)=R y_i+t yi′=T(p,yi)=Ryi+t
旋转和平移后的点与目标点集中的点在同一坐标系下，此时可计算各点的联合概率：
f ( X , y i ′ ) = 1 2 π ∣ Σ ∣ exp ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 ) f\left(X, y_i^{\prime}\right)=\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) f(X,yi′)=2π∣Σ∣1exp(−2(yi′−μ)TΣ−1(yi′−μ))
所有点的联合概率(就是所有点的概率乘一起)：
Ψ = ∏ i = 1 N y f ( X , T ( p , y i ) ) = ∏ i = 1 N y 1 2 π ∣ Σ ∣ exp ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 ) \begin{aligned} \Psi & =\prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) \\ & =\prod_{i=1}^{N_y} \frac{1}{\sqrt{2 \pi} \sqrt{|\mathbf{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) \end{aligned} Ψ=i=1∏Nyf(X,T(p,yi))=i=1∏Ny2π∣Σ∣1exp(−2(yi′−μ)TΣ−1(yi′−μ))
目标是让所有点的联合概率最大。但是上式中的 R 和 t 是在 y i ′ y'_i yi′ 内的。这里还有一个exp，这个指数项会让公式变得复杂，需要消掉指数项。这时去对数可以解决。
取对数，简化问题(目标是让所有点的联合概率最大)：
ln Ψ = ∑ i = 1 N y ( − ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) 2 + ln ( 1 2 π ∣ Σ ∣ ) ) \ln \Psi=\sum_{i=1}^{N_y}\left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}+\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)\right) lnΨ=i=1∑Ny(−2(yi′−μ)TΣ−1(yi′−μ)+ln(2π∣Σ∣1))
去除常数项 ln ( 1 2 π ∣ Σ ∣ ) \ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right) ln(2π∣Σ∣1)，得到：
max Ψ = max ln Ψ = min Ψ 1 = min ∑ i = 1 N y ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) \max \Psi=\max \ln \Psi=\min \Psi_1=\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right) maxΨ=maxlnΨ=minΨ1=mini=1∑Ny(yi′−μ)TΣ−1(yi′−μ)
因为之前是负数，我们上式只需要求最小值就行。
这时就变成了一个优化问题了：
目标函数： min ∑ i = 1 N y ( y i ′ − μ ) T Σ − 1 ( y i ′ − μ ) \min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right) min∑i=1Ny(yi′−μ)TΣ−1(yi′−μ) y i ′ = T ( p , y i ) = R y i + t y_i^{\prime}=T\left(p, y_i\right)=R y_i+t yi′=T(p,yi)=Ryi+t
待求参数： R , t R, t R,t
定义残差函数： f i ( p ) = y i ′ − μ f_i(p)=y_i^{\prime}-\mu fi(p)=yi′−μ
按照高斯牛顿法的流程，只需计算残差函数关于待求参数的雅可比，便可迭代优化。
J i = d f i ( p ) d p J_i=\frac{d f_i(p)}{d p} Ji=dpdfi(p)
2.1 2D场景求解:
p = [ t x t y ϕ z ] T y i ′ = T ( p , y i ) = [ cos ϕ z − sin ϕ z sin ϕ z cos ϕ z ] y i + [ t x t y ] \begin{aligned} p & =\left[\begin{array}{lll} t_x & t_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ \\ y_i^{\prime} & =T\left(p, y_i\right) \\ & =\left[\begin{array}{cc} \cos \phi_z & -\sin \phi_z \\ \sin \phi_z & \cos \phi_z \end{array}\right] y_i+\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \end{aligned} pyi′=[txtyϕz]T=T(p,yi)=[cosϕzsinϕz−sinϕzcosϕz]yi+[txty]
雅可比：
J i = d f i ( p ) d p = [ 1 0 − y i 1 sin ϕ z − y i 2 cos ϕ z 0 1 y i 1 cos ϕ z − y i 2 sin ϕ z ] J_i=\frac{d f_i(p)}{d p}=\left[\begin{array}{ccc}1 & 0 & -y_{i 1} \sin \phi_z-y_{i 2} \cos \phi_z \\ 0 & 1 & y_{i 1} \cos \phi_z-y_{i 2} \sin \phi_z\end{array}\right] Ji=dpdfi(p)=[1001−yi1sinϕz−yi2cosϕzyi1cosϕz−yi2sinϕz]
2.2 3D场景求解：
p = [ t x t y t z ϕ x ϕ y ϕ z ] T y i ′ = T ( p , y i ) = R x R y R z y i + t = [ c y c z − c y s z s y c x s z + s x s y c z c x c z − s x s y s z − s x c y s x s z − c x s y c z c x s y s z + s x c z c x c y ] y i + [ t x t y t z ] \begin{aligned} & p=\left[\begin{array}{llllll} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ & y_i^{\prime}=T\left(p, y_i\right)=R_x R_y R_z y_i+t=\left[\begin{array}{ccc} c_y c_z & -c_y s_z & s_y \\ c_x s_z+s_x s_y c_z & c_x c_z-s_x s_y s_z & -s_x c_y \\ s_x s_z-c_x s_y c_z & c_x s_y s_z+s_x c_z & c_x c_y \end{array}\right] y_i+\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \end{aligned} p=[txtytzϕxϕyϕz]Tyi′=T(p,yi)=RxRyRzyi+t= cyczcxsz+sxsyczsxsz−cxsycz−cyszcxcz−sxsyszcxsysz+sxczsy−sxcycxcy yi+ txtytz
雅可比：
雅可比 J i = [ 1 0 0 0 c f 0 1 0 a d g 0 0 1 b e h ] \begin{aligned} & \text { 雅可比 } J_i=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right] \end{aligned} 雅可比 Ji= 1000100010abcdefgh
其中：
a = y i 1 ( − s x s z + c x s y c z ) + y i 2 ( − s x c z − c x s y s z ) + y i 3 ( − c x c y ) b = y i 1 ( c x s z + s x s y c z ) + y i 2 ( − s x s y s z + c x c z ) + y i 3 ( − s x c y ) c = y i 1 ( − s y c z ) + y i 2 ( s y s z ) + y i 3 ( c y ) d = y i 1 ( s x c y c z ) + y i 2 ( − s x c y s z ) + y i 3 ( s x s y ) e = y i 1 ( − c x c y c z ) + y i 2 ( c x c y s z ) + y i 3 ( − c x s y ) f = y i 1 ( − c y s z ) + y i 2 ( − c y c z ) g = y i 1 ( c x c z − s x s y s z ) + y i 2 ( − c x s z − s x s y c z ) h = y i 1 ( s x c z + c x s y s z ) + y i 2 ( c x s y c z − s x s z ) \begin{aligned} & a=y_{i 1}\left(-s_x s_z+c_x s_y c_z\right)+y_{i 2}\left(-s_x c_z-c_x s_y s_z\right)+y_{i 3}\left(-c_x c_y\right) \\ & b=y_{i 1}\left(c_x s_z+s_x s_y c_z\right)+y_{i 2}\left(-s_x s_y s_z+c_x c_z\right)+y_{i 3}\left(-s_x c_y\right) \\ & c=y_{i 1}\left(-s_y c_z\right)+y_{i 2}\left(s_y s_z\right)+y_{i 3}\left(c_y\right) \\ & d=y_{i 1}\left(s_x c_y c_z\right)+y_{i 2}\left(-s_x c_y s_z\right)+y_{i 3}\left(s_x s_y\right) \\ & e=y_{i 1}\left(-c_x c_y c_z\right)+y_{i 2}\left(c_x c_y s_z\right)+y_{i 3}\left(-c_x s_y\right) \\ & f=y_{i 1}\left(-c_y s_z\right)+y_{i 2}\left(-c_y c_z\right) \\ & g=y_{i 1}\left(c_x c_z-s_x s_y s_z\right)+y_{i 2}\left(-c_x s_z-s_x s_y c_z\right) \\ & h=y_{i 1}\left(s_x c_z+c_x s_y s_z\right)+y_{i 2}\left(c_x s_y c_z-s_x s_z\right) \end{aligned} a=yi1(−sxsz+cxsycz)+yi2(−sxcz−cxsysz)+yi3(−cxcy)b=yi1(cxsz+sxsycz)+yi2(−sxsysz+cxcz)+yi3(−sxcy)c=yi1(−sycz)+yi2(sysz)+yi3(cy)d=yi1(sxcycz)+yi2(−sxcysz)+yi3(sxsy)e=yi1(−cxcycz)+yi2(cxcysz)+yi3(−cxsy)f=yi1(−cysz)+yi2(−cycz)g=yi1(cxcz−sxsysz)+yi2(−cxsz−sxsycz)h=yi1(sxcz+cxsysz)+yi2(cxsycz−sxsz)
上面是使用旋转矩阵求导，也可以使用李代数求解。
3. 其他 NDT
