SLAM中的块矩阵与schur补

SLAM中的块矩阵与schur补

Schur补的另一种解释

Schur从概率角度来解释是比较常见的一种推导，可以参考博客https://blog.csdn.net/weixin_41469272/article/details/121994485，此外也可以通过消元与回代的基本原理得到相同的结论。

首先设我们需要求解以下问题：
[ A B C D ] [ x 1 x 2 ] = [ v w ]

$$\begin{bmatrix}A&B\\C&D\end{bmatrix}$$ $$\begin{bmatrix}x_{1}\\x_{2}\end{bmatrix}$$ = $$\begin{bmatrix}v\\w\end{bmatrix}$$ [AC​BD​][x1​x2​​]=[vw​]
通常对于SLAM问题，信息矩阵中的{$B=C^T$}，$A，D$为可逆（对称）方阵

可以得到：
$$\begin{gathered} A{x}_1+B{x}_2=v \\ C{x}_1+D{x}_2=w \end{gathered}$$
设我们需要marg掉$x_2$，或者先求解$x_1$（$H\Delta x=b$问题），

我们对第一行左右两侧均乘以$B{D}^{-1}$，继而可以得到：
$$B{D}^{-1}C{x}_1+B{D}^{-1}D{x}_2=B{D}^{-1}w\Rightarrow B{x}_2=B{D}^{-1}w-B{D}^{-1}C{x}_1$$

带入$C{x}_1+D{x}_2=w$得：
$$(A-B{D}^{-1}C)x_1=v-B{D}^{-1}w$$

从而可以看出，我们通过回代同样得到了Schur补的情况。从而，我们可以知道无论是边缘化变量，还是更新参数的$H\Delta x=b$的分块求解问题，归根到底都可以理解为变量的消元问题。

对角块矩阵的逆为各个块的逆的组合

对于给定的 ($9\times 9$) 矩阵：

B = [ A 1 0 0 0 A 2 0 0 0 A 3 ] B =

$$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix}$$ B= ​A1​00​0A2​0​00A3​​ ​

其中 $A_1$, $A_2$, 和 $A_3$ 均为可逆的 $3\times 3$ 矩阵，可以通过求解块对角矩阵的逆来找到 $B$的逆。

1. 块对角矩阵的逆

对于一个块对角矩阵：

B = [ A 1 0 0 0 A 2 0 0 0 A 3 ] B =

$$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix}$$ B= ​A1​00​0A2​0​00A3​​ ​

其逆矩阵也是一个块对角矩阵，其形式为：

B − 1 = [ A 1 − 1 0 0 0 A 2 − 1 0 0 0 A 3 − 1 ] B^{-1} =

$$\begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix}$$ B−1= ​A1−1​00​0A2−1​0​00A3−1​​ ​

2. 证明

设 $X$ 为 $B$ 的逆矩阵，即 $BX=I$，其中 $I$ 是 $9\times 9$ 的单位矩阵。

考虑以下矩阵乘法：

[ A 1 0 0 0 A 2 0 0 0 A 3 ] [ A 1 − 1 0 0 0 A 2 − 1 0 0 0 A 3 − 1 ] = [ A 1 A 1 − 1 0 0 0 A 2 A 2 − 1 0 0 0 A 3 A 3 − 1 ] = [ I 3 0 0 0 I 3 0 0 0 I 3 ]

$$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix}$$ $$\begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix}$$ = $$\begin{bmatrix} A_{1}A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}A_{3}^{-1} \end{bmatrix}$$ = $$\begin{bmatrix} I_{3} & 0 & 0 \\ 0 & I_{3} & 0 \\ 0 & 0 & I_{3} \end{bmatrix}$$ ​A1​00​0A2​0​00A3​​ ​ ​A1−1​00​0A2−1​0​00A3−1​​ ​= ​A1​A1−1​00​0A2​A2−1​0​00A3​A3−1​​ ​= ​I3​00​0I3​0​00I3​​ ​

这里的 $I_3$ 是 $3\times 3$ 的单位矩阵。

可以看到，右边的矩阵确实是 $9\times 9$ 的单位矩阵 $I$。因此，

[ A 1 − 1 0 0 0 A 2 − 1 0 0 0 A 3 − 1 ]

$$\begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix}$$ ​A1−1​00​0A2−1​0​00A3−1​​ ​

是 B$ 的逆矩阵。

因而型如以下的SLAM求解问题，可以使用schur补+对角块矩阵逆的特性，高效求解
[ B E E T C ] [ Δ x c Δ x p ] = [ 2 20 ] \left[

$$\begin{matrix}B&E\\E^{T}&C\end{matrix}$$ \right]\left[ $$\begin{matrix}\Delta x_{c}\\\Delta x_{p}\end{matrix}$$ \right]=\left[ $$\begin{matrix}2\\20\end{matrix}$$ \right] [BET​EC​][Δxc​Δxp​​]=[220​]
其中，$\Delta x_c$对应传感器位姿变量的更新量；$\Delta x_c$特征点位置对应的更新量。SLAM问题中的信息矩阵的结构对应如下图所示，其中关于特征点的部分(该矩阵中的C块矩阵)为对角块矩阵。

Schur补得到：
[ B − E C − 1 E T 0 E T C ] [ Δ x c Δ x p ] = [ v − E C − 1 w w ] .

$$\begin{bmatrix}B-EC^{-1}E^T&\mathbf{0}\\E^T&C\end{bmatrix}$$ \left[ $$\begin{array}{c}\Delta\boldsymbol{x}_c\\\Delta\boldsymbol{x}_p\end{array}$$ \right]=\left[ $$\begin{array}{c}\boldsymbol{v}-\boldsymbol{E}C^{-1}\boldsymbol{w}\\\\\boldsymbol{w}\end{array}$$ \right]. [B−EC−1ETET​0C​][Δxc​Δxp​​]= ​v−EC−1ww​ ​.

从而可以将求逆问题简化为对角块矩阵求逆的问题，先得到$\Delta {\mathbf{x}}_c$
$$[B-C^{-1}{E}^T]\Delta {\mathbf{x}}_c=\mathbf{v}-E{C}^{-1}\mathbf{w}.$$

而后，再带入$B\Delta {\mathbf{x}}_c+E\Delta {\mathbf{x}}_p=\mathbf{v}$计算得到$\Delta {\mathbf{x}}_p$

此外，也可以利用SAM的方法参考链接，经过因式分解，得到上三角阵，而后使用回代的方法进行求解。