滑动窗口优化与边缘化

  • Description：VIO 后端 — 滑窗优化的三类残差 (prior / IMU / 视觉)、状态向量与逆深度参数化、边缘化与 Schur 补、信息矩阵稠密化、FEJ 与可观性、VINS-Mono 的边缘化策略
  • Source：VIO (Notion id 7a997893-a828-4193-ac08-0750457679fc) 的滑窗/边缘化/FEJ 部分；整理自深蓝学院 VIO 课程笔记
  • Created：2022-04-27
  • Updated：2026-06-03
  • License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

Table of Contents

1. 为什么用滑动窗口

全量 BA (优化所有历史帧) 计算量随时间无限增长，无法实时。滑动窗口 = 只保留最近 $N$ 个关键帧 + 窗口内路标，优化这个固定大小的子问题。

代价：滑出窗口的老帧不能直接丢 (丢了会损失约束信息) → 用边缘化 (marginalization) 把老帧的信息压缩成先验保留下来 (见 §6)。

(滑窗 vs ORB-SLAM 的 Local Map 对比见 SLAM 面试题汇总 C4)

2. 状态向量与逆深度

窗口内 $N$ 帧、$M$ 个路标。单帧状态：

$$ \mathbf{x}i = [\mathbf{p}{wb_i}, , q_{wb_i}, , \mathbf{v}^w, , \mathbf{b}_a^{b_i}, , \mathbf{b}_g^{b_i}]^T $$

(位置 + 姿态 + 速度 + 加速度计 bias + 陀螺 bias)

完整优化变量：

$$ \boldsymbol{\chi} = [\mathbf{x}n, \mathbf{x}{n+1}, \dots, \lambda_m, \lambda_{m+1}, \dots] $$

路标用逆深度 $\lambda = 1/z$ 参数化 (而非 xyz)。理由见 三角化 D4 / 远点数值稳定性 — 逆深度对远点和高斯近似更友好，是 VINS-Mono 的选择。

3. 三类残差

VIO 滑窗优化目标 = 三类残差的鲁棒加权和：

$$ \min_{\boldsymbol{\chi}} ; \underbrace{\sum \rho(|\mathbf{r}p - J_p \boldsymbol{\chi}|^2)}{\text{先验}} + \underbrace{\sum \rho(|\mathbf{r}b(z{b_i b_{i+1}}, \boldsymbol{\chi})|^2)}{\text{IMU}} + \underbrace{\sum \rho(|\mathbf{r}f(z{f_j}^{c_i}, \boldsymbol{\chi})|^2)}{\text{视觉}} $$

  • 先验残差 $\mathbf{r}_p$ — 边缘化掉的老帧压缩成的先验信息 (见 §6)
  • IMU 残差 $\mathbf{r}_b$ — 预积分约束 (见 ch02 §6)
  • 视觉残差 $\mathbf{r}_f$ — 重投影误差 (见 §4)

$\rho(\cdot)$ 是鲁棒核 (Huber/Cauchy，见 RANSAC 与鲁棒估计 §5)。求解走 GN/LM (见 非线性优化方法)。

4. 视觉重投影残差 (逆深度)

特征点在第 $i$ 帧被观测 (归一化坐标 $[u_{c_i}, v_{c_i}, 1]$)、逆深度 $\lambda$，投影到第 $j$ 帧：

$$ \begin{bmatrix} x_{c_j} \ y_{c_j} \ z_{c_j} \ 1 \end{bmatrix} = T_{bc}^{-1} T_{wb_j}^{-1} T_{wb_i} T_{bc} \begin{bmatrix} \tfrac{u_{c_i}}{\lambda} \ \tfrac{v_{c_i}}{\lambda} \ \tfrac{1}{\lambda} \ 1 \end{bmatrix} $$

链条：第 $i$ 相机系 → 第 $i$ body 系 ($T_{bc}$) → 世界系 ($T_{wb_i}$) → 第 $j$ body 系 ($T_{wb_j}^{-1}$) → 第 $j$ 相机系 ($T_{bc}^{-1}$)。

重投影误差 (归一化平面)：

$$ \mathbf{r}c = \begin{bmatrix} \tfrac{x{c_j}}{z_{c_j}} - u_j \ \tfrac{y_{c_j}}{z_{c_j}} - v_j \end{bmatrix} $$

5. Schur 补加速求解

GN/LM 增量方程 $H \Delta \mathbf{x} = \mathbf{b}$，$H = J^T \Sigma^{-1} J$。把变量分成相机/位姿块 (p) 和路标块 (l)：

$$ \begin{bmatrix} H_{pp} & H_{pl} \ H_{lp} & H_{ll} \end{bmatrix} \begin{bmatrix} \Delta \mathbf{x}_p \ \Delta \mathbf{x}_l \end{bmatrix} = \begin{bmatrix} -\mathbf{b}_p \ -\mathbf{b}_l \end{bmatrix} $$

路标块 $H_{ll}$ 是块对角的 (每个路标只跟自己有关) → 容易求逆。用 Schur 补先消掉路标，得到只含位姿的小系统：

$$ (H_{pp} - H_{pl} H_{ll}^{-1} H_{pl}^T) \Delta \mathbf{x}p = -\mathbf{b}p + H{pl} H{ll}^{-1} \mathbf{b}_l $$

解出位姿增量后回代求路标：

$$ H_{ll} \Delta \mathbf{x}_l = -\mathbf{b}l - H{pl}^T \Delta \mathbf{x}_p $$

g2o 和 Ceres 都用这套 Schur 补简化。(Schur 补的代数恒等式见 矩阵分解 / 边缘化数学)

6. 边缘化

滑出窗口的老帧不能直接丢 — 要边缘化 (marginalization)：把它从联合分布积分掉，但保留它对剩余变量的约束压成先验。数学上是对信息矩阵做 Schur 补 (与 §5 同一个操作，但目的不同 — §5 是加速，这里是消元保信息)。代价是 fill-in 稠密化 — 原先独立的变量变相关，信息矩阵变稠密，所以要谨慎选择边缘化什么。

完整推导 (Schur 消元、条件高斯、协方差 vs 信息矩阵下边缘化的区别、fill-in) 见 边缘化 Marginalization 那篇 — 它是通用后端话题 (BA / pose-graph / iSAM 都用)，这里不重复。VIO 滑窗里具体边缘化什么由 VINS-Mono 策略决定 (见 §8)。

7. 可观性与 FEJ

可观性定义

测量系统 $\mathbf{z} = h(\boldsymbol{\theta}) + \boldsymbol{\epsilon}$ 可观 ⟺ 不同状态产生不同测量：$\forall \boldsymbol{\theta} \neq \boldsymbol{\theta}'$，$h(\boldsymbol{\theta}) \neq h(\boldsymbol{\theta}')$。

VIO 的不可观自由度

  • 纯单目 SLAM：7 自由度不可观 — 3 旋转 + 3 平移 + 1 尺度 (整体平移/旋转/缩放世界不改变图像)
  • 单目 + IMU：4 自由度不可观 — yaw + 3 平移 (重力让 roll/pitch 可观、加速度让尺度可观，但 yaw 和绝对位置仍不可观)

FEJ 问题

边缘化产生的先验 $\Lambda_p(k)$ 对 $\mathbf{x}_r$ 的 Jacobian 没法更新 (老帧已被边缘化)，但后续新残差仍在不停重新线性化。两边线性化点不一致 → 线性化系统的可观子空间维度被人为抬高 (不可观零空间收缩、获得虚假秩) → 沿不可观方向 (yaw、全局位置) 吸收虚假信息 → 协方差过于乐观 (over-confident)，估计不一致 (inconsistency) 甚至发散。

FEJ (First Estimate Jacobian) 的解法：对同一个变量，所有涉及它的 Jacobian 都固定在它第一次出现时的估计值上线性化，保证零空间不被人为破坏，维持可观性一致。

(MSCKF / OpenVINS 等滤波框架用 FEJ；VINS-Mono 是优化框架，仅在先验残差部分固定线性化点，不完全等同于 FEJ)

8. VINS-Mono 的边缘化策略

VINS-Mono 的滑窗根据次新帧是否关键帧二选一：

  • 次新帧是关键帧 → 边缘化最老帧 + 其上的路标点 (marg old)，先验信息保留传递
  • 次新帧不是关键帧 → 直接丢弃次新帧的视觉观测 (但其 IMU 预积分延续至下一帧 — 将该段 IMU 数据并入相邻预积分块)，不边缘化，避免引入过多 fill-in

这套策略平衡了"保留信息"与"控制稠密化"。

先验残差随状态更新用一阶泰勒近似：

$$ \mathbf{b}_p' = \mathbf{b}_p - \Lambda_p , \delta \mathbf{x}_p $$

References

  • 深蓝学院《视觉惯性 SLAM (VIO)》课程 (贺一家 / 崔华坤) — 本笔记主要来源
  • Qin, T., Li, P., & Shen, S. (2018). VINS-Mono. IEEE T-RO. — 滑窗 + 边缘化策略
  • Leutenegger, S., et al. (2015). Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization (OKVIS). IJRR. — 滑窗边缘化经典
  • Huang, G., Mourikis, A. I., & Roumeliotis, S. I. (2010). Observability-based Rules for Designing Consistent EKF SLAM Estimators. IJRR. — 可观性 + FEJ 理论
  • Walter, M. R., Eustice, R. M., & Leonard, J. J. (2007). Exactly Sparse Extended Information Filters for Feature-Based SLAM. IJRR. — 信息矩阵稀疏化
  • Grisetti, G., Kümmerle, R., Stachniss, C., & Burgard, W. (2010). A Tutorial on Graph-Based SLAM. IEEE ITS Magazine.
  • Kümmerle, R., et al. (2011). g2o: A General Framework for Graph Optimization. ICRA.
  • Zhang, Z., et al. (2018). On the Comparison of Gauge Freedom Handling in Optimization-based Visual-Inertial State Estimation. RA-L. — gauge freedom / 可观性处理对比