前言
相关中文博客1：CSDN
强烈推荐相关中文博客2：CSDN
3D变换雅可比海森矩阵计算：CSDN
一、基本原理
1.原理介绍
在使用NDT（Normal Distributions Transform）进行扫描配准时，目标是找到当前扫描的姿态，使得当前扫描的点云落在参考扫描表面上的可能性最大。姿态的旋转和平移可以用向量 p \mathbf{p} p 表示：
p = [ t x t y t z ϕ θ ψ ] \mathbf{p} = \begin{bmatrix} t_x \\ t_y \\ t_z \\ \phi \\ \theta \\ \psi \end{bmatrix} p=
txtytzϕθψ
给定一组点云 X = { x 1 , … , x n } X = \{\mathbf{x}_1, \dots, \mathbf{x}_n\} X={
x1,…,xn}，并假设有一个空间变换函数 T ( p , x ) T(\mathbf{p}, \mathbf{x}) T(p,x) 可以根据姿态 p \mathbf{p} p 移动点 x \mathbf{x} x。对于正态分布部分，其概率密度函数的形式为：
p ( x ) = 1 ( 2 π ) d / 2 ∣ Σ ∣ 1 / 2 exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) p(\mathbf{x}) = \frac{1}{(2\pi)^{d/2} |\Sigma|^{1/2}} \exp\left( -\frac{1}{2} (\mathbf{x} - \mathbf{\mu})^T \Sigma^{-1} (\mathbf{x} - \mathbf{\mu}) \right) p(x)=(2π)d/2∣Σ∣1/21exp(−21(x−μ)TΣ−1(x−μ))
- d d d 是数据的维度（例如，对于三维点云， d = 3 d = 3 d=3）
- ∣ Σ ∣ |\Sigma| ∣Σ∣ 是协方差矩阵的行列式
在NDT中，通常使用一个混合模型来描述点的概率分布。该模型结合了正态分布和均匀分布，以提高对异常值的鲁棒性。
具体形式如下：
p ˉ ( x ) = c 1 exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) + c 2 p 0 \bar{p}(\mathbf{x}) = c_1 \exp\left( -\frac{1}{2} (\mathbf{x} - \mathbf{\mu})^T \Sigma^{-1} (\mathbf{x} - \mathbf{\mu}) \right) + c_2 p_0 pˉ(x)=c1exp(−21(x−μ)TΣ−1(x−μ))+c2p0
其中：
- p ˉ ( x ) \bar{p}(\mathbf{x}) pˉ(x) 是点 x \mathbf{x} x 的混合概率密度函数。
- c 1 c_1 c1 和 c 2 c_2 c2 是归一化常数，确保整个概率分布的总和为1。
- μ \mathbf{\mu} μ 是NDT单元内的均值。
- Σ \Sigma Σ 是NDT单元内的协方差矩阵。
- p 0 p_0 p0 是均匀分布的概率密度，用于表示异常值的期望比例。
在NDT中，优化目标是最小化负对数似然函数，它是基于概率密度函数的。给定点云 X = { x 1 , … , x n } X = \{\mathbf{x}_1, \ldots, \mathbf{x}_n\} X={ x1,…,xn}，NDT的负对数似然函数可以写为：
− log Ψ = − ∑ k = 1 n log ( p ( T ( p , x k ) ) ) -\log \Psi = -\sum_{k=1}^{n} \log \left( p(T(\mathbf{p}, \mathbf{x}_k)) \right) −logΨ=−k=1∑nlog(p(T(p,xk)))
这里的 p ( T ( p , x k ) p(T(\mathbf{p}, \mathbf{x}_k) p(T(p,xk) 是在变换后的点 T ( p , x k ) T(\mathbf{p}, \mathbf{x}_k) T(p,xk) 下的概率密度函数值。通过最小化这个负对数似然函数，我们可以优化姿态参数，使得当前扫描点云与参考扫描的匹配度最大化。对于优化目的，可以将上述混合模型近似为高斯分布：
p ~ ( x ) = − d 1 exp ( − d 2 2 ( x − μ ) T Σ − 1 ( x − μ ) ) \tilde{p}(\mathbf{x}) = -d_1 \exp \left( -\frac{d_2}{2} (\mathbf{x} - \mathbf{\mu})^T \Sigma^{-1} (\mathbf{x} - \mathbf{\mu}) \right) p~(x)=−d1exp(−2d2(x−μ)TΣ−1(x−μ))
其中， d 1 d_1 d1, d 2 d_2 d2 和 d 3 d_3 d3 是通过拟合得到的参数。对于NDT配准， d 3 d_3 d3 常数被省略，因为它不影响优化过程中的结果。
给定一组点云 X = { x 1 , … , x n } X = \{\mathbf{x}_1, \dots, \mathbf{x}_n\} X={
x1,…,xn}，姿态 p \mathbf{p} p，以及变换函数 T ( p , x ) T(\mathbf{p}, \mathbf{x}) T(p,x)，NDT得分函数为：
s ( p ) = − ∑ k = 1 n p ~ ( T ( p , x k ) ) s(\mathbf{p}) = -\sum_{k=1}^{n} \tilde{p}(T(\mathbf{p}, \mathbf{x}_k)) s(p)=−k=1∑np~(T(p,xk))
这个函数对应的是点 x k \mathbf{x}_k xk 在经过变换后的姿态 p \mathbf{p} p 下，落在参考扫描表面上的可能性。
2.优化流程
(1) 初始化阶段
- 初始化时，分配网格结构 B \ B B。
- 对参考点云 Y \ Y Y 中的每个点 y k ∈ Y \ \mathbf{y}_k \in Y yk∈Y，进行如下操作：
- 找到包含点 y k \ \mathbf{y}_k yk 的网格单元 b i ∈ B \ b_i \in B bi∈B。
- 将点 y k \ \mathbf{y}_k yk 存储到网格单元 b i \ b_i bi 中。
- 对所有的网格单元 b i ∈ B \ b_i \in B bi∈B，进行如下操作：
- 从网格单元 b i \ b_i bi 中提取所有的点 Y ′ = { y 1 ′ , … , y m ′ } \ Y' = \{\mathbf{y}_1', \dots, \mathbf{y}_m'\} Y′={ y1′,…,ym′}。
- 计算点的均值 μ i \mathbf{\mu}_i μi，公式如下：
μ i = 1 m ∑ k = 1 m y k ′ \mathbf{\mu}_i = \frac{1}{m} \sum_{k=1}^{m} \mathbf{y}_k' μi=m1k=1∑myk′ - 计算协方差矩阵 Σ i \Sigma_i Σi，公式如下：
Σ i = 1 m − 1 ∑ k = 1 m ( y k ′ − μ i ) ( y k ′ − μ i ) T \Sigma_i = \frac{1}{m-1} \sum_{k=1}^{m} (\mathbf{y}_k' - \mathbf{\mu}_i)(\mathbf{y}_k' - \mathbf{\mu}_i)^T Σi=m−11k=1∑m(yk′−μi)(yk′−μi)T
(2) 配准阶段
- 初始化目标函数得分 score = 0 \text{score} = 0 score=0，梯度 g = 0 \mathbf{g} = 0 g=0，以及海森矩阵 H = 0 \ H = 0 H=0。
- 对扫描点云 X \ X X 中的每个点 x k ∈ X \ \mathbf{x}_k \in X xk∈X，进行如下操作：
- 找到包含变换后点 T ( p , x k ) \ T(\mathbf{p}, \mathbf{x}_k) T(p,xk) 的网格单元 b i \ b_i bi。
- 更新得分 score \ \text{score} score，公式如下：
score = score + p ~ ( T ( p , x k ) ) \text{score} = \text{score} + \tilde{p}(T(\mathbf{p}, \mathbf{x}_k)) score=score+p~(T(p,xk))
其中， p ~ ( T ( p , x k ) ) \ \tilde{p}(T(\mathbf{p}, \mathbf{x}_k)) p~(T(p,xk)) 是根据方程计算的概率值:
p ~ ( x k ) = − d 1 exp ( − d 2 2 ( x k − μ k ) T Σ k − 1 ( x k − μ k ) ) p̃(\mathbf{x}_k) = -d_1 \exp\left( -\frac{d_2}{2} (\mathbf{x}_k - \mathbf{\mu}_k)^T \Sigma_k^{-1} (\mathbf{x}_k - \mathbf{\mu}_k) \right) p~(xk)=−d1exp(−2d2(xk−μk)TΣk−1(xk−μk)) - 更新梯度 g \mathbf{g} g，参考方程:
g i = ∂ s ∂ p i = ∑ k = 1 n d 1 d 2 x k ′ T Σ k − 1 ∂ x k ′ ∂ p i exp ( − d 2 2 x k ′ T Σ k − 1 x k ′ ) g_i = \frac{\partial s}{\partial p_i} = \sum_{k=1}^{n} d_1 d_2 \mathbf{x}_k'^T \Sigma_k^{-1} \frac{\partial \mathbf{x}_k'}{\partial p_i} \exp\left( -\frac{d_2}{2} \mathbf{x}_k'^T \Sigma_k^{-1} \mathbf{x}_k' \right) gi=∂pi∂s=k=1∑nd1d2xk′TΣk−1∂pi∂xk′exp(−2d2xk′TΣk−1xk′)
其中， x k ′ = T ( p , x k ) − μ k \mathbf{x}_k' = T(\mathbf{p}, \mathbf{x}_k) - \mathbf{\mu}_k xk′=T(p,xk)−μk。
∂ x k ′ ∂ p i = [ 1 0 0 0 c f 0 1 0 a d g 0 0 1 b e h ] \frac{\partial \mathbf{x}_k'}{\partial p_i}= \begin{bmatrix} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{bmatrix} ∂pi∂xk′= 1000100010abcdefgh
其中：
- a = x 1 ( − s x s z + c x s y c z ) + x 2 ( − s x c z − c x s y s z ) + x 3 ( − c x c y ) a = x_1(-s_x s_z + c_x s_y c_z) + x_2(-s_x c_z - c_x s_y s_z) + x_3(-c_x c_y) a=x1(−sxsz+cxsycz)+x2(−sxcz−cxsysz)+x3(−cxcy)
- b = x 1 ( c x s z + s x s y c z ) + x 2 ( − s x s y s z + c x c z ) + x 3 ( − s x c y ) b = x_1(c_x s_z + s_x s_y c_z) + x_2(-s_x s_y s_z + c_x c_z) + x_3(-s_x c_y) b=x1(cxsz+sxsycz)+x2(−sxsysz+cxcz)+x3(−sxcy)
- c = x 1 ( − s y c z ) + x 2 ( s y s z ) + x 3 ( c y ) c = x_1(-s_y c_z) + x_2(s_y s_z) + x_3(c_y) c=x1(−sycz)+x2(sysz)+x3(cy)
- d = x 1 ( s x c y c z ) + x 2 ( − s x c y s z ) + x 3 ( s x s y ) d = x_1(s_x c_y c_z) + x_2(-s_x c_y s_z) + x_3(s_x s_y) d=x1(sxcycz)+x2(−sxcysz)+x3(sxsy)
- e = x 1 ( − c x c y c z ) + x 2 ( c x c y s z ) + x 3 ( − c x s y ) e = x_1(-c_x c_y c_z) + x_2(c_x c_y s_z) + x_3(-c_x s_y) e=x1(−cxcycz)+x2(cxcysz)+x3(−cxsy)
- f = x 1 ( − c y s z ) + x 2 ( − c y c z ) f = x_1(-c_y s_z) + x_2(-c_y c_z) f=x1(−cysz)+x2(−cycz)
- g = x 1 ( c x c z − s x s y s z ) + x 2 ( − c x s z − s x s y c z ) g = x_1(c_x c_z - s_x s_y s_z) + x_2(-c_x s_z - s_x s_y c_z) g=x1(cxcz−sxsysz)+x2(−cxsz−sxsycz)
- h = x 1 ( s x c z + c x s y s z ) + x 2 ( c x s y c z − s x s z ) h = x_1(s_x c_z + c_x s_y s_z) + x_2(c_x s_y c_z - s_x s_z) h=x1(sxcz+cxsysz)+x2(cxsycz−sxsz)
- 更新海森矩阵 H \ H H，参考方程:
H i j = ∂ 2 s ∂ p i ∂ p j = ∑ k = 1 n d 1 d 2 exp ( − d 2 2 x k ′ T Σ k − 1 x k ′ ) [ − d 2 ( x k ′ T Σ k − 1 ∂ x k ′ ∂ p i ) ( x k ′ T Σ k − 1 ∂ x k ′ ∂ p j ) + x k ′ T Σ k − 1 ∂ 2 x k ′ ∂ p i ∂ p j + ∂ x k ′ ∂ p j T Σ k − 1 ∂ x k ′ ∂ p i ] H_{ij} = \frac{\partial^2 s}{\partial p_i \partial p_j} = \sum_{k=1}^{n} d_1 d_2 \exp\left( -\frac{d_2}{2} \mathbf{x}_k'^T \Sigma_k^{-1} \mathbf{x}_k' \right) \left[ -d_2 \left( \mathbf{x}_k'^T \Sigma_k^{-1} \frac{\partial \mathbf{x}_k'}{\partial p_i} \right) \left( \mathbf{x}_k'^T \Sigma_k^{-1} \frac{\partial \mathbf{x}_k'}{\partial p_j} \right) + \mathbf{x}_k'^T \Sigma_k^{-1} \frac{\partial^2 \mathbf{x}_k'}{\partial p_i \partial p_j} + \frac{\partial \mathbf{x}_k'}{\partial p_j}^T \Sigma_k^{-1} \frac{\partial \mathbf{x}_k'}{\partial p_i} \right] Hij=∂pi∂pj∂2s=k=1∑nd1d2exp(−2d2xk′TΣk−1xk′)[−d2(xk′TΣk−1∂pi∂xk′)(xk′TΣk−1∂pj∂xk′)+xk′TΣk−1∂pi∂pj∂2xk′+∂pj∂xk′TΣk−1∂pi∂xk′]
∂ 2 x k ′ ∂ p i ∂ p j = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 a b c 0 0 0 b d e 0 0 0 c e f ] \frac{\partial^2 \mathbf{x}_k'}{\partial p_i \partial p_j} = \begin{bmatrix} \begin{array}{cccccc} \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{a} & \mathbf{b} & \mathbf{c} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{b} & \mathbf{d} & \mathbf{e} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{c} & \mathbf{e} & \mathbf{f} \end{array} \end{bmatrix} ∂pi∂pj∂2xk′= 000000000000000000000abc000bde000cef
其中：
- a = [ 0 x 1 ( − c x s z − s x s y c z ) + x 2 ( − c x c z + s x s y s z ) + x 3 ( s x c y ) x 1 ( − s x s z + c x s y c z ) + x 2 ( − s x s y s z − c x c z ) + x 3 ( − c x c y ) ] \mathbf{a} = \begin{bmatrix} 0 \\ x_1(-c_x s_z - s_x s_y c_z) + x_2(-c_x c_z + s_x s_y s_z) + x_3(s_x c_y) \\ x_1(-s_x s_z + c_x s_y c_z) + x_2(-s_x s_y s_z - c_x c_z) + x_3(-c_x c_y) \end{bmatrix} a= 0x1(−cxsz−sxsycz)+x2(−cxcz+sxsysz)+x3(sxcy)x1(−sxsz+cxsycz)+x2(−sxsysz−cxcz)+x3(−cxcy)
- b = [ 0 x 1 ( c x c y c z ) + x 2 ( − c x c y s z ) + x 3 ( c x s y ) x 1 ( s x c y c z ) + x 2 ( − s x c y s z ) + x 3 ( s x s y ) ] \mathbf{b} = \begin{bmatrix} 0 \\ x_1(c_x c_y c_z) + x_2(-c_x c_y s_z) + x_3(c_x s_y) \\ x_1(s_x c_y c_z) + x_2(-s_x c_y s_z) + x_3(s_x s_y) \end{bmatrix} b= 0x1(cxcycz)+x2(−cxcysz)+x3(cxsy)x1(sxcycz)+x2(−sxcysz)+x3(sxsy)
- c = [ 0 x 1 ( − s x c z − c x s y s z ) + x 2 ( − s x s z − s x s y c z ) x 1 ( c x c z − s x s y s z ) + x 2 ( − s x s y c z − s x s z ) ] \mathbf{c} = \begin{bmatrix} 0 \\ x_1(-s_x c_z - c_x s_y s_z) + x_2(-s_x s_z - s_x s_y c_z) \\ x_1(c_x c_z - s_x s_y s_z) + x_2(-s_x s_y c_z - s_x s_z) \end{bmatrix} c= 0x1(−sxcz−cxsysz)+x2(−sxsz−sxsycz)x1(cxcz−sxsysz)+x2(−sxsycz−sxsz)
- d = [ x 1 ( − c y c z ) + x 2 ( c y s z ) + x 3 ( − s y ) x 1 ( − s x s y c z ) + x 2 ( s x s y s z ) + x 3 ( s x c y ) x 1 ( c x s y c z ) + x 2 ( − c x s y s z ) + x 3 ( − c x c y ) ] \mathbf{d} = \begin{bmatrix} x_1(-c_y c_z) + x_2(c_y s_z) + x_3(-s_y) \\ x_1(-s_x s_y c_z) + x_2(s_x s_y s_z) + x_3(s_x c_y) \\ x_1(c_x s_y c_z) + x_2(-c_x s_y s_z) + x_3(-c_x c_y) \end{bmatrix} d= x1(−cycz)+x2(cysz)+x3(−sy)x1(−sxsycz)+x2(sxsysz)+x3(sxcy)x1(cxsycz)+x2(−cxsysz)+x3(−cxcy)
- e = [ x 1 ( s y s z ) + x 2 ( s y c z ) x 1 ( − s x c y s z ) + x 2 ( − s x c y c z ) x 1 ( c x c y s z ) + x 2 ( c x c y c z ) ] \mathbf{e} = \begin{bmatrix} x_1(s_y s_z) + x_2(s_y c_z) \\ x_1(-s_x c_y s_z) + x_2(-s_x c_y c_z) \\ x_1(c_x c_y s_z) + x_2(c_x c_y c_z) \end{bmatrix} e= x1(sysz)+x2(sycz)x1(−sxcysz)+x2(−sxcycz)x1(cxcysz)+x2(cxcycz)
- f = [ x 1 ( − c y c z ) + x 2 ( c y s z ) x 1 ( − c x s z − s x s y c z ) + x 2 ( − c x c z + s x s y s z ) x 1 ( − s x s z + c x s y c z ) + x 2 ( − c x s y s z − s x s z ) ] \mathbf{f} = \begin{bmatrix} x_1(-c_y c_z) + x_2(c_y s_z) \\ x_1(-c_x s_z - s_x s_y c_z) + x_2(-c_x c_z + s_x s_y s_z) \\ x_1(-s_x s_z + c_x s_y c_z) + x_2(-c_x s_y s_z - s_x s_z) \end{bmatrix} f= x1(−cycz)+x2(cysz)x1(−cxsz−sxsycz)+x2(−cxcz+sxsysz)x1(−sxsz+cxsycz)+x2(−cxsysz−sxsz)
- 求解线性方程 H Δ p = − g \ H \Delta \mathbf{p} = -\mathbf{g} HΔp=−g。
- 更新参数 p \mathbf{p} p，公式如下：
p = p + Δ p \mathbf{p} = \mathbf{p} + \Delta \mathbf{p} p=p+Δp