VIINS源码解读 -- 第 13 篇 · 边缘化原理（上）- 舒尔补与先验构建

· 本篇你将学到

一、为何需要边缘化

第 10 篇 established：滑动窗口固定大小（WINDOW_SIZE=10，11 帧），新帧入窗必须丢一帧。问题是–丢掉的帧携带的信息怎么办？

考虑要丢掉最老帧 $f_0$。$f_0$ 与窗口内其他帧的关联：

• IMU 预积分：$f_0\to f_1$ 的预积分约束 $f_0$ 与 $f_1$ 的状态（第 11 篇）；
• 视觉共视：$f_0$ 观测的某些路标，可能在 $f_2,f_3,\dots$ 仍被观测（第 12 篇）；
• 上轮先验：上一轮边缘化留下的先验约束也可能涉及 $f_0$。

若直接删 $f_0$ 的状态变量，这些约束的参数块少了一个，残差无法计算–约束失效。等于白白丢弃了 $f_0$ 观测时段的全部信息，窗口越滑信息越少，估计退化。

边缘化（marginalization）的解法：不直接删，而是用边际概率把 $f_0$ 的变量"积分掉"，把它对剩余变量的影响浓缩成一个先验约束，挂回 BA。这样信息不丢，窗口又能滑动。

类比：联合概率 $p(x_0,x_1,\dots ,x_n)$ 里积分掉 $x_0$ 得边际 $p(x_1,\dots ,x_n)=\int p(x_0,\dots ,x_n)d{x}_0$。$x_0$ 虽消失，但它对 $x_1,\dots ,x_n$ 的影响保留在边际分布里。边缘化即此操作的线性高斯版本。

与 BA 求解中舒尔补的区别

BA 求解时也用舒尔补（如 DENSE_SCHUR，第 15 篇）–那是求解技巧：消去路标变量加速方程求解，路标事后回代，变量不丢。边缘化的舒尔补是概率操作：积分掉变量，变量永久消失，只剩先验。两者数学同源（舒尔补），目的不同。

二、丢最老帧 vs 丢次新帧

第 4 篇关键帧判据决定 marginalization_flag，对应两种丢帧策略，边缘化处理截然不同：

2.1 MARGIN_OLD（关键帧，丢最老帧）

当前帧视差大/信息足，是关键帧，应保留。丢窗口最老帧 $f_0$，其余左移，新帧入末尾（第 10 篇图）。

边缘化对象：$f_0$ 的状态 $[p_0,q_0,v_0,b_{a,0},b_{g,0}]$（16 维局部）+ $f_0$ 独有的路标（仅 $f_0$ 观测、窗口内无其他帧共视的）。这些变量要被积分掉，信息经舒尔补转成对保留变量（$f_1\dots f_{10}$ + 共视路标）的先验。

→ 触发完整舒尔补边缘化，更新 last_marginalization_info（本轮先验）。

2.2 MARGIN_SECOND_NEW（非关键帧，丢次新帧）

当前帧与上一帧重复（静止/纯旋转），是非关键帧。直接丢掉次新帧 $f_9$（上一帧），新帧替换其位置。

为何不边缘化：次新帧 $f_9$ 与最新帧 $f_{10}$ 几乎重复观测同一场景，$f_9$ 的约束可被 $f_{10}$ 完全替代–丢弃 $f_9$ 不损失信息。处理方式：把 $f_9\to f_{10}$ 的 IMU 预积分合并进 $f_{10}$ 的预积分，$f_9$ 状态直接删除，不调用舒尔补。

→ 不生成新的边缘化先验，沿用上一轮 last_marginalization_info。

2.3 对比

关键结论：边缘化先验只在丢最老帧时更新。丢次新帧是无损的"窗口压缩"。这解释了为何静止场景下（频繁 MARGIN_SECOND_NEW）先验长期不变。

三、边缘化的概率基础：边际分布

3.1 联合信息矩阵

把所有与被边缘化变量相关的残差块（IMU 残差、视觉残差、上轮先验）聚合。每个残差 $r_i$ 对状态 $x=[x_m;x_r]$（$x_m$ 边缘化变量、$x_r$ 保留变量）有雅可比 $J_i$，信息加权后贡献信息矩阵：

$$H=\sum _i{J}_i^{\top }{\Sigma }_i^{-1}{J}_i=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right],b=\sum _i{J}_i^{\top }{\Sigma }_i^{-1}{r}_i=\left[\begin{array}{c}{b}_m\\ b_r\end{array}\right]$$

其中 $H_{mm}$（$m\times m$）是边缘化变量块，$H_{rr}$（$n\times n$）是保留变量块，$H_{mr}=H_{rm}^{\top }$ 是耦合块。

3.2 边际分布

联合高斯 $p(x_m,x_r)\propto \exp (-\frac{1}{2}\Vert x-\hat{x}{\Vert }_H^2)$。边缘化 $x_m$ 即积分：

$$p(x_r)=\int p(x_m,x_r)d{x}_m$$

高斯分布的边际仍是高斯。其信息矩阵与残差由舒尔补给出（下一节）。

推导留白：边际分布的均值/协方差推导见 Hager 或 Kummerle 的因子图教程。本篇直接用结论：边际信息矩阵 = 舒尔补。

四、舒尔补详解

4.1 舒尔补公式

对分块矩阵 $H=[H_{mm},H_{mr};H_{rm},H_{rr}]$，消去 $x_m$ 后保留变量 $x_r$ 的边际信息矩阵：

$$\boxed{H_r=H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}}$$

$$b_r^{\prime }=b_r-H_{rm}{H}_{mm}^{-1}{b}_m$$

$H_r$ 即保留变量的边际信息矩阵，$b_r^{\prime }$ 是对应的边际残差向量。边际分布 $p(x_r)\propto \exp (-\frac{1}{2}(x_r-{\hat{x}}_r{)}^{\top }{H}_r(x_r-{\hat{x}}_r))$。

4.2 舒尔补分块图

$$\underset{\text{联合信息矩阵 }H}{\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]}\stackrel{\text{舒尔补消去 }{x}_m}{}\underset{\text{边际信息矩阵 }{H}_r}{H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}}$$

各块含义：

图 1 · 舒尔补流程（边缘化与先验构建）

联合信息矩阵 H 按保留/边缘化变量分块，经舒尔补消去边缘化变量 $x_m$ 得到保留变量的边际信息矩阵 $H_r$，再转成「雅可比+残差」先验因子挂回 BA。图中配色区分三类块：绿色=保留块 $H_{rr}$、橙色=边缘化块 $H_{mm}$、黄色=交叉块 $H_{mr}/H_{rm}$。

#mermaid-svg-hYSol0AwXFyf70Ph{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-hYSol0AwXFyf70Ph .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-hYSol0AwXFyf70Ph .error-icon{fill:#552222;}#mermaid-svg-hYSol0AwXFyf70Ph .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-hYSol0AwXFyf70Ph .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-hYSol0AwXFyf70Ph .marker{fill:#333333;stroke:#333333;}#mermaid-svg-hYSol0AwXFyf70Ph .marker.cross{stroke:#333333;}#mermaid-svg-hYSol0AwXFyf70Ph svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-hYSol0AwXFyf70Ph p{margin:0;}#mermaid-svg-hYSol0AwXFyf70Ph .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-hYSol0AwXFyf70Ph .cluster-label text{fill:#333;}#mermaid-svg-hYSol0AwXFyf70Ph .cluster-label span{color:#333;}#mermaid-svg-hYSol0AwXFyf70Ph .cluster-label span p{background-color:transparent;}#mermaid-svg-hYSol0AwXFyf70Ph .label text,#mermaid-svg-hYSol0AwXFyf70Ph span{fill:#333;color:#333;}#mermaid-svg-hYSol0AwXFyf70Ph .node rect,#mermaid-svg-hYSol0AwXFyf70Ph .node circle,#mermaid-svg-hYSol0AwXFyf70Ph .node ellipse,#mermaid-svg-hYSol0AwXFyf70Ph .node polygon,#mermaid-svg-hYSol0AwXFyf70Ph .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-hYSol0AwXFyf70Ph .rough-node .label text,#mermaid-svg-hYSol0AwXFyf70Ph .node .label text,#mermaid-svg-hYSol0AwXFyf70Ph .image-shape .label,#mermaid-svg-hYSol0AwXFyf70Ph .icon-shape .label{text-anchor:middle;}#mermaid-svg-hYSol0AwXFyf70Ph .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-hYSol0AwXFyf70Ph .rough-node .label,#mermaid-svg-hYSol0AwXFyf70Ph .node .label,#mermaid-svg-hYSol0AwXFyf70Ph .image-shape .label,#mermaid-svg-hYSol0AwXFyf70Ph .icon-shape .label{text-align:center;}#mermaid-svg-hYSol0AwXFyf70Ph .node.clickable{cursor:pointer;}#mermaid-svg-hYSol0AwXFyf70Ph .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-hYSol0AwXFyf70Ph .arrowheadPath{fill:#333333;}#mermaid-svg-hYSol0AwXFyf70Ph .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-hYSol0AwXFyf70Ph .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-hYSol0AwXFyf70Ph .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-hYSol0AwXFyf70Ph .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-hYSol0AwXFyf70Ph .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-hYSol0AwXFyf70Ph .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-hYSol0AwXFyf70Ph .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-hYSol0AwXFyf70Ph .cluster text{fill:#333;}#mermaid-svg-hYSol0AwXFyf70Ph .cluster span{color:#333;}#mermaid-svg-hYSol0AwXFyf70Ph div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-hYSol0AwXFyf70Ph .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-hYSol0AwXFyf70Ph rect.text{fill:none;stroke-width:0;}#mermaid-svg-hYSol0AwXFyf70Ph .icon-shape,#mermaid-svg-hYSol0AwXFyf70Ph .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-hYSol0AwXFyf70Ph .icon-shape p,#mermaid-svg-hYSol0AwXFyf70Ph .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-hYSol0AwXFyf70Ph .icon-shape rect,#mermaid-svg-hYSol0AwXFyf70Ph .image-shape rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-hYSol0AwXFyf70Ph .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-hYSol0AwXFyf70Ph .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-hYSol0AwXFyf70Ph :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}#mermaid-svg-hYSol0AwXFyf70Ph .keep>*{fill:#efe!important;stroke:#3c3!important;}#mermaid-svg-hYSol0AwXFyf70Ph .keep span{fill:#efe!important;stroke:#3c3!important;}#mermaid-svg-hYSol0AwXFyf70Ph .cross>*{fill:#ffe!important;stroke:#fc6!important;}#mermaid-svg-hYSol0AwXFyf70Ph .cross span{fill:#ffe!important;stroke:#fc6!important;}#mermaid-svg-hYSol0AwXFyf70Ph .margin>*{fill:#fee!important;stroke:#f88!important;}#mermaid-svg-hYSol0AwXFyf70Ph .margin span{fill:#fee!important;stroke:#f88!important;}

联合信息矩阵 H = Σ JiᵀΣ⁻¹Ji（残差聚合）

求 Hmm⁻¹

耦合注入

耦合注入

保留,减去注入

Hmm 边缘化块

Hmr 交叉块

Hrm 交叉块

Hrr 保留块

Hmm⁻¹特征分解+eps截断

舒尔补Hr = Hrr − Hrm·Hmm⁻¹·Hmr

Hr 边际信息矩阵(保留变量)

先验 p(xr)雅可比+残差

先验因子挂回BAMarginalizationFactor

4.3 几何直觉

$H_{rr}$ 是保留变量"自己"的信息，$H_{rm}{H}_{mm}^{-1}{H}_{mr}$ 是"经由边缘化变量传过来的耦合信息"。舒尔补把后者从前者中扣除–因为边缘化变量被积分掉后，它原本提供的独立约束不能再算全额（其不确定性已被边际化放大）。结果 $H_r$ 比 $H_{rr}$ 小，保留变量的不确定性略增（信息略减），但 $x_m$ 的影响被正确保留。

4.4 数值实现

$H_{mm}^{-1}$ 不显式求逆（数值不稳），而是对 $H$ 做 LDLT 分解，通过解方程组 $H_{mm}^{-1}{H}_{mr}$ 等完成舒尔补。VINS 还用多线程（NUM_THREADS=4，marginalization_factor.h:22）分块累加 $H$。具体代码第 14 篇精读 MarginalizationInfo::marginalize()。

五、先验表达：雅可比 + 残差

5.1 从边际高斯到先验残差

舒尔补得边际高斯 $p(x_r)\sim \mathcal{N}({\hat{x}}_r,H_r^{-1})$。但 Ceres 优化需要残差+雅可比形式，而非协方差。转换：

边际分布的对数似然（去掉常数）：

$$-\ln p(x_r)\propto \frac{1}{2}(x_r-{\hat{x}}_r{)}^{\top }{H}_r(x_r-{\hat{x}}_r)=\frac{1}{2}\Vert J_{\text{marg}}(x_r-{\hat{x}}_r){\Vert }^2$$

其中 $J_{\text{marg}}$ 是 $H_r$ 的 Cholesky 因子（$J_{\text{marg}}^{\top }{J}_{\text{marg}}=H_r$）。故先验残差：

$$\boxed{r_{\text{prior}}=e_0+J_{\text{marg}}(x_r-x_0)}$$

• $x_0$：边缘化时的线性化点（parameter_block_data）；
• $e_0$：边缘化时的残差（linearized_residuals）；
• $J_{\text{marg}}$：先验雅可比，由 $H_r$ Cholesky 得（linearized_jacobians）。

5.2 作为先验因子挂回 BA

MarginalizationFactor（marginalization_factor.h:85）封装这个先验，作为 Ceres CostFunction 挂回下次 BA：

• Evaluate：$r_{\text{prior}}=e_0+J_{\text{marg}}(\text{para}-x_0)$，雅可比即 $J_{\text{marg}}$（固定）；
• 参数块：所有保留变量（keep_block_data 对应的状态）。

于是下一轮 BA 的代价函数（第 11 篇总览）里多了 $\Vert r_{\text{marg}}{\Vert }^2$ 项，把历史信息软约束进来。

5.3 与第 11/12 篇残差的聚合

边缘化时聚合的残差块包括：

• 被边缘化帧相关的 IMU 残差（第 11 篇 IMUFactor）；
• 被边缘化帧观测的视觉残差（第 12 篇 ProjectionFactor）；
• 上轮先验（若涉及被边缘化帧）。

这些残差的雅可比/残差由 ResidualBlockInfo::Evaluate（marginalization_factor.h:29）计算，聚合进 $H$。drop_set（:34）标记每个残差块里哪些参数块属于 $x_m$（要丢）。第 14 篇详述。

六、FEJ：固定第一次估计雅可比

6.1 线性化不一致问题

边缘化先验在线性化点 $x_0$ 处展开，雅可比 $J_{\text{marg}}$ 是那里的导数。但下一轮 BA 迭代时，状态 $x$ 会更新偏离 $x_0$。若先验雅可比随当前 $x$ 重新计算（正常 BA 做法），会出现线性化不一致：

• 先验信息矩阵 $H_r$ 是在 $x_0$ 算的；
• 新残差雅可比是在新 $x$ 算的；
• 两者拼在同一优化里，信息矩阵的零空间（不可观测方向）会被错误"填充"–原本不可观测的方向（如全局 yaw）被先验错误约束，导致nullspace 膨胀、估计漂移。

6.2 FEJ（First Estimate Jacobian）

解法：先验因子的雅可比固定为边缘化时的 $J_{\text{marg}}$（第一次估计），不随迭代更新。即 MarginalizationFactor::Evaluate 永远返回同一个 $J_{\text{marg}}$（linearized_jacobians 常量），只有残差 $e_0+J_{\text{marg}}(x-x_0)$ 随 $x$ 线性变化。

这样所有残差（先验 + 新 IMU/视觉）的信息矩阵都在同一 $x_0$ 线性化，零空间一致，可观测性保持。

代价：FEJ 牺牲了二阶精度（雅可比不随 $x$ 更新），但保证了可观性一致性–对 SLAM 这种弱可观系统更重要。VINS 采用 FEJ。

6.3 何时失效

若状态偏离 $x_0$ 过远，FEJ 的线性化误差累积，先验变得不准。VINS 通过滑窗持续滑动（每丢一帧重新边缘化、重置 $x_0$）来限制这种漂移–先验"保质期"大致一个滑窗周期。

七、边缘化的代价与局限

工程权衡：边缘化是紧耦合 VIO 的"记忆"，让窗口外的信息持续发挥作用；但它是线性高斯近似的"记忆"，会失真、会累积。VINS 的设计在"保留信息"与"避免污染"间走钢丝–第 14 篇代码会看到大量为这个权衡服务的工程细节（valid 标志、drop_set 管理、线程化舒尔补等）。

小结与下一篇预告

本篇完成边缘化的理论铺垫：

下一篇（第 14 篇）：边缘化原理（下）- 代码实现与坑点。精读 marginalization_factor.cpp/.h–MarginalizationInfo 的 preMarginalize/marginalize 流程、ResidualBlockInfo 的残差块聚合、多线程舒尔补、localSize(7)=6 流形处理、estimator.cpp 的 marginOld/marginNew 调度。本篇的 $H_r$、$J_{\text{marg}}$、FEJ 都将落地成具体代码行。

本篇为纯理论推导，公式与 marginalization_factor.h 结构对齐。舒尔补与 FEJ 的更严格论证见 Hager 因子图教程或 VINS-Mono 论文附录。如发现表述有误，欢迎评论区指正。