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=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$变成上三角或者下三角形过程中，会用到舒尔补操作：$$\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& {\Delta }_A\end{array}\right]$$ $$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]=\left[\begin{array}{cc}A& 0\\ C& {\Delta }_A\end{array}\right]$$ 其中$A$为可逆矩阵，${\Delta }_A=D-C{A}^{-1}B$，称为A关于M的舒尔补，联合起来，将M变形成对角矩阵：$$\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]=\left[\begin{array}{cc}A& 0\\ 0& {\Delta }_A\end{array}\right]$$ 反过来又能从对角矩阵恢复成矩阵$M$：$$\left[\begin{array}{cc}I& 0\\ C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& 0\\ 0& {\Delta }_A\end{array}\right]\left[\begin{array}{cc}I& A^{-1}B\\ 0& I\end{array}\right]=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$$ 以上变换均建立在矩阵$A$可逆的前提下，如果矩阵$A$不可逆而矩阵$D$可逆，同样可以进行舒尔补操作将$M$矩阵变为上三角或者下三角的形式： $$\left[\begin{array}{cc}I& -B{D}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}{\Delta }_D& 0\\ C& D\end{array}\right]$$ 其中${\Delta }_D=A-B{D}^{-1}C$，称为$D$关于$M$的舒尔补。

舒尔补操作在实现将矩阵$M$分解为上三角下三角形式的同时，也实现了矩阵M的快速求逆：$$M=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}I& 0\\ C{A}^{-1}& I\end{array}\right]\left[\begin{array}{cc}A& 0\\ 0& {\Delta }_A\end{array}\right]\left[\begin{array}{cc}I& A^{-1}B\\ 0& I\end{array}\right]$$ 求逆可得：$$M^{-1}={\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_A^{-1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]$$ 其中 $$\left[\begin{array}{cc}I& -A^{-1}B\\ 0& I\end{array}\right]\left[\begin{array}{cc}I& A^{-1}B\\ 0& I\end{array}\right]=I$$

2.2 多元高斯分布下的舒尔补分解：边际分布 (marginal distribution) 及条件分布

假设多元变量服从零均值高斯分布$P(\alpha ,\beta )$，且由俩部分组成：$\mathbf{x}=\left[\begin{array}{c}\alpha \\ \beta \end{array}\right]$，变量之间构成的协方差矩阵为：$$K=\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]\text{\hspace{1em}(1)}$$ 其中$A=cov(\alpha ,\alpha )$，$D=cov(\beta ,\beta )$，$C=cov(\alpha ,\beta )$，由此变量$x$的分布为：$$\begin{gathered} P(\alpha ,\beta )=P(\alpha )P(\beta |\alpha )\propto exp(-\frac{1}{2}{\left[\begin{array}{c}\alpha \\ \beta \end{array}\right]}^T{\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{-1}\left[\begin{array}{c}\alpha \\ \beta \end{array}\right]) \\ \propto exp(-\frac{1}{2}{\left[\begin{array}{c}\alpha \\ \beta \end{array}\right]}^T\left[\begin{array}{cc}I& -A^{-1}{C}^T\\ 0& I\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_A^{-1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ -C{A}^{-1}& I\end{array}\right]\left[\begin{array}{c}\alpha \\ \beta \end{array}\right]) \\ \propto exp(-\frac{1}{2}\left[\begin{array}{cc}{\alpha }^T& (\beta -C{A}^{-1}\alpha {)}^T\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& {\Delta }_A^{-1}\end{array}\right]\left[\begin{array}{cc}\alpha & \beta -C{A}^{-1}\alpha \end{array}\right]) \\ \propto exp(-\frac{1}{2}({\alpha }^T{A}^{-1}\alpha )+(\beta -C{A}^{-1}\alpha {)}^T{\Delta }_A^{-1}(\beta -C{A}^{-1}\alpha )) \\ \propto \underset{P(\alpha )}{exp(-\frac{1}{2}{\alpha }^T{A}^{-1}\alpha )}\underset{P(\beta |\alpha )}{exp(-\frac{1}{2}(\beta -C{A}^{-1}\alpha {)}^T{\Delta }_A^{-1}(\beta -C{A}^{-1}\alpha ))}\text{\hspace{1em}(2)} \end{gathered}$$
意味着通过舒尔补操作能从高斯联合分布$P(\alpha ,\beta )$分解出边际分布$P(\alpha )$ (边缘掉了$\beta$)和条件分布$P(\beta |\alpha )$。可以看出边际分布的协方差为$A$，即为从联合分布中取对应的矩阵块，而条件分布的协方差为${\Delta }_A$，即$A$对应的舒尔补$D-C{A}^{-1}B$，均值也变了。

在SLAM的优化问题中，我们往往操作的是信息矩阵，而不是协方差矩阵，因此需要计算边际分布和条件分布对应的信息矩阵：
对于高斯联合分布$P(a,\beta )$的信息矩阵，由于信息矩阵等于协方差矩阵的逆，根据公式$(1)(2)$我们有：$${\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}{A}^{-1}+A^{-1}{C}^T{\Delta }_A^{-1}C{A}^{-1}& -A^{-1}{C}^T{\Delta }_A^{-1}\\ -C{A}^{-1}\Delta A^{-1}& {\Delta }_A^{-1}\end{array}\right]=\left[\begin{array}{cc}{\Lambda }_{\alpha \alpha }& {\Lambda }_{\alpha \beta }\\ {\Lambda }_{\beta \alpha }& {\Lambda }_{\beta \beta }\end{array}\right]$$ 由于条件分布$P(\beta |\alpha )$和边际分布$P(\alpha )$的协方差矩阵分别为${\Delta }_A$、$A$，故其信息矩阵分别为：$${\Delta }_A^{-1}={\Lambda }_{\beta \beta },A^{-1}={\Lambda }_{\alpha \alpha }-{\Lambda }_{\alpha \beta }{\Lambda }_{\beta \beta }^{-1}{\Lambda }_{\beta \alpha }\text{\hspace{1em}(3)}$$

对于非零矩阵的高斯分布$P(\alpha ,\beta )=\mathcal{N}(\left[\begin{array}{c}{\mu }_{\alpha }\\ {\mu }_{\beta }\end{array}\right],\left[\begin{array}{c}{\Sigma }_{\alpha \alpha },{\Sigma }_{a\beta }\\ {\Sigma }_{\beta \alpha },{\Sigma }_{\beta \beta }\end{array}\right])={\mathcal{N}}^{-1}(\left[\begin{array}{c}{\eta }_{\alpha }\\ {\eta }_{\beta }\end{array}\right],\left[\begin{array}{cc}{\Lambda }_{\alpha \alpha }& {\Lambda }_{\alpha \beta }\\ {\Lambda }_{\beta \alpha }& {\Lambda }_{\beta \beta }\end{array}\right])$，

分解出的边际分布 (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$矩阵具备稀疏性，其结构一般如下：

构建的方程为：$$\left[\begin{array}{cc}{\Lambda }_a& {\Lambda }_b\\ {\Lambda }_b^T& \Lambda c\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{g}_a\\ g_b\end{array}\right]\text{\hspace{1em}(4)}$$ 假如这里我们要边缘化掉$\delta x_b$，通过舒尔补对上面方程进行消元得：$$\left[\begin{array}{cc}{\Lambda }_a-{\Lambda }_b{\Lambda }_c^{-1}{\Lambda }_b^T& 0\\ {\Lambda }_b^T& {\Lambda }_c\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{g}_a-{\Lambda }_b{\Lambda }_c^{-1}{g}_b\\ g_b\end{array}\right]$$ 此时关于$\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)$ 进行消元得：$$\left[\begin{array}{cc}{\Lambda }_a& {\Lambda }_b\\ 0& {\Lambda }_c-{\Lambda }_b^T{\Lambda }_a^{-1}{\Lambda }_b\end{array}\right]\left[\begin{array}{c}\delta x_a\\ \delta x_b\end{array}\right]=\left[\begin{array}{c}{g}_a\\ g_b-{\Lambda }_b^T{\Lambda }_a^{-1}{g}_a\end{array}\right]$$ 此时关于$\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)\text{\hspace{1em}(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)类似的矩阵等式: $$\left[\begin{array}{cc}{{\Lambda }_a}^{6\times 6}& {{\Lambda }_b}^{6\times 36}\\ {{\Lambda }_b^T}^{36\times 6}& {\Lambda c}^{36\times 36}\end{array}\right]\left[\begin{array}{c}{\delta x_a}^{6\times 1}\\ {\delta x_b}^{36\times 1}\end{array}\right]=\left[\begin{array}{c}{g_a}^{6\times 1}\\ {g_b}^{36\times 1}\end{array}\right]\text{\hspace{1em}(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)=\sum _{(i,j)\in S_m}{J}_{ij}^T(k){\Sigma }_{ij}^{-1}{J}_{ij}(k)=\left[\begin{array}{cc}{\Lambda }_{\beta \beta }(k)& {\Lambda }_{\beta \alpha }(k)\\ {\Lambda }_{\alpha \beta }(k)& {\Lambda }_{\alpha \alpha }(k)\end{array}\right]\text{\hspace{1em}(7)}$$$$b_m(k)=\left[\begin{array}{c}{b}_{\beta \beta }(k)\\ b_{\beta \alpha }(k)\end{array}\right]=-\sum _{(i,j)\in S_m}{J}_{ij}^T(k){\Sigma }_{ij}^{-1}{r}_{ij}\text{\hspace{1em}(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)\text{\hspace{1em}(9)}$$ $${\Lambda }_p(k)={\Lambda }_{\alpha \alpha }(k)-{\Lambda }_{\alpha \beta }(k){\Lambda }_{\beta \beta }^{-1}(k){\Lambda }_{\beta \alpha }(k)\text{\hspace{1em}(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^{\prime }$时刻，由于引入了新的位姿${\xi }_7$，因此新的残差$r_{27}$和先验信息$b_p(k)$, ${\Lambda }_p(k)$构建新的最小二乘问题: $$b(k^{\prime })={\Pi }^T{J}_p(k)r_p(k^{\prime })-J_{2,7}^T(k^{\prime }){\Sigma }_{2,7}^{-1}{r}_{2,7}(k^{\prime })\text{\hspace{1em}(11)}$$ $$\Lambda (k^{\prime })={\Pi }^T{\Lambda }_p(k)\Pi +J_{2,7}^T(k^{\prime }){\Sigma }_{2,7}^{-1}{J}_{2,7}(k^{\prime })\text{\hspace{1em}(12)}$$ 其中$\Pi =[I_{dim{J}_p(k)},0]$ 用来将矩阵的维度扩张，$J_p(k)$为先验部分对应的雅克比矩阵，$r_{2,7}(k^{\prime })$和$J_{2,7}(k^{\prime })$分别表示新残差和新残差$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^{\prime }$时刻下${\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^{\prime }$对应函数 $E_1$ 在点$(0.5,1.4)$处的二阶泰勒展开，第三张能量函数图中的$E_2^{\prime }$对应函数 $E2$ 在点$(1.2,0.5)$处的二阶泰勒展开。注意这两个近似的能量函数$E_2^{\prime }$和$E_1^{\prime }$是在不同的线性点附近对原函数展开得到的。最后一张图就是把这个近似得到的能量函数合并起来，对整个系统 $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 后导致结果不佳。