Description：OKVIS 论文与源码笔记 — 紧耦合关键帧滑窗 VIO 的开山系统之一，非线性优化（Ceres）联合视觉重投影误差与 IMU 误差，含完整代码架构走读
My Notion Note ID：K2E-B-S16-2
Created：2023-04-10
Updated：2026-06-11
License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

1. Summary

Title: Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization Authors: Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, Paul Furgale (ETH ASL) Paper: IJRR 2015（会议版 RSS 2013） Github: ethz-asl/okvis

OKVIS 是早期将关键帧滑窗 + 非线性优化用于紧耦合 VIO 的代表系统（与 VINS-Mono 并列为优化派 VIO 标杆）。放弃 EKF 框架，把 VIO 表述为一个关键帧窗口上的非线性最小二乘问题，联合优化视觉重投影误差与 IMU 误差，用边缘化把窗口外信息压缩为先验。

系统基于 Google Ceres 求解器，支持单目 / 双目 / 多目配置（通过 NCameraSystem 管理），立体三角化带不确定度（ProbabilisticStereoTriangulator），Ceres 的 ParameterBlock 抽象使位姿（ $\mathrm{SE}(3)$ ）和速度/bias 的流形更新得以正确处理。

OKVIS 系统架构：Frontend（特征检测、匹配）→ Realtime Estimator（因子图优化），IMU 数据驱动状态传播

在 EuRoC 数据集上 OKVIS 平均 ATE 0.089 m，优于 VINS-Fusion (0.138 m)，与 BASALT (平均 0.051 m)、Kimera (0.119 m) 同期相比处于领先水平（OKVIS2 论文 2022 引用的 OKVIS1 重跑数据）。

2. Key Contributions

首批优化派紧耦合 VIO 系统之一（2013 会议版 / 2015 期刊版），证明优化派精度优于 EKF 派（MSCKF）
联合代价函数：视觉重投影误差 + IMU 误差在同一 Ceres 问题中求解
多相机支持：NCameraSystem 统一管理单目 / 双目 / 多目，外参一并优化
部分边缘化：关键帧滑窗维持有界规模，老帧边缘化保留对剩余变量的约束
立体带不确定度三角化：远点齐次表示避免退化

3. Method

3.1 状态变量

每帧惯性状态（9 维速度 + bias）：

\mathbf{x}_R = \bigl[{}_W\mathbf{r}_S^T,\;\mathbf{q}_{WS}^T,\;{}_S\mathbf{v}^T,\;\mathbf{b}_g^T,\;\mathbf{b}_a^T\bigr]^T \tag{1}

${}_W\mathbf{r}_S$ ：world 中传感器位置； $\mathbf{q}_{WS}$ ：world←sensor 姿态四元数； ${}_S\mathbf{v}$ ：传感器系速度； $\mathbf{b}_g, \mathbf{b}_a$ ：陀螺 / 加速度计 bias。

3.2 联合代价函数

J(\mathbf{x}) = \underbrace{\sum_{i,k,j\in\mathcal{J}(i,k)} \mathbf{e}_r^{i,j,k\,T} \mathbf{W}_r^{i,j,k}\,\mathbf{e}_r^{i,j,k}}_{\text{视觉重投影误差}} + \underbrace{\sum_{k=1}^{K-1} \mathbf{e}_s^{k\,T} \mathbf{W}_s^k\,\mathbf{e}_s^k}_{\text{IMU 误差}} + \underbrace{\mathbf{e}_p^T \mathbf{W}_p\,\mathbf{e}_p}_{\text{边缘化先验}} \tag{2}

$i$ ：相机索引， $k$ ：帧索引， $j$ ：路标索引； $\mathcal{J}(i,k)$ ：相机 $i$ 在帧 $k$ 时观测到的路标集合（只对实际有视觉观测的 $(i,k,j)$ 三元组求和）； $\mathbf{W}_r$ 、 $\mathbf{W}_s$ 为各自信息矩阵； $\mathbf{e}_p, \mathbf{W}_p$ 为边缘化先验残差和信息矩阵（通过 Schur 补把滑出帧的信息压缩而来，是滑窗优化区别于截断式最小二乘的关键）。

3.3 重投影误差

路标 $\mathbf{l}^j$ 在相机 $i$ 、帧 $k$ 上的重投影误差（2 维）：

\mathbf{e}_r^{i,j,k} = \mathbf{z}^{i,j,k} - \pi_i\!\left(\mathbf{T}_{CS}^i\,\mathbf{T}_{SW}^k\,\mathbf{l}^j\right) \tag{3}

$\mathbf{z}^{i,j,k}$ ：实测像素； $\pi_i$ ：相机 $i$ 投影函数（含畸变，见相机模型与畸变）； $\mathbf{T}_{SW}^k$ ：第 $k$ 帧 world→sensor 位姿； $\mathbf{T}_{CS}^i$ ：sensor→camera 外参； $\mathbf{l}^j$ ：齐次路标坐标。

路标用齐次坐标 + ProbabilisticStereoTriangulator 初始化（带不确定度，远点不退化）。

3.4 IMU 误差

相邻关键帧 $k$ → $k+1$ 的 IMU 预积分误差（15 维）：

\mathbf{e}_s^k = \begin{bmatrix}\hat{\mathbf{p}}_{k+1} - \mathbf{p}_{k+1}\\\;2[\hat{\mathbf{q}}_{k+1}\otimes\mathbf{q}_{k+1}^{-1}]_{xyz}\\\hat{\mathbf{v}}_{k+1} - \mathbf{v}_{k+1}\\\hat{\mathbf{b}}_{g,k+1} - \mathbf{b}_{g,k+1}\\\hat{\mathbf{b}}_{a,k+1} - \mathbf{b}_{a,k+1}\end{bmatrix} \tag{4}

$\hat{(\cdot)}$ 为由第 $k$ 帧状态 + IMU 测量积分预测的第 $k+1$ 帧状态。

注：OKVIS 做预积分（ImuError 存 $\Delta q$ 、 $\mathbf{C}_\text{integral}$ 、协方差），但无一阶解析 bias 修正——bias 漂移超阈值时全量重传播（redoPreintegration）。Forster 预积分（VINS-Mono 采用）用解析修正消除重积分开销（对比见 VIO 预积分）。

3.5 关键帧窗口与边缘化

维护两类帧：关键帧（空间分散，保留较久）+ 时间窗口帧（最近几帧，保 IMU 链连续）。帧出窗口时边缘化（Schur 补）压缩为先验，代价是 fill-in 稠密化。边缘化策略（applyMarginalizationStrategy）：

非关键帧滑出（ $x^{c-S}$ 不是关键帧）：边缘化 $x^{c-S}$ 的位姿 + 速度/bias；与 $x^{c-S}$ 相关的路标不边缘化，其在 $x^{c-S}$ 的观测在边缘化前先丢弃（drop），以避免填充路标–路标信息块、保持稀疏性
关键帧滑出（ $x^{c-S}$ 是关键帧）： $x^{c-S}$ 本身不立即边缘化；改为边缘化最老关键帧 $x^{k_1}$ （而非触发本次决策的 $x^{c-S}$ ）；只有被 $x^{k_1}$ 独占、不在 $x^{c-S}$ 或更新帧中可见的路标才随之边缘化； $x^{k_1}$ 的其余路标观测同样先丢弃再边缘化

4. Experiments & Results

OKVIS 原论文（IJRR 2015）在 ETH ASL 自采 MAV 序列上与 MSCKF 比较，精度优势明显。以下为 OKVIS2（2022）论文重跑 OKVIS1 在 EuRoC 11 个序列上的 ATE（m）结果：

序列	OKVIS (v1)	BASALT	Kimera	VINS-Fusion	OKVIS2 Final
MH_01	0.079	0.080	0.080	0.166	0.027
MH_02	0.044	0.060	0.090	0.152	0.023
MH_03	0.096	0.050	0.110	0.125	0.028
MH_04	0.197	0.100	0.150	0.280	0.066
MH_05	0.206	0.080	0.240	0.284	0.068
V1_01	0.050	0.040	0.050	0.076	0.035
V1_02	0.066	0.020	0.110	0.069	0.013
V1_03	0.071	0.030	0.120	0.114	0.019
V2_01	0.062	0.030	0.070	0.066	0.023
V2_02	0.077	0.020	0.100	0.091	0.015
V2_03	0.028	—	0.190	0.096	0.020
平均	0.089	0.051	0.119	0.138	0.031

OKVIS v1 平均 ATE 0.089 m，优于 VINS-Fusion（0.138 m）；BASALT（0.051 m）和 OKVIS2（0.031 m）后续在回环 / 位姿图优化的帮助下进一步提升。数据来源：arXiv:2202.09199 Table I。

5. Ablation & Discussion

原 IJRR 2015 论文未做系统性消融实验（时代局限，2015 年 VIO 基准尚不成熟）。以下为论文中的主要设计权衡讨论：

优化 vs 滤波：OKVIS 是同期"优化派优于滤波派"的直接证据；和同期 MSCKF（EKF）比较精度显著更优
预积分策略：无解析 bias 修正，bias 变大时全量重传播——在低 bias 变化率场景影响不大，长时间运动时成本较高（VINS-Mono 和后续工作用解析修正解决）
边缘化 fill-in：OKVIS 谨慎管理稀疏结构，但路标边缘化仍带来一定稠密化

6. Strengths / Limitations / Future Work

优点：

优化派 VIO 的基石工作，框架清晰，Ceres 实现工程可靠
多相机支持，双目 / 单目统一，外参在线优化
开源（BSD 3-clause），代码整洁，是后续系统（VINS-Mono、ORB-SLAM3-VI）的重要参考

局限：

无回环 / 无全局地图（ETH 发布时明确标注）
预积分无解析 bias 修正，bias 漂移时全量重传播开销高
边缘化实现复杂；不支持非线性相机 / 鱼眼（OKVIS2 扩展了这点）

后续工作：

OKVIS2（arXiv:2202.09199）：加位姿图 + 回环，精度提升到 0.031 m 均值
VINS-Mono / ORB-SLAM3-VI：继承滑窗优化思路，加回环与重定位（见 VINS-Mono 笔记）

7. Code Walkthrough

代码库：ethz-asl/okvis

7.1 模块总览

模块	职责
`okvis_frontend`	特征检测 / 数据关联 / 匹配 / IMU 传播
`okvis_ceres`	Estimator、各种 Error/ParameterBlock、边缘化、优化
`okvis_matcher`	DenseMatcher + 线程池
`okvis_cv`	相机模型 + Frame / MultiFrame 数据结构
`okvis_common`	公共类型 (Measurement、Parameters、接口定义)
`okvis_kinematics`	Transformation (SE(3) 位姿)

7.2 okvis_frontend — 前端三种匹配

继承 VioFrontendInterface，三个核心接口：detectAndDescribe、dataAssociationAndInitialization、propagation。

matchToKeyframes（3d2d + 2d2d，倒序遍历关键帧）：

第 1 轮：3d2d 匹配，最多遍历 3 个关键帧（kfcounter > 2 时退出，不论匹配是否成功）
第 2 轮：2d2d 匹配，最多遍历 2 个关键帧（kfcounter > 1 时退出）；未初始化时加 runRansac2d2d（RANSAC：随机抽样一致，外点剔除算法）
doWeNeedANewKeyframe：用 convexHull overlap（当前帧特征点凸包与关键帧重叠面积比，衡量视场覆盖新颖程度）判断

matchToLastFrame：仅匹配上一帧（age-1 帧；若该帧是关键帧则直接返回 0，由 matchToKeyframes 覆盖），先 3d2d 后 2d2d

matchStereo：单 MultiFrame 内的 1-to-1 立体匹配（无 ransac，代码留 TODO）

7.3 okvis_ceres — 后端

Estimator 继承 VioBackendInterface：

addState：第 1 帧加 PoseError + SpeedAndBiasParameterBlock；后续帧加 ImuError；若外参在线估计，还加 RelativePoseError 约束相邻帧 T_SCi 漂移
addLandmark：Eigen::Vector4d → MapPoint 存入 landmarksMap_
applyMarginalizationStrategy：滑窗边缘化核心（见 §3.5）

Error 类型：

类	对应误差
`ReprojectionError`	视觉重投影（式 3）
`ImuError`	IMU 预积分误差（式 4），含协方差传播
`PoseError`	初始帧先验约束
`RelativePoseError`	相邻帧相机外参漂移约束（T_SCi 时间一致性先验，仅外参在线估计时添加）

7.4 VioKeyframeWindowMatchingAlgorithm

把 Estimator 传入（访问 landmark + 添加观测）：

3d2d：A 的点投影到 B → 验证重投影误差 → 加 measurement
2d2d：ProbabilisticStereoTriangulator 判断三角化可行 → 建 landmark → 加 A/B 两侧观测

7.5 关键参数

参数	含义
`VioParameters.optimization.*`	GN/LM 最大迭代、收敛阈值
`numKeyframes`	关键帧窗口最大帧数
`numImuFrames`	时间窗口非关键帧数
立体外参	`NCameraSystem` 管理，支持在线精调

7.6 paper-vs-code 差异

论文	代码
预积分有一致性保证	bias 漂移超阈值直接全量 `redoPreintegration`，无解析修正
边缘化压缩为先验	`applyMarginalizationStrategy` 实现复杂，处理路标 / 非关键帧两条路径
立体三角化带不确定度	`ProbabilisticStereoTriangulator` 用角度不确定度过滤退化情况

References

类别	链接
论文	Leutenegger S., Lynen S., Bosse M., Siegwart R., Furgale P. (2015). Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization. IJRR, 34(3). DOI
OKVIS2	Leutenegger S. (2022). OKVIS2: Realtime Scalable Visual-Inertial SLAM with Loop Closure. arXiv:2202.09199
代码	github.com/ethz-asl/okvis
解读	CSDN: OKVIS 代码解读 / fuxingyin
解读	博客园: OKVIS 笔记 / JingeTU
相关	预积分理论见 VIO 预积分；滑窗优化见 VIO 滑窗优化；相机畸变见相机模型与畸变

OKVIS — Keyframe-Based Visual-Inertial SLAM

Table of Contents