边缘化与 FEJ：滑窗 VIO 的一致性

1. 滑窗不能无限长

算力与内存不允许滑窗无限增长。边缘化（Marginalization）把滑出窗口的老状态「压缩」成先验因子，保留对剩余变量的约束。听起来完美，做错时系统慢慢过度自信，急转弯或强回环时突然炸掉——First-Estimate Jacobian（FEJ）是理解这个问题的钥匙。

2. 为何需要边缘化

滑窗有 N 帧 + M 路标。新帧进来、老帧出去时，不能直接丢弃——它观测过的路标、IMU 预积分因子、视觉因子携带的信息会丢失。边缘化在 Gauss-Newton 框架下对老变量做 Schur 补，得到只涉及剩余变量的先验 $(x ˉ prior, H prior)$ 。VINS-Mono 的 marginalization_factor、OpenVINS 的 FEJ、SchurVINS 都是这一路。若滑出帧与仍在窗内的路标有强耦合， $H_{mm}$ 接近奇异——此时应调整 marginalize 顺序或先 marginalize 路标再 marginalize 帧，否则 Schur 数值不稳定。

3. Schur 边缘化

分块 Hessian（r = remain，m = marginalize）：

H = [H_{r r} H_{r m} H_{m r} H_{mm}]

Schur 补：

H_{prior} = H_{r r} - H_{r m} H_{mm}^{- 1} H_{m r}, \quadb_{prior} = b_{r} - H_{r m} H_{mm}^{- 1} b_{m}

$H_{prior}$ 作为先验加回优化。实现中 Cholesky 分解 $H_{mm}$ 求逆—— $H_{mm}$ 必须正定，否则边缘化失败。

4. FEJ：重复线性化的一致性

标准 BA 每次迭代 relinearize 雅可比。边缘化后老测量只出现一次（在先验里），新测量每次 relinearize → 信息被重复计算，协方差过于乐观（inconsistent）。

FEJ：固定首次线性化点 $x ˉ$ ：

J = \frac{\partial r}{\partial x}_{x = x ˉ}

边缘化先验和 FEJ 视觉因子都在 $x ˉ$ 处线性化——保持一致。Huang 2016 observability-constrained EKF 与 FEJ 是同一问题的不同表述。

5. 常见陷阱

陷阱	后果
丢弃交叉项 $H_{r m}$	先验信息不完整
同一测量边缘化两次	过度自信
不检查 $H_{mm}$ 正定性	数值崩溃
FEJ 与 relinearize 混用	不一致，可观性破坏
先验信息矩阵不正定	Cholesky 失败

同一测量若既进入当前视觉因子又被打包进边缘化先验，信息会被算两次——改滑窗逻辑时最容易引入这类重复。数值 fix（特征值 clamp、对角加 $ε I$ ）是最后手段—— $ε$ 太大会丢信息，应优先查线性化点和 FEJ 实现。VINS-Mono 边缘化是社区讨论最多的——任何改 FEJ 的 PR 都要跑 EuRoC 全序列 + 长时间 stress test。

6. 症状与 iSAM2 对比

运行 >10min 后遇急转弯/回环轨迹跳变
协方差 trace 持续下降（虚假收敛）
去掉边缘化先验：短期精度差但长期更稳
Chi2 持续 < 0.5（过度拟合）

边缘化先验条件数持续升高，说明历史信息被压缩得过于「硬」——常见于重复 marginalize 同一帧或 FEJ 点漂移。此时与其加大 $ε I$ ，应先查 marginalize 队列是否 duplicate。

iSAM2 用 Bayes tree 增量更新，边缘化是 tree 重组的自然部分——不需手写 Schur。但 relinearize 策略（relinearizeThreshold）仍可能影响可观性；FEJ 解决「线性化点不一致」，iSAM2 relinearize 解决「何时重算线性化点」——不要混为一谈。

滑窗 VIO 改边缘化代码后，短期 EuRoC ATE 可能不变甚至略好，长期 stress（>30min、急转弯）才暴露问题——验收必须含长时间 bag，不能只看前五分钟的曲线。

7. 验收

边缘化先验条件数
滑窗 30min vs 5min 的 ATE（长期一致性）
FEJ vs standard relinearize 的 Chi2
边缘化耗时 vs 优化总耗时
急转弯/回环前后的位姿跳变量

边缘化是把历史信息压进先验的契约；线性化点不一致，先验会在你最信任它的时候撒谎。改 FEJ 或 Schur 实现后，除了 EuRoC，至少跑一条含多次急转弯的室内 bag——直线走廊 ATE 对边缘化 bug 不敏感，转弯段才放大可观性破坏。VINS 社区对 marginalization_factor 的任何改动，都应附带 Chi2 与 30min stress 对比曲线。先验与当前因子必须在同一 FEJ 线性化点维护，否则「短期 ATE 更好、长期炸」的模式会反复出现。