VINS-Mono之邊緣化 Marginalization 和 FEJ (First Estimated Jocobian)

1. 前言

本博客主要介紹了VINS-Mono中邊緣化的相關知識，由於VINS-Mono中只是提及了邊緣化的策略並沒有提及邊緣化信息傳遞的原理，因此本博客主要參考了崔化坤的《VINS論文推導及代碼解析》和深藍學院的VIO課程。

VINS-Mono的邊緣化與在《SLAM14講》中也有提及邊緣化 (可看博客SLAM學習——後端（二）) 的不同：

• 《SLAM14講》中提及的邊緣化 (G2O邊緣化) 是在計算求解過程中，先消去路標點變量，實現先求解相機位姿，然後再利用求解出來的相機位姿反過來計算路標點的過程，目的是爲了加速求解，並非真的將路標點給邊緣化點。
• VINS-Mono的邊緣化則真正需要邊緣化掉滑動窗口中的最老幀或次新幀，目的是希望不再計算這一幀的位姿或者與其相關的路標點，但是希望保留該幀對窗口內其他幀的約束關係。

2. 舒爾補 (Schur complement) 的應用：邊界概率，條件概率

將矩陣$M=\begin{bmatrix} A & B \\ C & D\end{bmatrix}$變成上三角或者下三角形過程中，會用到舒爾補操作：$\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}$ $\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}$ 其中$A$爲可逆矩陣，$\Delta_{A}=D-CA^{-1}B$，稱爲A關於M的舒爾補，聯合起來，將M變形成對角矩陣：$\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}$ 反過來又能從對角矩陣恢復成矩陣$M$：$\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}$ 以上變換均建立在矩陣$A$可逆的前提下，如果矩陣$A$不可逆而矩陣$D$可逆，同樣可以進行舒爾補操作將$M$矩陣變爲上三角或者下三角的形式： $\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}$ 其中$\Delta_{D}=A-BD^{-1}C$，稱爲$D$關於$M$的舒爾補。