Description：OKVIS (Open Keyframe-based Visual-Inertial SLAM) 论文与源码笔记 — 紧耦合关键帧滑窗 VIO 的开山系统之一，含代码架构走读 (frontend / ceres / matcher / cv / common 各模块)
Paper：Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., & Furgale, P. (2015). Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization. IJRR.
K2E-B ID：[K2E-B-S16-2]
Max3 PDF：[K2E] SLAM/[K2E-B-S] SLAM Systems/[K2E-B-S16] OKVIS/[K2E-B-S16-2][2015] Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization.pdf
Notion ID：(待创建)
Created：2023-04-10
Updated：2026-06-02
License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

1. OKVIS 概述
2. 联合代价函数
3. 重投影误差项
4. IMU 误差项
5. 关键帧窗口与边缘化
6. 贡献与地位
7. 系统模块总览 (代码走读)
8. okvis_frontend — 前端
9. okvis_ceres — 后端优化
10. okvis_matcher — 匹配
11. okvis_cv — 相机模型与帧
12. okvis_common — 公共数据结构
13. 基础模块
References

1. OKVIS 概述

OKVIS (Open Keyframe-based Visual-Inertial SLAM) — Leutenegger 等 2015 年的紧耦合优化 VIO 系统，是早期把关键帧滑窗 + 非线性优化用于视觉惯性的代表作 (与 VINS-Mono 并列为优化派 VIO 标杆)。

核心思想：放弃滤波 (EKF) 框架，把 VIO 表述成一个关键帧窗口上的非线性最小二乘问题，联合优化视觉重投影误差和 IMU 误差，用边缘化把窗口外的信息压缩成先验以控制规模。

核心特点：

紧耦合 — 视觉重投影残差 + IMU 误差项共同进一个非线性优化 (基于 Ceres)
关键帧滑窗 — 维护关键帧 + 最近帧的有界窗口，老帧通过部分边缘化压缩成先验
多相机支持 — 通过 NCameraSystem 管理任意相机配置 (单目/双目/多目统一)
立体三角化 — ProbabilisticStereoTriangulator 带不确定度的三角化

通用 VIO 滑窗优化 / 预积分 / 边缘化理论见 VIO 系列 (ch02 预积分、ch03 滑窗) 与 边缘化 那篇；本笔记是 OKVIS 这个具体系统的论文方法 + 代码笔记，不重复理论。

2. 联合代价函数

OKVIS 的核心是一个非线性最小二乘目标，同时含视觉和惯性两类误差：

$$ J(\mathbf{x}) = \underbrace{\sum_{i=1}^{I} \sum_{k=1}^{K} \sum_{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 误差}} $$

$i$ 相机索引 (多相机)，$k$ 帧索引，$j$ 路标索引
$\mathbf{e}_r$ 重投影误差 (§3)，$\mathbf{W}_r$ 信息矩阵 (重投影不确定度的逆)
$\mathbf{e}_s$ 相邻帧间 IMU 误差 (§4)，$\mathbf{W}_s$ IMU 误差信息矩阵

用 Google Ceres 求解 (Gauss-Newton / LM，见 非线性优化方法)。

状态变量

每帧状态：

$$ \mathbf{x}_R = [{}_W\mathbf{r}S^T, \mathbf{q}{WS}^T, {}_S\mathbf{v}^T, \mathbf{b}_g^T, \mathbf{b}_a^T]^T $$

(位置 ${}_W\mathbf{r}S$ + 姿态四元数 $\mathbf{q}{WS}$ + 速度 ${}_S\mathbf{v}$ 传感器系 + 陀螺 bias + 加速度计 bias)，外加相机-IMU 外参和路标。

3. 重投影误差项

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

$$ \mathbf{e}r^{i,j,k} = \mathbf{z}^{i,j,k} - \pi_i(\mathbf{T}{CS}^i , \mathbf{T}_{SW}^k , \mathbf{l}^j) $$

$\mathbf{z}$ 实测像素观测
$\pi_i$ 相机 $i$ 投影函数 (含畸变，见 相机模型与畸变)
$\mathbf{T}{SW}^k$ 帧 $k$ 的 world→sensor 位姿，$\mathbf{T}{CS}^i$ sensor→camera 外参
$\mathbf{l}^j$ 路标齐次坐标

OKVIS 用齐次坐标 + 带不确定度的立体三角化 (ProbabilisticStereoTriangulator) 初始化路标，远点用齐次表示避免退化。

4. IMU 误差项

相邻两关键帧间，用 IMU 测量预测后一帧状态，与优化变量的差作为误差：

$$ \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} $$

带 hat 的是从第 $k$ 帧状态 + IMU 测量积分预测出的第 $k+1$ 帧状态。

注意：OKVIS 原版做预积分 (ImuError 存 $\Delta q$、$C_{\text{integral}}$、协方差等增量)，但没有 Forster/VINS-Mono 那种一阶解析 bias 修正 — bias 漂移超阈值时直接全量重传播 (redoPreintegration)。后续工作 (Forster 预积分) 才用解析 bias 修正免去重积分 (理论对比见 VIO 系列 ch02 预积分)。误差协方差通过 IMU 误差传播得到，作为信息矩阵 $\mathbf{W}_s$。

5. 关键帧窗口与边缘化

有界窗口

OKVIS 维护两类帧：

关键帧 (keyframes) — 空间上分散，保留较久，提供视差
时间窗口帧 (temporal frames) — 最近几帧，保证 IMU 链连续

窗口大小固定 → 优化规模有界 → 实时。

部分边缘化

帧滑出窗口时边缘化它 (Schur 补) 压缩成先验，保留对剩余变量约束。OKVIS 的边缘化策略 (对应代码 applyMarginalizationStrategy)：

非关键帧滑出 → 边缘化位姿 + 速度/bias，路标观测处理需保持稀疏
关键帧滑出 → 边缘化关键帧及其专属路标

代价是 fill-in 稠密化 (见 边缘化 那篇)。OKVIS 谨慎管理边缘化平衡信息保留与稀疏性。

6. 贡献与地位

首批把关键帧 BA 思想用于紧耦合 VIO 的系统之一 (2013 会议版 / 2015 期刊版)
证明了优化派 VIO 精度优于滤波派 (相比当时 MSCKF)
多相机 + 立体支持，工程完整，开源 (ETH ASL)
影响后续 VINS-Mono / ICE-BA / ORB-SLAM3-VI

局限：IMU 预积分缺一阶解析 bias 修正，bias 变化要全量重传播 (后被 Forster 解析修正取代)；边缘化实现复杂；无回环 (纯里程计)。

7. 系统模块总览 (代码走读)

以下是 OKVIS 开源代码的架构走读 (加分内容)。代码按功能拆成若干模块：

模块	职责
`okvis_frontend`	前端 — 特征检测、数据关联、匹配、IMU 传播
`okvis_ceres`	后端 — Estimator、各种 Error/ParameterBlock、边缘化、优化
`okvis_matcher`	匹配框架 — DenseMatcher + 线程池
`okvis_cv`	相机模型 + Frame / MultiFrame 数据结构
`okvis_common`	公共类型 — Measurement、Parameters、接口定义
`okvis_kinematics`	Transformation (位姿)
`okvis_time` / `okvis_timing` / `okvis_util`	时间、计时、工具

8. okvis_frontend — 前端

Frontend 类

继承 VioFrontendInterface，增加 BRISK 特征部分。三个核心接口：

detectAndDescribe — frameOut->setDetector / setExtractor / detect / describe
dataAssociationAndInitialization — 基于相机畸变模型做 template，分三大步骤 matchXXXX
propagation — 直接调 okvis::ceres::ImuError::propagation 做 IMU 传播

三种匹配

matchToKeyframes

先检查至少有 2 帧
第 1 个 for loop：Estimator::frameIdByAge 是从 rbegin 开始的迭代器，age=1 起 age++ — 即除当前帧外，倒着往前匹配关键帧；非关键帧 continue；成功匹配 ≥ 3 个关键帧后退出。内层小 for loop 遍历所有相机，用 VioKeyframeWindowMatchingAlgorithm 做 3d2d 匹配
第 2 个 for loop：类似倒着匹配关键帧，成功 2 个退出。用 2d2d 匹配；未初始化时额外 runRansac2d2d 去外点，已初始化但匹配最后一个关键帧时用 runRansac3d2d
完成后 doWeNeedANewKeyframe — 检查当前帧与之前所有帧的最高 overlap (两帧 convexHull → OpenCV contourArea 判断)

matchToLastFrame

与 matchToKeyframes 类似但不遍历所有帧，只匹配最后一个关键帧
先 3d2d (带 ransac3d2d)，再 2d2d，未初始化再加 ransac2d2d

matchStereo

主要在 1 个 MultiFrame 内检查 overlap + 1-to-1 匹配
先 2d2d 再 3d2d；TODO 提到没有 ransac

三角化

stereo_triangulation — 只有一个 triangulateFast
ProbabilisticStereoTriangulator — 带不确定度的立体三角化

VioKeyframeWindowMatchingAlgorithm

继承 MatchingAlgorithm，把 Estimator 传进来 (主要处理 landmark + add measurement，本质是 map API)，带一个 ProbabilisticStereoTriangulator：

doSetup — 先设置 uncertainty，然后大 if-else 区分 3d2d / 2d2d。3d2d 把 A 中的点投影到 B (主要处理 uncertainty)；2d2d 主要是 raysigma，之后加 landmark
verifyMatch — 3d2d 判断投影误差；2d2d 用 probabilisticStereoTriangulator 判断能否三角化
setBestMatch — 被 DenseMatcher 的 matchBody 调用。大 if-else 区分 3d2d / 2d2d
- 2d2d：先三角化判断是否可行 → 检查 isLandmarkAdded → 增加 landmark → 把 landmark 在 A、B 中的观测加入 estimator → 检查与其他观测一致性 (整个过程可叫 re-triangulate，因新建了 landmark)
- 3d2d：先 A 投影到 B，按重投影误差判断 → 若未加过 measurement 则增加一个 measurement 到 estimator

9. okvis_ceres — 后端优化

Estimator

继承 VioBackendInterface：

addState — 先判断是否可建新 State，根据是否第一个 State 不同，建立 State Id → 建 State 中 Camera 部分 → 建 IMU 部分 → 大 if-else 区分是否第 1 帧：
- 第 1 帧：增加 Camera 的 PoseError、IMU 的 SpeedAndBiasParameterBlock
- 非第 1 帧：增加 ImuError 和 RelativePoseError
addLandmark — 把 Eigen::Vector4d 变为 MapPoint 插入 landmarksMap_，最后 isLandmarkAdded 检查
isLandmarkAdded — 检查 landmarksMap_ 有无该 landmark ID
applyMarginalizationStrategy — 复杂实现 (边缘化策略，对应滑窗老帧压缩成先验)
initPoseFromImu — 从 IMU 初始化位姿

10. okvis_matcher — 匹配

框架

ThreadPool — 线程池
MatchingAlgorithm — 纯 API，用 multimap<size_t, size_t> 做底层 tree structure；VioKeyframeWindowMatchingAlgorithm 扩展此 API
DenseMatcher — 用于 match 两帧的 keypoints，具体用 MATCHING_ALGORITHM_T 模板完成

DenseMatcher 内部

MatchJob — 保存当前线程里 match 的最好 pair
match / matchInImageSpace — 大 API，先 doSetup，把 doWorkLinearMatching / doWorkImageSpaceMatching 传给 matchBody
doWorkLinearMatching — 核心 for loop：A 里每个点建 aiBest，调 listBIteration (传入 matchingAlgorithm)，最后 assignbest
doWorkImageSpaceMatching — 结构与上面基本一样
assignbest — 遍历通过 score 找最好 pair，递归函数
matchBody — 建立各线程 (数据结构在 MatchJob)，用 thread pool enqueue 管理
listBIteration — 遍历各 pair 找最好 pair 存 aiBest，被两个 doWork 调用

11. okvis_cv — 相机模型与帧

两类：相机模型 + 帧数据结构。

相机模型

CameraBase — 典型相机模型，带 project / backproject
DistortionBase — 畸变基类
四种畸变：RadialTangentialDistortion、RadialTangentialDistortion8、EquidistantDistortion、NoDistortion (见 相机模型与畸变)
PinholeCamera — 相机模型，扩展 CameraBase，在 Distortion 上做模板
NCameraSystem — 多相机的 list + manager，addCamera 把新相机和外参存进去

帧

Frame — 把 detector + extractor 传进去：

Frame(const cv::Mat & image,
      std::shared_ptr<cameras::CameraBase> & cameraGeometry,
      std::shared_ptr<cv::FeatureDetector> & detector,
      std::shared_ptr<cv::DescriptorExtractor> & extractor);

存 keypoint、descriptor、landmark (如能 associate)

MultiFrame — 底层一堆 Frame + 一个 camera system，接口与 Frame 基本一样但多 cameraIdx，额外两个 overlap 接口

12. okvis_common — 公共数据结构

FrameTypedefs — 定义 KeyPointIdentifier、MapPoint、Match、Observation 等，非常直接
Measurements — 各种观测。Measurement 基类存时间 + sensorid；子类：ImuSensorReadings、DepthCameraData、PositionReading、GpsPositionReading、MagnetometerReading、BarometerReading、DifferentialPressureReading。各 measurement 定义如 typedef Measurement<ImuSensorReadings> ImuMeasurement;
Parameters — 各种 XXXXParameters 是传感器设置；Optimization 是优化设置；PublishingParameters 是输出设置；VioParameters 是全部的集合
Variables — 给 SpeedAndBias、ImuBias 等设 typedef Eigen::Matrix<double,9,1>
VioParametersReader — 读取 VioParameters 和 CameraCalibration
接口：
- VioFrontendInterface — 前端接口，3 个 (detectAndDescribe / dataAssociationAndInitialization / propagation)，大部分传参不保存
- VioBackendInterface — 类似容器，getter/setter + 核心 optimize
- VioInterface — 像保存各种 measurement 及 callback 的地方
地图用 okvis::MapPointVector 存

13. 基础模块

okvis_kinematics

Transformation — Affine3d + Quaternion 的结合体
operators — pose 相关 utils

okvis_time

TimeBase — 定义一些 operator，子类 Time / WallTime (用 CRTP 实现静态多态)
DurationBase — FromSec / FromNSec + operator，子类 Duration (同样 CRTP)

okvis_timing

NsecTimeUtilities — 用 boost int64_t 做 nsec，numeric_limits 的 min 做 invalid timestamp，基于 chrono system_clock::time_point + duration 的 utils
timer — BOOST_DATE_TIME_NO_LOCALE；用 boost accumulators 做累加器 (mean / standard deviation，可输入权重，看着像 python 但是结构体)

okvis_util

assert_macros — 自定义 exception + assert 打印 (没用 STL static_assert)
source_file_pos — 方便打印 __FUNCTION__ / __FILE__ / __LINE__ 的 class + macro

References

Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., & Furgale, P. (2015). Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization. IJRR, 34(3). — OKVIS 论文
OKVIS 开源代码: github.com/ethz-asl/okvis
CSDN: OKVIS 代码解读 / fuxingyin — 本笔记代码走读主参考
博客园: OKVIS 笔记 / JingeTU
滑窗优化 / 预积分 / 边缘化理论见 VIO 系列；相机畸变模型见 相机模型与畸变

OKVIS — Keyframe-Based Visual-Inertial SLAM

Table of Contents