SLAM中的块矩阵与schur补
文章目录
- SLAM中的块矩阵与schur补
- 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} [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的方法参考链接，经过因式分解，得到上三角阵，而后使用回代的方法进行求解。