SLAM中的块矩阵与schur补

Schur补的另一种解释

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

首先设我们需要求解以下问题：
$$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right]$$
通常对于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=\left[\begin{array}{ccc}{A}_1& 0& 0\\ 0& A_2& 0\\ 0& 0& A_3\end{array}\right]$$

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

1. 块对角矩阵的逆

对于一个块对角矩阵：

$$B=\left[\begin{array}{ccc}{A}_1& 0& 0\\ 0& A_2& 0\\ 0& 0& A_3\end{array}\right]$$

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

$$B^{-1}=\left[\begin{array}{ccc}{A}_1^{-1}& 0& 0\\ 0& A_2^{-1}& 0\\ 0& 0& A_3^{-1}\end{array}\right]$$

2. 证明

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

考虑以下矩阵乘法：

$$\left[\begin{array}{ccc}{A}_1& 0& 0\\ 0& A_2& 0\\ 0& 0& A_3\end{array}\right]\left[\begin{array}{ccc}{A}_1^{-1}& 0& 0\\ 0& A_2^{-1}& 0\\ 0& 0& A_3^{-1}\end{array}\right]=\left[\begin{array}{ccc}{A}_1{A}_1^{-1}& 0& 0\\ 0& A_2{A}_2^{-1}& 0\\ 0& 0& A_3{A}_3^{-1}\end{array}\right]=\left[\begin{array}{ccc}{I}_3& 0& 0\\ 0& I_3& 0\\ 0& 0& I_3\end{array}\right]$$

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

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

$$\left[\begin{array}{ccc}{A}_1^{-1}& 0& 0\\ 0& A_2^{-1}& 0\\ 0& 0& A_3^{-1}\end{array}\right]$$

是 B$ 的逆矩阵。

因而型如以下的SLAM求解问题，可以使用schur补+对角块矩阵逆的特性，高效求解
$$\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}2\\ 20\end{array}\right]$$
其中，$\Delta x_c$对应传感器位姿变量的更新量；$\Delta x_c$特征点位置对应的更新量。SLAM问题中的信息矩阵的结构对应如下图所示，其中关于特征点的部分(该矩阵中的C块矩阵)为对角块矩阵。

Schur补得到：
$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{c}\mathbf{v}-\mathbf{E}{C}^{-1}\mathbf{w}\\ \\ \mathbf{w}\end{array}\right].$$

从而可以将求逆问题简化为对角块矩阵求逆的问题，先得到$\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的方法参考链接，经过因式分解，得到上三角阵，而后使用回代的方法进行求解。