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}
[ACBD][x1x2]=[vw]
通常对于SLAM问题，信息矩阵中的{
B
=
C
T
B=C^T
B=CT}，
A
，
D
A，D
A，D为可逆（对称）方阵
可以得到：
A
x
1
+
B
x
2
=
v
C
x
1
+
D
x
2
=
w
Ax_{1}+Bx_{2}=v\\Cx_{1}+Dx_{2}=w
Ax1+Bx2=vCx1+Dx2=w
设我们需要marg掉
x
2
x_2
x2，或者先求解
x
1
x_1
x1（
H
Δ
x
=
b
H\Delta x=b
HΔx=b问题），
我们对第一行左右两侧均乘以
B
D
−
1
BD^{-1}
BD−1，继而可以得到：
B
D
−
1
C
x
1
+
B
D
−
1
D
x
2
=
B
D
−
1
w
⇒
B
x
2
=
B
D
−
1
w
−
B
D
−
1
C
x
1
BD^{-1}Cx_{1}+BD^{-1}Dx_{2}=BD^{-1}w \Rightarrow Bx_{2}=BD^{-1}w-BD^{-1}Cx_{1}
BD−1Cx1+BD−1Dx2=BD−1w⇒Bx2=BD−1w−BD−1Cx1
带入
C
x
1
+
D
x
2
=
w
Cx_{1}+Dx_{2}=w
Cx1+Dx2=w得：
(
A
−
B
D
−
1
C
)
x
1
=
v
−
B
D
−
1
w
(A-BD^{-1}C)x_{1}=v-BD^{-1}w
(A−BD−1C)x1=v−BD−1w
从而可以看出，我们通过回代同样得到了Schur补的情况。从而，我们可以知道无论是边缘化变量，还是更新参数的 H Δ x = b H\Delta x=b HΔx=b的分块求解问题，归根到底都可以理解为变量的消元问题。
对角块矩阵的逆为各个块的逆的组合
对于给定的 ( 9 × 9 9 \times 9 9×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= A1000A2000A3
其中 A 1 A_1 A1, A 2 A_2 A2, 和 A 3 A_3 A3 均为可逆的 3 × 3 3 \times 3 3×3 矩阵，可以通过求解块对角矩阵的逆来找到 B B 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= A1000A2000A3
其逆矩阵也是一个块对角矩阵，其形式为：
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−1000A2−1000A3−1
2. 证明
设 X X X 为 B B B 的逆矩阵，即 B X = I BX = I BX=I，其中 I I I 是 9 × 9 9 \times 9 9×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} A1000A2000A3 A1−1000A2−1000A3−1 = A1A1−1000A2A2−1000A3A3−1 = I3000I3000I3
这里的 I 3 I_{3} I3 是 3 × 3 3 \times 3 3×3 的单位矩阵。
可以看到，右边的矩阵确实是 9 × 9 9 \times 9 9×9 的单位矩阵 I I 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−1000A2−1000A3−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]
[BETEC][ΔxcΔxp]=[220]
其中，
Δ
x
c
\Delta x_{c}
Δxc对应传感器位姿变量的更新量；
Δ
x
c
\Delta x_{c}
Δxc特征点位置对应的更新量。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−1ETET0C][ΔxcΔxp]=
v−EC−1ww
.
从而可以将求逆问题简化为对角块矩阵求逆的问题，先得到
Δ
x
c
\Delta\boldsymbol{x}_c
Δxc
[
B
−
C
−
1
E
T
]
Δ
x
c
=
v
−
E
C
−
1
w
.
[B-C^{-1}E^T]\Delta\boldsymbol{x}_c=\boldsymbol{v}-EC^{-1}\boldsymbol{w}.
[B−C−1ET]Δxc=v−EC−1w.
而后，再带入 B Δ x c + E Δ x p = v B\Delta\boldsymbol{x}_c+E\Delta\boldsymbol{x}_p=\boldsymbol{v} BΔxc+EΔxp=v计算得到 Δ x p \Delta\boldsymbol{x}_p Δxp
此外，也可以利用SAM的方法参考链接，经过因式分解，得到上三角阵，而后使用回代的方法进行求解。