边缘化 (Marginalization)，简单来说也就是删掉某些东西但是仍保存它的影响。比如最常见的求解 BA 的过程中，求解 H 矩阵时利用它的稀疏性进行的舒尔消元，就是把路标点 “忽略掉” 先求出相机的共视矩阵。这类 “忽略掉” 的操作就是所谓的 “边缘化”。

本文参考有：

• 崔华坤老师讲解SLAM的PPT。
• 高翔《视觉SLAM十四讲》。
• 博客链接均有注明。

VINS-Mono 中的边缘化误差项

问题提出

$\begin{array}{c} \min _{\mathcal{X}}\left\{\left\|\mathbf{r}_{p}-\mathbf{H}_{p} \mathcal{X}\right\|^{2}+\sum_{k \in \mathcal{B}}\left\|\mathbf{r}_{\mathcal{B}}\left(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{b_{k+1}}^{b_{k}}}^{2}+\right. \\ \left.\sum_{(l, j) \in \mathcal{C}} \rho\left(\left\|\mathbf{r}_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{l}^{c_{j}}}^{2}\right)\right\} \end{array}$

上述式子是 VINS-Mono 的 Local BA 的总的优化的目标函数。第二项是 IMU 的观测误差（预积分而得），第三项是相机的观测误差（球面模型），而第一项，是边缘化操作带来的误差项。

因子图

在找关于边缘化相关资料的过程中，发现很多通过因子图来解释和理解，后来觉得那因子图来说更容易理解。所以在 SLAM 十四讲讲的一点因子图相关的基础之上，又补了一些关于因子图的相关知识。下面是简要的一些要点。

---

该分割线内内容来自《SLAM 十四讲》

因子图是一种无向图，由两种节点组成：表示优化变量的变量节点，以及表示因子的因子节点。

在因子图的表述下，传统的 SLAM 过程可以由第一张图的表述方式转换第二张图：

一个更加实际的较完整的 slam 系统：

先验因子如 GPS、IMU 等。

---

上面是十四讲中的一些基本概念。十四讲中提到可以拿因子图的增量特性来做一些文章等。在平时，因子图还可以帮助更好的分析增量方程的 H 矩阵中的各项的组成和之间的关系。下面几张图很好，来自于深蓝学院崔华坤老师的公开课。这样的话就可以把变量之间的约束和 H 矩阵之间关联起来。（PPT 里有个 bug，那个不是 Hession 矩阵，而是增量方程的那个 $H=J^TJ$ 的 H 矩阵。Hession 矩阵是二阶偏导）

一个红色的块就是一项约束，三个变量由一项约束连接起来，就可以化作最右边的两两相连的图，然后就可以对应到 H 矩阵中的各项。

带 IMU 约束的，单个的可以这样表示：

多个观测可以表示成这个样子：

顺便用因子图来表示一下松耦合和紧耦合之间最主要的区别：

下面用因子图来分析 VINS-Mono 的边缘化操作过程。

VINS-Mono 的边缘化操作

VINS 边缘化掉滑动窗口中的最老帧或者次新帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其他帧的约束关系。BA 中为加速求解而边缘化掉路标点，VINS 中的边缘化则类比起来，可以理解为为了求滑窗中的有效帧而边缘化掉最老帧。边缘化掉最老帧对系统的影响可以由因子图来比较形象的解释和帮助理解。

边缘化操作的作用

首先先看有篇 博客 里总结边缘化这种做法的好处，总结的挺好:

边缘化的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为将联合概率分布分解为边缘概率分布和条件概率分布的过程 (说白了，就是利用 shur 补减少优化参数的过程)。利用 Sliding Window 做优化的过程中，边缘化的目的主要有两个：

• 为了使得计算量维持一个稳定的量级，滑窗内的 pose 和 feature 个数是有限的，在系统优化的过程中，势必要不断将一些 pose 和 feature 移除滑窗。

• 如果当前帧图像和上一帧添加到滑窗的图像帧视差很小，则测量的协方差 (重投影误差) 会很大，进而会恶化优化结果。LIFO 导致了协方差的增大，而恶化优化结果

  这一点待讨论，因为 VINS 把东西加入到滑窗进行优化的不都是 keyframe 吗？vins 在选择 keyframe 的时候考虑了视差，若视差很小的话根本就不会加入到滑窗中进行优化。后面看到博客作者的理解，他是把新帧但是不满足条件然后删掉也当做边缘化了，但我觉得边缘化的操作应该是删掉并考虑先验概率。vins 的文章中正说明了两者的不一样：

  • 次新帧为 keyframe，边缘化最老帧
  • 次新帧不为 keyframe，去掉次新帧

  

直接进行删除操作而不考虑先验概率的后果：

• 无故地移除这些 pose 和 feature 会丢弃帧间约束，会降低了优化器的精度，所以在移除 pose 和 feature 的时候需要将相关联的约束转变为一个先验的约束条件作为 prior 放到优化问题中
• 在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失 (参考 [6])，尤其是系统在进行__退化运动__时 (如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为 0 的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的客观性。

边缘化操作的因子图理解

首先，现将问题建模成因子图形式：

先求出共视矩阵，相当于先把路标点给边缘化掉不考虑，然后再考虑边缘化掉最老帧对系统的影响。

(有张图暂时找不到了… 能非常直观的从因子图角度看出来边缘化掉最老帧后新增的约束因子，暂时先贴上这个不是因子图的图，大概也能看出来一些。上面为位姿下面是特征。其中 ${x_p}_1$ 被边缘化掉，所产生的影响跟预料的不太一样。等找到因子图的解释那张图后理解起来就会非常直观)

边缘化的一大利器：Schur 补公式。

关于为何会产生上述的变化，可以由因子图来解释：

有了上面 schur 补的公式：

$\left (\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right) \delta x_{b}= g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}$

令等式的左边为 $H$，右边为 $g$，上述式子就是关于 $\delta x_{b}$ 的新的带有先验信息的关系式，然后将这部分 带入到代价函数的边缘化残差中即 可。也就是说由边缘化得到了条件概率：

$p\left (\delta x_{b} | \delta x_{a}\right) \sim N\left (-\Lambda_{b}^{T} \Lambda_{a}^{-1},\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right)$

Sibley G. A Sliding Window Filter for SLAM. University of Southern California, 2006.: SLAM is tracking a noraml distribution through a large state space

FEJ

关于这点是边缘化操作的深入讨论。说实话我现在还没有完全理解，这里先贴一些东西吧：

迭代中的 consistency（一致性）的讨论

这篇 博客：

虽然上面这些总结在 marg 的过程中很重要，但是我认为更重要的是关于 marg 过程中 consistency 的讨论。dso 论文中提到计算 H 矩阵的雅克比时用 FEJ (first estimate jacobian) [5]，okvis 论文中也提到要:

fix the linearization point around x0x0, the value of xx at the time of marginalization.

因为迭代过程中，状态变量会被不断更新，计算雅克比时我们要 fix the linearization point。 所谓 linearization point 就是线性化时的状态变量，即求雅克比时的变量，因为计算雅克比是为了泰勒展开对方程进行线性化。我们要固定在点 x0 (marg 时的状态变量) 附近去计算雅克比，而不是每次迭代更新以后的 x。[7] 是 2016 年 IROS 的论文，对这个原因表述的很清楚也容易阅读（套用张腾大神的话包教包会，感谢他的推荐）， [6][5] 是两篇年代更久一点的论文，这两篇论文都很有裨益，但是难读，单单这个 consistency 分析就值得仔细去看看，因为它直接涉及到优化结果。

为了更直观的理解上述这个结果，“泡泡机器人” 里部分成员对这个过程进行了讨论，TUM 的杨楠请教了其师兄 DSO 作者 Engel。Engel 用一张图就解释了这个过程：

在刘毅 (稀疏毅)，王京，晓佳等人讨论下，对这张图作出了如下解释：四张能量图中，第一张是说明能量函数 EE 由两个同样的非线性函数 E1E1 和 E2E2 组成，我们令函数 E=0E=0，这时方程的解为 x**y=1xy=1，对应图中深蓝色的一条曲线。第二张能量函数图中的 E′1E1′对应函数 E1E1 在点 (0.5,1.4) 处的二阶泰勒展开，第三张能量函数图中的 E′2E2′对应函数 E2E2 在点 (1.2,0.5) 处的二阶泰勒展开。注意这两个近似的能量函数 E′1E1′和 E′2E2′是在不同的线性点附近对原函数展开得到的。最后一张图就是把这个近似得到的能量函数合并起来，对整个系统 EE 的二阶近似。从第四个能量函数图中，我们发现一个大问题，能量函数为 0 的解由以前的一条曲线变成了一个点，不确定性的东西变得确定了，专业的术语叫不可观的状态变量变得可观了，说明我们人为的引入了错误的信息。回到 marg 过程，上面这个例子告诉我们，marg 时，被 marg 的那些变量的雅克比已经不更新了，而此时留在滑动窗口里的其他变量的雅克比要用和 marg 时一样的线性点，就是 FEJ 去算，不要用新的线性点了。有了这些基础以后，大家可以再去看看王京，张腾大神们在知乎上关于 FEJ 更理论的表述，链接请戳。

至此，VINS 中的边缘化操作就可以告一段落了。

总结来说，借鉴贺一家博士的说法：

滑窗的三大法宝 “Marginalization”，“Schur complement”，"First estimate jacobin"