利用舒尔补进行边缘化

边缘化是SLAM中常用的消元方法，能够有效减少计算量。在SLAM的BA过程中，对于状态变量的更新，可以给出一般化的更新方程：
$H\delta x = g$Hδx=g
其中，$H$H是信息矩阵。
上面的方程可以写成：
$\begin{bmatrix} \Lambda_a & \Lambda_b\\ \Lambda_b^T& \Lambda_c \end{bmatrix} \begin{bmatrix} \delta x_a \\ \delta x_b \end{bmatrix} = \begin{bmatrix} g_a \\ g_b \end{bmatrix}$[Λa​ΛbT​​Λb​Λc​​][δxa​δxb​​]=[ga​gb​​]
其中，$\delta x_a$δxa​是我们不希望更新的变量。
在等式两边右乘$\begin{bmatrix}I &0 \\ -\Lambda _b^T \Lambda _a^{-1} & I \end{bmatrix}$[I−ΛbT​Λa−1​​0I​]
可得：
$\begin{bmatrix} \Lambda_a & \Lambda_b\\ 0& \Lambda_c -\Lambda_b^T \Lambda_a^{-1} \Lambda_b \end{bmatrix} \begin{bmatrix} \delta x_a \\ \delta x_b \end{bmatrix} = \begin{bmatrix} g_a \\ g_b -\Lambda_b^T \Lambda_a^{-1}g_a \end{bmatrix}$[Λa​0​Λb​Λc​−ΛbT​Λa−1​Λb​​][δxa​δxb​​]=[ga​gb​−ΛbT​Λa−1​ga​​]
其中，$\Lambda_c-\Lambda_b^T \Lambda_a^{-1} \Lambda_b$Λc​−ΛbT​Λa−1​Λb​被称为$\Lambda _a$Λa​的舒尔补。
所以：
$(\Lambda_c-\Lambda_b^T \Lambda_a^{-1} \Lambda_b)\delta x_b = g_b -\Lambda_b^T \Lambda_a^{-1}g_a$(Λc​−ΛbT​Λa−1​Λb​)δxb​=gb​−ΛbT​Λa−1​ga​
上面就是$\delta x_b$δxb​的更新方程。
边缘化在SLAM中的使用图示：



参考：https://github.com/StevenCui/VIO-Doc/blob/master/VINS%E8%AE%BA%E6%96%87%E6%8E%A8%E5%AF%BC%E5%8F%8A%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90_V13_190317.pdf