文章目录
1. 前言
本博客主要介绍了VINS-Mono中边缘化的相关知识，由于VINS-Mono中只是提及了边缘化的策略并没有提及边缘化信息传递的原理，因此本博客主要参考了崔化坤的《VINS论文推导及代码解析》、深蓝学院的VIO课程以及贺博的博客SLAM中的marginalization 和 Schur complement。
VINS-Mono的边缘化与在《SLAM14讲》中提及的边缘化 (可看博客SLAM学习——后端（二）) 不同：
- 《SLAM14讲》中提及的边缘化 (G2O边缘化) 是在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机位姿反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化点。
- VINS-Mono的边缘化则真正需要边缘化掉滑动窗口中的最老帧或次新帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其他帧的约束关系。
2. 舒尔补 (Schur complement) 实现边缘化 (Marginalization)
2.1 舒尔补操作及矩阵快速求逆
将矩阵 M = [ A B C D ] M=\begin{bmatrix} A & B \\ C & D\end{bmatrix} M=[ACBD]变成上三角或者下三角形过程中，会用到舒尔补操作： [ I 0 − C A − 1 I ] [ A B C D ] = [ A B 0 Δ A ] \begin{bmatrix} I & 0 \\ -CA^{-1} & I \end{bmatrix} \begin{bmatrix} A & B \\ C & D \end{bmatrix} = \begin{bmatrix} A & B \\ 0 & \Delta_{A}\end{bmatrix} [I−CA−10I][ACBD]=[A0BΔA] [ A B C D ] [ I − A − 1 B 0 I ] = [ A 0 C Δ A ] \begin{bmatrix}A & B \\ C & D\end{bmatrix}\begin{bmatrix} I & -A^{-1}B \\ 0 & I\end{bmatrix}=\begin{bmatrix} A & 0 \\ C & \Delta _{A}\end{bmatrix} [ACBD][I0−A−1BI]=[AC0ΔA] 其中 A A A为可逆矩阵， Δ A = D − C A − 1 B \Delta_{A}=D-CA^{-1}B ΔA=D−CA−1B，称为A关于M的舒尔补，联合起来，将M变形成对角矩阵： [ I 0 − C A − 1 I ] [ A B C D ] [ I − A − 1 B 0 I ] = [ A 0 0 Δ A ] \begin{bmatrix} I & 0 \\ -CA^{-1} & I \end{bmatrix} \begin{bmatrix} A & B \\ C & D \end{bmatrix} \begin{bmatrix} I & -A^{-1}B \\ 0 & I\end{bmatrix}=\begin{bmatrix} A & 0 \\ 0 & \Delta _{A}\end{bmatrix} [I−CA−10I][ACBD][I0−A−1BI]=[A00ΔA] 反过来又能从对角矩阵恢复成矩阵 M M M： [ I 0 C A − 1 I ] [ A 0 0 Δ A ] [ I A − 1 B 0 I ] = [ A B C D ] \begin{bmatrix} I & 0 \\ CA^{-1} & I \end{bmatrix}\begin{bmatrix} A & 0 \\ 0 & \Delta _{A}\end{bmatrix} \begin{bmatrix} I & A^{-1}B \\ 0 & I\end{bmatrix} = \begin{bmatrix} A & B \\ C & D \end{bmatrix} [ICA−10I][A00ΔA][I0A−1BI]=[ACBD] 以上变换均建立在矩阵 A A A可逆的前提下，如果矩阵 A A A不可逆而矩阵 D D D可逆，同样可以进行舒尔补操作将 M M M矩阵变为上三角或者下三角的形式： [ I − B D − 1 0 I ] [ A B C D ] = [ Δ D 0 C D ] \begin{bmatrix} I & -BD^{-1} \\ 0 & I \end{bmatrix} \begin{bmatrix} A & B \\ C & D \end{bmatrix} = \begin{bmatrix} \Delta_{D} & 0 \\ C & D \end{bmatrix} [I0−BD−1I][ACBD]=[ΔDC0D] 其中 Δ D = A − B D − 1 C \Delta_{D}=A-BD^{-1}C ΔD=A−BD−1C，称为 D D D关于 M M M的舒尔补。
舒尔补操作在实现将矩阵 M M M分解为上三角下三角形式的同时，也实现了矩阵M的快速求逆： M = [ A B C D ] = [ I 0 C A − 1 I ] [ A 0 0 Δ A ] [ I A − 1 B 0 I ] M=\begin{bmatrix}A & B \\ C & D \end{bmatrix}=\begin{bmatrix}I & 0 \\CA^{-1} & I \end{bmatrix} \begin{bmatrix} A & 0 \\ 0 & \Delta_{A}\end{bmatrix}\begin{bmatrix} I & A^{-1}B \\ 0 & I\end{bmatrix} M=[ACBD]=[ICA−10I][A00ΔA][I0A−1BI] 求逆可得： M − 1 = [ A B C D ] − 1 = [ I − A − 1 B 0 I ] [ A − 1 0 0 Δ A − 1 ] [ I 0 − C A − 1 I ] M^{-1}=\begin{bmatrix}A & B \\ C & D\end{bmatrix}^{-1}=\begin{bmatrix}I & -A^{-1}B \\ 0 & I\end{bmatrix} \begin{bmatrix} A^{-1}& 0 \\ 0 & \Delta_{A}^{-1}\end{bmatrix} \begin{bmatrix} I & 0 \\ -CA^{-1} & I \end{bmatrix} M−1=[ACBD]−1=[I0−A−1BI][A−100ΔA−1][I−CA−10I] 其中 [ I − A − 1 B 0 I ] [ I A − 1 B 0 I ] = I \begin{bmatrix}I & -A^{-1}B \\ 0 & I\end{bmatrix}\begin{bmatrix}I & A^{-1}B \\ 0 & I\end{bmatrix}=I [I0−A−1BI][I0A−1BI]=I
2.2 多元高斯分布下的舒尔补分解：边际分布 (marginal distribution) 及条件分布
假设多元变量服从零均值高斯分布 P ( α , β ) P(\alpha, \beta) P(α,β)，且由俩部分组成： x = [ α β ] \mathbf{x}=\begin{bmatrix} \alpha \\ \beta \end{bmatrix} x=[αβ]，变量之间构成的协方差矩阵为： K = [ A C T C D ] (1) K=\begin{bmatrix} A & C^{T} \\ C & D\end{bmatrix} \tag{1} K=[ACCTD](1) 其中 A = c o v ( α ,
最低0.47元/天 解锁文章
2611
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
