VSLAM之邊緣化 Marginalization 和 FEJ (First Estimated Jocobian)

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=\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$的舒爾補。

舒爾補操作在實現將矩陣$M$分解爲上三角下三角形式的同時，也實現了矩陣M的快速求逆：$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^{-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}$ 其中 $\begin{bmatrix}I & -A^{-1}B \\ 0 & I\end{bmatrix}\begin{bmatrix}I & A^{-1}B \\ 0 & I\end{bmatrix}=I$

2.2 多元高斯分佈下的舒爾補分解：邊際分佈 (marginal distribution) 及條件分佈

假設多元變量服從零均值高斯分佈$P(\alpha, \beta)$，且由倆部分組成：$\mathbf{x}=\begin{bmatrix} \alpha \\ \beta \end{bmatrix}$，變量之間構成的協方差矩陣爲：$K=\begin{bmatrix} A & C^{T} \\ C & D\end{bmatrix} \tag{1}$ 其中$A=cov(\alpha, \alpha)$，$D=cov(\beta, \beta)$，$C=cov(\alpha, \beta)$，由此變量$x$的分佈爲：$P(\alpha,\beta)=P(\alpha)P(\beta|\alpha) \propto exp(-\frac{1}{2}\begin{bmatrix} \alpha \\ \beta \end{bmatrix}^{T}\begin{bmatrix} A & C^{T} \\ C & D\end{bmatrix}^{-1}\begin{bmatrix} \alpha \\ \beta \end{bmatrix}) \\ \propto exp(-\frac{1}{2} \begin{bmatrix} \alpha \\ \beta \end{bmatrix}^{T} \begin{bmatrix}I & -A^{-1}C^{T} \\ 0 & I\end{bmatrix} \begin{bmatrix} A^{-1}& 0 \\ 0 & \Delta_{A}^{-1}\end{bmatrix} \begin{bmatrix} I & 0 \\ -CA^{-1} & I \end{bmatrix}\begin{bmatrix} \alpha \\ \beta \end{bmatrix}) \\ \propto exp(-\frac{1}{2}\begin{bmatrix}\alpha^{T} & (\beta-CA^{-1}\alpha)^{T}\end{bmatrix}\begin{bmatrix} A^{-1}& 0 \\ 0 & \Delta_{A}^{-1}\end{bmatrix}\begin{bmatrix} \alpha & \beta-CA^{-1}\alpha\end{bmatrix}) \\ \propto exp(-\frac{1}{2}(\alpha^{T}A^{-1}\alpha)+(\beta-CA^{-1}\alpha)^{T}\Delta_{A}^{-1}(\beta-CA^{-1}\alpha)) \\ \propto \underset{P(\alpha)}{\underbrace{exp(-\frac{1}{2}\alpha^{T}A^{-1}\alpha)}}\underset{P(\beta|\alpha)}{\underbrace{exp(-\frac{1}{2}(\beta-CA^{-1}\alpha)^{T}\Delta_{A}^{-1}(\beta-CA^{-1}\alpha))}} \tag{2}$
意味着通過舒爾補操作能從高斯聯合分佈$P(\alpha,\beta)$分解出邊際分佈$P(\alpha)$ (邊緣掉了$\beta$)和條件分佈$P(\beta|\alpha)$。可以看出邊際分佈的協方差爲$A$，即爲從聯合分佈中取對應的矩陣塊，而條件分佈的協方差爲$\Delta_{A}$，即$A$對應的舒爾補$D-CA^{-1}B$，均值也變了。

在SLAM的優化問題中，我們往往操作的是信息矩陣，而不是協方差矩陣，因此需要計算邊際分佈和條件分佈對應的信息矩陣：
對於高斯聯合分佈$P(a, \beta)$的信息矩陣，由於信息矩陣等於協方差矩陣的逆，根據公式$(1)(2)$我們有：$\begin{bmatrix} A & C^{T} \\ C & D\end{bmatrix}^{-1} = \begin{bmatrix} A^{-1}+A^{-1}C^{T}\Delta_{A}^{-1}CA^{-1} & -A^{-1}C^{T}\Delta_{A}^{-1} \\ -CA^{-1}\Delta A^{-1} & \Delta_{A}^{-1}\end{bmatrix} = \begin{bmatrix} \Lambda_{\alpha \alpha} & \Lambda_{\alpha \beta} \\ \Lambda_{\beta \alpha} & \Lambda_{\beta\beta} \end{bmatrix}$ 由於條件分佈$P(\beta|\alpha)$和邊際分佈$P(\alpha)$的協方差矩陣分別爲$\Delta_{A}$、$A$，故其信息矩陣分別爲：$\Delta_{A}^{-1} = \Lambda_{\beta \beta} \ , \ A^{-1}=\Lambda_{\alpha \alpha}-\Lambda_{\alpha \beta}\Lambda^{-1}_{\beta \beta}\Lambda_{\beta \alpha} \tag{3}$

對於非零矩陣的高斯分佈$P(\alpha, \beta)=\mathcal{N}(\begin{bmatrix} \mu_{\alpha} \\ \mu_{\beta}\end{bmatrix}, \begin{bmatrix} \Sigma_{\alpha \alpha}, \Sigma_{a \beta} \\ \Sigma_{\beta \alpha}, \Sigma_{\beta \beta} \end{bmatrix}) = \mathcal{N}^{-1}(\begin{bmatrix} \eta_{\alpha} \\ \eta_{\beta} \end{bmatrix}, \begin{bmatrix} \Lambda_{\alpha \alpha} & \Lambda_{\alpha \beta} \\ \Lambda_{\beta \alpha} & \Lambda_{\beta \beta}\end{bmatrix})$，

分解出的邊際分佈 (Marginalization) $P(\alpha)$ (邊緣掉了$\beta$) 和條件分佈 (Conditioning) $P(\alpha|\beta )$ (注意我們前面的推導是$P(\beta|\alpha)$) 對應的協方差矩陣 (Covariance Form) 和信息矩陣 (Information Form） 如下：

3. 視覺SLAM優化變量的邊緣化

3.1 不丟棄優化變量的邊緣化 (加速更新位姿求解)

在視覺SLAM中Bundle Adjustment優化相機和位姿時，構建的非線性最小二乘問題可以通過高斯牛頓迭代獲得，即$H \delta x = b$, 這裏的$H$矩陣 (通過$\Sigma$範數構建的殘差項) 也是本文所提及的信息矩陣，由於$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} \tag{4}$ 假如這裏我們要邊緣化掉$\delta x_{b}$，通過舒爾補對上面方程進行消元得：$\begin{bmatrix} \Lambda_{a}-\Lambda_{b}\Lambda_{c}^{-1}\Lambda_{b}^{T} & 0 \\ \Lambda_{b}^{T} & \Lambda_{c}\end{bmatrix} \begin{bmatrix} \delta x_{a} \\ \delta x_{b} \end{bmatrix} = \begin{bmatrix} g_{a}- \Lambda_{b}\Lambda_{c}^{-1} g_{b}\\ g_{b}\end{bmatrix}$ 此時關於$\delta a$的方程爲：$(\Lambda_{a}-\Lambda_{b}\Lambda_{c}^{-1}\Lambda_{b}^{T})\delta x_{a} = g_{a}-\Lambda_{b}\Lambda_{c}^{-1}g_{b}$ 關於變量$$\delta x_{a}$的信息矩陣爲：$A^{-1}= \Lambda_{a}-\Lambda_{b}\Lambda_{c}^{-1}\Lambda_{b}^{T}$， 與公式$(3)$一致。

假如要邊緣掉$\delta x_{a}$, 通過舒爾補對方程 $(4)$ 進行消元得：$\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}$ 此時關於$\delta b$的方程爲：$(\Lambda_{c}-\Lambda_{b}^{T}\Lambda_{a}^{-1}\Lambda_{b})\delta x_{b} = g_{b}- \Lambda_{b}^{T}\Lambda_{a}^{-1} g_{a}$ 關於變量$\delta x_{b}$的信息矩陣爲：$B^{-1} =(\Lambda_{c}-\Lambda_{b}^{T}\Lambda_{a}^{-1}\Lambda_{b}) \tag{5}$

上述倆種操作由於邊緣化的變量不同，導致後面得到的信息矩陣亦不同，因此在邊緣化中應明確保留的變量和邊緣化掉的變量。

在上面這個過程中，我們要注意，構建出來的$Hx=b$是利用了邊緣化變量的信息，也就是說我們沒有人爲的丟棄約束，所以不會丟失信息，但是計算結果的時候，我們只去更新了我們希望保留的那些變量的值。

3.2 丟棄優化變量的邊緣化 (減少優化變量並傳遞邊緣化先驗)

下面用一個具體例子來形象說明邊緣化過程及其導致的矩陣稠密現象(fill-in)。假設有4個相機位姿$x_{pt}$，以及6個路標點$x_{mk}$ (路標點用$xyz$的參數化)，相機與路標點的邊表示一次觀測，相鄰相機之間的邊表示IMU約束，相互關係如下：

圖1. 4個相機觀察到6個路標點的圖關係

下面試圖將$x_{p1}$給marg (邊緣化) 掉，然後再將$x_{m1}$給marg掉，看看信息矩陣H會如何變化：

圖2. 邊緣化掉位姿和路標點時H矩陣的變化

其中，圖(2-a)表示原始的$H$矩陣，注意這裏的左上角爲路標點相關部分，而左上角是Pose的相關部分，圖(2-b)是把$H$矩陣中跟$x_{p1}$相關的部分移動到$H$矩陣的左上角，詳細表示如下：

圖3. 圖(2-a)的詳細表示

圖4. 圖(2-b)的詳細表示

當將$H$矩陣關於Pose1 $x_{p1}$ 的相關部分移到左上角後，根據圖4 $H$ 矩陣的變量的分佈情況構建與公式(4)類似的矩陣等式: $\begin{bmatrix} {\Lambda_{a}}^{6 \times 6} & {\Lambda_{b}}^{6 \times 36} \\ {\Lambda_{b}^{T}}^{36 \times6} & {\Lambda c}^{36 \times 36}\end{bmatrix} \begin{bmatrix} {\delta x_{a}}^{6 \times 1} \\ {\delta x_{b}}^{36 \times 1} \end{bmatrix} = \begin{bmatrix} {g_{a}}^{6 \times 1} \\ {g_{b}}^{36 \times 1}\end{bmatrix} \tag{6}$ 我們通過舒爾補操作將$x_{p1}$邊緣化，即爲公式$(5)$對應的$(\Lambda_{c}-\Lambda_{b}^{T}\Lambda_{a}^{-1}\Lambda_{b})$，得到新的信息矩陣$H_{new}$即爲$(\Lambda_{c}-\Lambda_{b}^{T}\Lambda_{a}^{-1}\Lambda_{b})$，相比於原來的信息矩陣，新的信息矩陣更加稠密，即marg掉一個pose後，會使得$H_{new}$有3個地方被fill-in, 如下黃色區域：

圖5. 圖(2-c)的詳細表示

這時圖關係則變爲：

圖6. 邊緣化掉Pose1後的圖關係

觀察圖6可知，$x_{m1}$、$x_{m2}$和$x_{m3}$彼此之間已經產生了新的約束關係，且$x_{p2}$和$m_{1}$產生了新的關係。因此，原先條件獨立的變量，在邊緣化某些變量之後，可能變得相關。
緊接着marg掉路標點$x_{m_{1}}$，新的信息矩陣變量的約束關係如下圖：

圖7. 圖(2-d)的詳細表示

對應的圖關係如下：

圖8. 圖(2-d)的詳細表示

可以發現，marg 掉$x_{m1}$後，並沒有使H矩陣更稠密，這是因爲$x_{m1}$之前並未與其他pose有約束關係，即並未被觀察到，因此如果marg掉那些不被其他幀觀察到的路標點，不會顯著使信息矩陣H變得稠密。而要marg掉的路標點中，對於那些被其他幀觀測到的路標點，要麼就別設置爲marg，要麼就寧願丟棄，這是OKVIS和DSO中用到的策略。

3.3 VINS倆種邊緣化策略

圖9. VINS-Mono邊緣化的倆種策略

VINS根據次新幀是否爲關鍵幀，分爲倆種邊緣化策略：通過對比次新幀和次次新幀的視差量，來決定marg掉次新幀或者最老幀：(由於這部分理論已在上面講完，待補充代碼講解部分)

1. 當次新幀爲關鍵幀時，MARGIN_OLD，將 marg 掉最老幀，及其看到的路標點和相關聯的 IMU 數據,將其轉化爲先驗信息加到整體的目標函數中。
2. 當次新幀不是關鍵幀時,MARGIN_SECOND_NEW，我們將直接扔掉次新幀及它的視覺觀測邊，而不對次新幀進行 marg，因爲我們認爲當前幀和次新幀很相似，也就是說當前幀跟路標點之間的約束和次新幀與路標點的約束很接近，直接丟棄並不會造成整個約束關係丟失過多信息。但是值得注意的是，我們要保留次新幀的 IMU 數據，從而保證 IMU 預積分的連貫性。

4. First Estimate Jacobian (FEJ)

舉一個簡單的例子引出新測量信息和舊測量信息構建的增量方程的解所存在的問題：

假如有5個相機位姿$\xi_{i}$，每個相機位姿與其他相機構建的殘差用邊表示，有一元邊，二元邊，多元邊等，相機之間的關係如下所示：

圖10. 5個相機之間的圖關係

通過誤差項和信息矩陣得到的優化變量的信息矩陣爲：$\Lambda_{m}(k) = \underset{(i,j)\in S_{m}}{\sum} J^{T}_{ij}(k) \Sigma_{ij}^{-1}J_{ij}(k) = \begin{bmatrix} \Lambda_{\beta \beta}(k) & \Lambda_{\beta \alpha}(k) \\ \Lambda_{\alpha \beta}(k) & \Lambda_{\alpha \alpha}(k) \end{bmatrix} \tag{7}$$b_{m}(k) = \begin{bmatrix} b_{\beta \beta}(k) \\ b_{\beta \alpha}(k) \end{bmatrix} = -\underset{(i,j) \in S_{m}}{\sum} J^{T}_{ij}(k)\Sigma_{ij}^{-1}r_{ij} \tag{8}$ 其中$(k)$代表在$k$時刻下誤差項對變量的雅克比，$S_{m}$代表所有的誤差邊，$\Lambda_{m}(k)$代表$k$時刻下的信息矩陣，根據之前的邊緣化操作，我們marg掉 $\xi_{1}$，信息矩陣的變換如下所示：

圖11. 邊緣化掉位姿1後信息矩陣變得稠密，原本條件獨立的位姿2-5也變得相關，新的位姿圖連接由左下角的圖關係變爲了右下角的圖關係

marg掉$\xi_{1}$後，$\xi_{1}$的測量信息傳遞給了剩餘的變量：
$b_{p}(k) = b_{\beta \alpha}(k)-\Lambda_{\alpha \beta}(k) \Lambda_{\beta \beta}^{-1}(k) b_{\beta \beta}(k) \tag{9}$ $\Lambda_{p}(k) = \Lambda_{\alpha \alpha}(k) - \Lambda_{\alpha \beta}(k) \Lambda_{\beta \beta}^{-1}(k) \Lambda_{\beta \alpha}(k) \tag{10}$ 其中小標$p$表示prior (先驗) 。

---

實際上， 我們可以從$b_{p}(k)$, $\Lambda_{p}(k)$分解出一個殘差$r_{p}(k)$和對應的雅克比矩陣$J_{p}(k)$，因爲$b_{p}(k)$, $\Lambda_{p}(k)$也服從$\Lambda_{p}(k) \delta \xi = J_{p}(k)^{T}\Sigma^{-1}J_{p}(k)= -J_{p}^{T}\Sigma^{-1}r_{p}(k) = b_{p}(k)$，其中$\delta \xi$爲marg掉$\xi_{1}$剩餘的變量。

需要注意的是，由於剩餘變量的不斷優化，殘差$r_{p}(k)$或者$b_{p}(k)$會跟着變化，但是雅克比$J_{p}(k)$取固定不變了。

---

當引入新的位姿$\xi_{7}$時，假如$\xi_{7}$與$\xi_{1}$有共同的觀測因此可以構建新的殘差項，則各個相機的位姿圖關係及其信息矩陣的變化如下：

圖12. 引入新的相機位姿7後各個相機的圖關係

圖13. 引入新的相機位姿7後信息矩陣的傳遞，其中位姿2對應的信息矩陣由新的信息矩陣和新引入的誤差項得到的信息矩陣組成

在$k'$時刻，由於引入了新的位姿$\xi_{7}$，因此新的殘差$r_{27}$和先驗信息$b_{p}(k)$, $\Lambda_{p}(k)$構建新的最小二乘問題: $b(k')=\Pi^{T}J_{p}(k)r_{p}(k') - J_{2,7}^{T}(k')\Sigma_{2,7}^{-1}r_{2,7}(k') \tag{11}$ $\Lambda(k') = \Pi^{T}\Lambda_{p}(k)\Pi + J_{2,7}^{T}(k')\Sigma_{2,7}^{-1}J_{2,7}(k') \tag{12}$ 其中$\Pi=[I_{dim J_{p}(k)}, \ 0]$ 用來將矩陣的維度擴張，$J_{p}(k)$爲先驗部分對應的雅克比矩陣，$r_{2,7}(k')$和$J_{2,7}(k')$分別表示新殘差和新殘差$r_{27}$對姿態$\xi{2}$和$\xi{7}$的雅克比矩陣。

1. 新測量信息和舊測量信息構建的增量方程的解會存在什麼問題？

• 由於被marg的變量 $\xi_{1}$ 以及對應的測量已被丟棄，先驗信息$\Lambda_{p}(k)$中關於位姿$\xi_{2}$、$\xi_{3}$、$\xi_{4}$和$\xi_{5}$在後續求解中沒法更新，而對於$\xi_{6}$是可以更新的，因爲在marg掉$\xi_{1}$之前，$\xi_{1}$和$\xi_{6}$並沒有產生共同觀測。
• 而$\xi_{2}$ (實際中可能有多個情況) 與新引入的$\xi_{7}$產生了新的觀測，這意味着新的殘差$r_{27}$對$\xi_{2}$的雅克比是在$k'$時刻下$\xi_{2}$的位姿處進行線性化的。
• 因此在滑動窗口優化的時候，信息矩陣如公式(12)是由倆部分組成的，而且這倆部分計算雅克比時的線性化點不同 (在這個例子中體現在$\xi_{2}$處線性化的地方不同 )，這可能會導致信息矩陣的零空間發生變化，從而在求解時引入錯誤信息。

2. SLAM系統中增量方程中的信息矩陣會存在零空間的情況？

單目 SLAM 系統 7 自由度不可觀： 6 自由度姿態 (單目SLAM系統沒有對齊東北天座標系，因此整條SLAM的軌跡可移動) + 尺度 (單目SLAM存在尺度不確定性)。

單目 + IMU 系統是 4 自由度不可觀： yaw 角 + 3 自由度位置不可觀。roll 和 pitch 由於重力向量的存在而可觀 (由於重力向量的存在，可知道軌跡與東北天座標系在roll 和 pitch的相對方向，由於yaw可以繞重力向量旋轉，故yaw不客觀)，尺度因子由於加速度計的存在而可觀。

3. 爲什麼雅克比線性化點不同會導致零空間發生變化？
這個時候要拿出這張廣爲流傳的圖了(感謝泡泡機器人)：

• 四張能量圖中，第一張是說明能量函數 E 由兩個同樣的非線性函數 $E_{1}$ 和 $E_{2}$ 組成，我們令函數 $E=0$，這時方程的解爲 $xy=1$，對應圖中深藍色的一條曲線。第二張能量函數圖中的$E_{1}'$對應函數 $E_{1}$ 在點$(0.5,1.4)$處的二階泰勒展開，第三張能量函數圖中的$E_{2}'$對應函數 $E2$ 在點$(1.2,0.5)$處的二階泰勒展開。注意這兩個近似的能量函數$E_{2}'$和$E_{1}'$是在不同的線性點附近對原函數展開得到的。最後一張圖就是把這個近似得到的能量函數合併起來，對整個系統 $E$ 的二階近似。
• 從第四個能量函數圖中，我們發現一個大問題，能量函數爲 0 的解由以前的一條曲線變成了一個點，不確定性的東西變得確定了，專業的術語叫不可觀的狀態變量變得可觀了，說明我們人爲的引入了錯誤的信息。 這個實驗的實質在於，在不同的點線性化後，強行加起來，實際上引入了一些人爲的約束，或者說引入了人爲的“錯誤觀測”，導致整個系統的崩潰。
• 對應到 marg 的問題上，本來我們是在最初(initial)那個點附近進行線性化，但是在 marg 的過程， initial 那個點變了，它一開始是有未 marg 的點的， marg 之後，把那些點的信息給了留下的那些點，這就使得剩下那些點進行了一些偏移， 他們和之前的狀態不同了，這個時候再線性化，就會導致在不同的地方進行了線性化，這樣就會像上面那個例子一樣，引入了錯誤的信息，導致整個優化過程的崩潰。因此， marg 時，被 marg 的那些變量的雅克比已經不更新了，而此時留在滑動窗口裏的其他變量的雅克比要用和 marg 時一樣的線性點，就是 FEJ去算，不要用新的線性點了。

最後引出解決方法FEJ：
FEJ 算法：不同殘差對同一個狀態求雅克比時，線性化點必須一致，這樣就能避免零空間退化而使得不可觀變量變得可觀。在上面的例子中我們計算$r_{27}$對姿態$\xi_{2}$的雅克比時，線性化點必須和$r_{12}$對齊求導一致。

但是， VINS 中並未使用 FEJ 的策略， 這裏我們進行簡要說明： 對於滑窗內剩餘的優化變量， 如倒數第二幀位姿 T1，當邊緣化掉最老幀 T0 後，會給 T1 加上新的約束。值得注意的是， 這個新約束的線性化點是在 marg 掉 T0 時刻的 T1 的值，而當之後 T1 迭代更新後，該marg 產生的約束並不會調整線性化點，即不會在新的 T1 處重新展開，這樣會導致兩次的線性化點不一致。 但據作者描述因未發現明顯的 yaw 不可觀性變化導致的軌跡漂移， 因此並未採用 FEJ 策略，反而加入 FEJ 後導致結果不佳。