VINS-Mono

  • Description：VINS-Mono 论文与源码笔记 — 紧耦合单目视觉惯性状态估计器，含系统流程 (前端光流 / 初始化 / 滑窗优化 / 边缘化 / 回环) 与代码走读 (estimator.cc / Ceres CostFunction / 逆深度重投影)
  • Paper：Qin, T., Li, P., & Shen, S. (2018). VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator. IEEE T-RO, 34(4).
  • K2E-B ID：[K2E-B-S1-1]
  • Max3 PDF：[K2E] SLAM/[K2E-B-S] SLAM Systems/[K2E-B-S1] VINS-Mono/[K2E-B-S1-1][2018] VINS-Mono A Robust and Versatile Monocular Visual Inertial State Estimator.pdf (同目录另有 S1-2..S1-5 公式推导/代码解析中文资料)
  • Notion ID：(待创建)
  • Created：2022-07-15
  • Updated：2026-06-02
  • License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

Table of Contents

1. VINS-Mono 概述

VINS-Mono (Qin, Li, Shen 2018, 港科 HKUST) — 紧耦合单目 + IMU 的优化派 VIO，是工程影响最大的开源 VIO 之一。相比 OKVIS：

  • 单目 (OKVIS 多相机/双目)，靠 IMU 解尺度
  • IMU 预积分 (中值法);对比背景:OKVIS (2015) 也做预积分,但缺 Forster 式解析 bias 修正 — bias 漂移超阈值时全量重传播 (redoPreintegration) — 注:OKVIS 是多相机系统，VINS-Mono 论文未提及 OKVIS
  • 完整 pipeline — 含鲁棒初始化 + 4-DOF 回环位姿图
  • 代码清晰，成为后续 VIO 学习/魔改的基础

VINS-Mono 的理论 (预积分、滑窗、边缘化、可观性) 本就是我 VIO 系列 笔记的蓝本 — 本文不重复理论，只记 VINS-Mono 的具体选择和代码。

2. 系统流程

图像 → 前端 (KLT 光流跟踪 FAST 角点)
IMU  → 预积分
       ↓
   初始化 (视觉 SfM + 视觉惯性对齐, 求尺度/重力/速度/bias)
       ↓
   滑窗紧耦合优化 (prior + IMU 预积分 + 视觉重投影)
       ↓
   回环检测 (DBoW2) → 4-DOF 位姿图优化

3. 前端

  • 角点 + 光流 — GFTT (Shi-Tomasi) 角点 + KLT 金字塔光流跟踪 (不算描述子，见 光流 那篇)；每帧保持约 150 个特征 (config 默认 max_cnt=150)
  • 每帧做 基础矩阵 RANSAC 去外点
  • 关键帧选择：平均视差超阈值 / 跟踪质量下降
  • 输出 ROS sensor_msgs/PointCloud (注意：用 Point 而非 Point32；u,v 是 row/column；新版 PointCloud2 无此坑)
  • IMU 消息若无姿态估计，协方差矩阵 element 0 设 -1

4. 初始化

VINS-Mono 用松耦合初始化 (见 VIO 系列 ch05 初始化)：

  1. 纯视觉 SfM — 以滑窗第一帧 $c_0$ 为参考系;用 5 点法在最新帧与某共视帧 (视差 >30px 且跟踪特征 >20) 间求相对位姿并三角化
  2. 以 $c_0$ 为参考，对其余帧做 PnP 求位姿 + 三角化各点 3D 坐标
  3. 视觉惯性对齐 — 用 IMU 预积分约束求解：陀螺 bias → 速度/重力/尺度 (线性求解)

5. 滑窗优化

紧耦合优化目标 = 三类残差 (见 VIO 系列 ch03)：

min  ‖先验‖² + Σ‖IMU 预积分残差‖² + Σ‖视觉重投影残差‖²

优化变量加入参数块：

  • 滑窗所有帧 pose
  • 相机-IMU 外参 (可通过 SetParameterBlockConstant 固定不优化)
  • 特征点逆深度
  • 相机-IMU 时间同步偏移 td

用 Ceres 求解。VINS 用 Ceres 的 SizedCostFunction (手写解析 Jacobian) + LocalParameterization (处理四元数流形上的 Plus / ComputeJacobian / GlobalSize=4 / LocalSize=3)。

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

逆深度参数化 $\lambda = 1/z$ — 把每个特征从 (x,y,z) 3 维降到 1 维：

  • 远点 $1/\lambda \to 0$，数值稳定
  • 求解快 (参数减到 1/3)

特征在锚帧 $c_i$ 观测，投影到 $c_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{1}{\lambda} u_{c_i} \ \tfrac{1}{\lambda} v_{c_i} \ \tfrac{1}{\lambda} \ 1 \end{bmatrix} $$

(过程：$(u,v,\lambda){c_i} \to$ cam_i 3D $\to$ imu_i $\to$ imu_j $\to$ cam_j $\to (u,v,\lambda){c_j}$)

Jacobian 用链式法则 $\frac{\partial r}{\partial x} = \frac{\partial r}{\partial p} \frac{\partial p}{\partial x}$。注:VINS-Mono 可选把残差定义在单位球面切平面 $[b_1\ b_2]^T$ 上 (编译开关 #ifdef UNIT_SPHERE_ERROR，默认构建用针孔残差);下面这个 $2\times3$ 针孔矩阵是链式法则中正确的中间项 (残差对相机系点)：

$$ \frac{\partial r}{\partial p} = \begin{bmatrix} \tfrac{1}{z} & 0 & -\tfrac{x}{z^2} \ 0 & \tfrac{1}{z} & -\tfrac{y}{z^2} \end{bmatrix} $$

7. 边缘化

VINS-Mono 边缘化策略 (见 边缘化 那篇 + VIO 系列 ch03 §8)：

  • 次新帧是关键帧 → marg 最老帧 + 其路标，信息压成先验
  • 次新帧不是关键帧 → 丢弃其视觉观测，但 IMU 预积分保留传给下一帧

信息矩阵 Schur 消元：

$$ \begin{bmatrix} \Lambda_a & \Lambda_b \ \Lambda_b^T & \Lambda_c \end{bmatrix} \begin{bmatrix} \delta x_a \ \delta x_b \end{bmatrix} = \begin{bmatrix} g_a \ g_b \end{bmatrix} ;\Rightarrow; (\Lambda_c - \Lambda_b^T \Lambda_a^{-1} \Lambda_b) \delta x_b = g_b - \Lambda_b^T \Lambda_a^{-1} g_a $$

为什么 VINS 用误差状态 (ESKF 思想)

(见 四元数 EKF + 高斯滤波家族 §7)

  1. 误差量用旋转矢量 (3 维) 表示，比四元数 (4 维) 好，不会过参数化
  2. 接近原点，避免奇异/万向锁
  3. error state 小，二阶量可忽略
  4. error state 变化缓慢，KF correction 频率可以更低

8. 回环与位姿图

  • 回环检测 — DBoW2 词袋找候选帧 → 特征匹配 + 几何验证
  • 重定位 — 回环帧与当前滑窗紧耦合
  • 4-DOF 位姿图优化 — 因为 roll/pitch 被重力固定可观，只优化 x/y/z/yaw 4 个自由度 (见 VIO 系列 ch03 可观性：单目+IMU 只有 4 DOF 不可观)

9. 代码走读

后端 estimator.cc

processImage → solveOdometry 主要步骤：

  1. corner case 重复判断
    • 滑窗是否满了，否则不做
    • 窗口满时主动调用 initialStructure() 尝试初始化 (非简单跳过)
  2. triangulate — 三角化新特征
  3. optimization
    • 滑窗所有 pose 加入参数块
    • 相机/IMU 外参 (可 SetParameterBlockConstant 不优化)
    • 特征点逆深度
    • 时间同步 td

Ceres 用法

// 手写解析 Jacobian
class CustomCostFunction : public ceres::SizedCostFunction<residual_size, param_block_size> {
  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    // add residual and jacobian
  }
};

// 流形参数化 (四元数)
class LocalParameterization {
  virtual bool Plus();              // 流形上加法
  virtual bool ComputeJacobian();
  virtual int GlobalSize();         // 实际参数维度 (4)
  virtual int LocalSize();          // 实际自由度 (3)
};

也可用 AutoDiffCostFunction 自动微分 (但 VINS 多用解析 Jacobian 求性能)。

四元数约定

VINS 用 Hamilton 四元数，左/右乘矩阵：

$$ p \otimes q = [q_1]_L q_2 = [q_2]_R q_1 $$ $$ [q]L = q_w I + \begin{bmatrix} 0 & -q_v^T \ q_v & [q_v]\times \end{bmatrix}, \quad [q]R = q_w I + \begin{bmatrix} 0 & -q_v^T \ q_v & -[q_v]\times \end{bmatrix} $$

(四元数代数见 四元数 EKF §2)

References