$(2) $然后使用 $F^t_e$中的边缘特征及其在 $\mathbb{F}^{t-1}_e$ 中的对应关系，并同时使用 $[t_z, θ_{roll}, θ_{pitch}]$ 作为约束一起来估计剩余的 $[t_x, t_y, θ_{yaw}]$。需要注意的是，虽然$[t_x, t_y, θ_{yaw}]$也可以从第一个优化步骤中获得，但它们的准确度较低，得到的结果也不能继续放在第二步中使用。最后，通过融合 $[t_z, θ_{roll}, θ_{pitch}]$ 和$[t_x, t_y, θ_{yaw}]$找到两个连续扫描之间的$ 6D$ 变换。通过使用所提出的两步优化方法，我们观察到在计算时间减少约 $35\% $的同时可以实现类似的精度（表 III）。通过计算联合高斯分布从而得到协方差矩阵的逆：

$$ \begin{align*} p(x_1,x_2,x_3)&=p(x_2)p(x_1|x_2)p(x_3|x_2)\\ &={\color{green}\frac{1}{Z_2}\exp(-\frac{x^2_2}{2\sigma_2^2})} {\color{red}\frac{1}{Z_1}\exp(-\frac{(x_1-w_1x_2)^2}{2\sigma_1^2})} {\color{blue}\frac{1}{Z_3}\exp(-\frac{(x_3-w_3x_2)^2}{2\sigma_3^2})}\\ &=\frac{1}{Z}\exp( -x_2^2[{\color{green}\frac{1}{2\sigma_2^2} } +{\color{red}\frac{w_1^2}{2\sigma_1^2} } -{\color{blue}\frac{w_3^2}{2\sigma_3^2} }] -x_1^2{\color{red}\frac{1}{2\sigma_1^2} } +2x_1x_2{\color{red}\frac{w_1}{2\sigma_1^2} } -x_3^2{\color{blue}\frac{1}{2\sigma_3^2} } +2x_3x_2{\color{blue}\frac{w_3}{2\sigma_3^2} })\\ &=\frac{1}{Z}\exp(-\frac{1}{2} \begin{bmatrix}x_1&x_2&x_3\end{bmatrix} \begin{bmatrix} {\color{red}\frac{1}{\sigma_1^2} }&{\color{red}\frac{w_1}{\sigma_1^2} }&0\\ {\color{red}\frac{w_1}{\sigma_1^2} }&{\color{red}\frac{w_1^2}{\sigma_1^2} }+{\color{green}\frac{1}{\sigma_2^2} }+{\color{blue}\frac{w_3^2}{\sigma_3^2} }& {\color{blue}-\frac{w_3}{\sigma_3^2} }\\ 0&{\color{blue}-\frac{w_3}{\sigma_3^2} }&{\color{blue}-\frac{1}{\sigma_3^2} } \end{bmatrix} \begin{bmatrix}x_1\\x_2\\x_3\end{bmatrix} ) \end{align*}\tag{20} $$

由此得到协方差矩阵的逆，即信息矩阵：

$$ Λ =\Sigma^{-1} =\begin{bmatrix} {\color{red}\frac{1}{\sigma_1^2} }&{\color{red}\frac{w_1}{\sigma_1^2} }&0\\ {\color{red}\frac{w_1}{\sigma_1^2} }&{\color{red}\frac{w_1^2}{\sigma_1^2} }+{\color{green}\frac{1}{\sigma_2^2} }+{\color{blue}\frac{w_3^2}{\sigma_3^2} }& {\color{blue}-\frac{w_3}{\sigma_3^2} }\\ 0&{\color{blue}-\frac{w_3}{\sigma_3^2} }&{\color{blue}-\frac{1}{\sigma_3^2} } \end{bmatrix}\tag{21} $$

总结：

协方差矩阵中，非对角元素 $\Sigma_{ij}>0，i\ne j$ 表示两个变量之间是正相关；非对角元素 $\Sigma_{ij}=0，i\ne j$ 表示两个变量之间是相互独立；

信息矩阵中，非对角元素 $Λ_{ij}<0，i\ne j$ 甚至于 $Λ_{ij}=0，i\ne j$ ，比如 $Λ_{12} < 0$ 表示在变量 $x_3$ 发生的条件下，元素 $x_1$ 和 $x_2$ 正相关。

• 推导 $\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{p}^b_c }$:

$$ \begin{align*} \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{p}^b_c} &=\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{p}^b_c}\\ &=\frac{\partial (\mathcal{P}^{c_j}_l- \hat{\overline{\mathcal{P} } }^{c_j}_l)}{\partial\mathbf{p}^b_c }\\ &=\frac{\partial (\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\}- \hat{\overline{\mathcal{P} } }^{c_j}_l)}{\partial\mathbf{p}^b_c }\\ &=\frac{\partial\ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{p}^{b}_{c} -\mathbf{R}^c_b \mathbf{p}^{b}_{c} }{\partial\mathbf{p}^b_c }\\ &=\mathbf{R}^c_b(\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}-I_{3\times3}) \end{align*}\tag{J2-1} $$

• 推导 $\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{q}^b_c }$:

$$ \begin{align*} \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{q}^b_c } =&\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\mathbf{q}^b_c }\\ =&\frac{\partial (\mathcal{P}^{c_j}_l- \hat{\overline{\mathcal{P} } }^{c_j}_l)}{\partial\mathbf{q}^b_c }\\ =&\frac{\partial (\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\}- \hat{\overline{\mathcal{P} } }^{c_j}_l)}{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ \left[\mathbf{R}^c_b\exp({\delta\theta^b_c}^{\land})\right]^{-1} \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}\exp({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ (I-{\delta\theta^b_c}^{\land})\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}(I+{\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ &-\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{-{\delta\theta^b_c}^{\land}\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}(I+{\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ \approx&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ &-\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{-{\delta\theta^b_c}^{\land}\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} }{\partial\mathbf{q}^b_c } -\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ {\delta\theta^b_c}^{\land} \left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} -{\delta\theta^b_c}^{\land} \left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\} }{\partial\mathbf{q}^b_c }\\ =&-\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c} \left[\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l}\right]^{\land} +\left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\}^{\land}\\ =&-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\left(\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\and} +\left(\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\and} +\left\{ \mathbf{R}^c_b \left[ \mathbf{R}^{b_j}_w \left( \mathbf{R}^w_{b_i}\mathbf{p}^{b}_{c}+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right)-\mathbf{p}^{b}_{c} \right] \right\}^{\land} \end{align*}\tag{J2-2} $$

$$ \mathbf{J}[3]^{3\times1}= \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial\lambda_l } =-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l^2}\tag{J3} $$