K2E-B-S1-1 · Paper Note

VINS-Mono

Created 2022-07-15Updated 2026-06-11slam / papers / s-slam-systems / s1-vins-mono
  • Description：VINS-Mono 论文与源码笔记 — 紧耦合单目 VIO 标杆;IMU 预积分 (α/β/γ + 一阶 bias 修正)、松耦合初始化、滑窗优化 (单位球残差/逆深度)、两段式边缘化 (FEJ)、紧耦合重定位与 4-DoF 位姿图;含完整公式摘录与源码解析
  • My Notion Note ID：K2E-B-S1-1
  • Created：2022-07-15
  • Updated：2026-06-11
  • License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

Table of Contents

1. Summary

Title: VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator Authors: Tong Qin, Peiliang Li, Shaojie Shen (HKUST) Paper: arXiv:1708.03852 (IEEE T-RO 34(4), 2018) Github: HKUST-Aerial-Robotics/VINS-Mono (PC/ROS) + VINS-Mobile (iOS)

VINS-Mono 是紧耦合单目 + IMU 的优化派 VIO 完整系统:KLT 光流前端 + IMU 预积分 → 松耦合初始化(视觉 SfM 与 IMU 对齐解出尺度/重力/速度/陀螺 bias)→ 滑窗紧耦合非线性优化(先验 + IMU + 视觉三类残差)→ DBoW2 回环 + 紧耦合重定位 + 4-DoF 全局位姿图。单目 + IMU 的最小传感器组合解决了纯单目的尺度不可观问题。

工程取舍非常体系化:预积分用一阶 bias 修正避免重传播;视觉残差定义在单位球切平面上(兼容鱼眼/大广角);边缘化分两种情形：次新帧是关键帧时边缘化最老帧，不是关键帧时丢弃它的视觉观测、只保留 IMU 预积分;位姿图只优化 4 个自由度(x/y/z/yaw)，因为 roll/pitch 由重力观测、本就无漂移。支持相机-IMU 外参与时间偏移 td 在线标定。

结果:EuRoC 全序列(含 V1_03_difficult)稳定运行;~700 m 室内外混合实验 VIO 漂移 0.88%，加回环后 ≈ 0(同场景 OKVIS 2.36%);5.62 km 校园跑 1 小时 34 分不崩;iPhone 7 Plus 上实时跑 AR(对比 Google Tango 不漂)。它与 OKVIS 同属滑窗优化 VIO，但提供了更完整的 pipeline(初始化 + 回环 + 重定位)，代码清晰，成为此后单目 VIO 学习与魔改的事实基线。

VINS-Mono 的理论(预积分、滑窗、边缘化、可观性)是我 VIO 系列 笔记的蓝本 — 理论推导见该系列，本文记论文的具体公式、系统选择与代码。

2. Key Contributions

  • 鲁棒初始化:松耦合视觉 SfM ↔ IMU 预积分对齐，几秒内恢复尺度/重力/速度/陀螺 bias，可从未知初始状态起飞
  • 紧耦合滑窗 VIO:先验 + IMU + 视觉统一 MAP 优化;单位球面残差兼容任意相机模型;在线外参 + td 标定
  • 紧耦合重定位:回环特征作为额外残差直接进滑窗优化(回环帧位姿固定)，而非简单的位姿对齐
  • 4-DoF 全局位姿图:利用 roll/pitch 可观性，只优化平移 + yaw — 比 6-DoF 位姿图更符合可观性结构
  • 全套开源:PC(ROS)+ iOS 移动端，飞控闭环验证

3. Method

3.1 系统架构

VINS-Mono 完整架构 — 测量预处理 / 初始化 / 滑窗 VIO 与重定位 / 4-DoF 全局位姿图 (论文 Fig. 2)

三个并发线程:① 前端(25 Hz:特征 15 ms + KLT 5 ms)② 滑窗优化(10 Hz，~50 ms)③ 回环检测(100 ms)+ 位姿图(130 ms)。另有两个高频输出:motion-only BA 出相机帧率位姿(~5 ms)，IMU 前向传播出 IMU 帧率位姿。

3.2 前端

  • Shi-Tomasi 角点 + KLT 金字塔光流(不算描述子，原理见 光流)，每帧维持 100-300 特征(EuRoC config max_cnt=150)，特征间最小间距 min_dist=30 px 保证分布
  • 基础矩阵 RANSAC（随机抽样一致，外点剔除算法）去外点(F_threshold=1.0 px)
  • 关键帧判据:旋转补偿后平均视差 > 阈值(论文 SfM 段 30 px;config keyframe_parallax=10.0，除以虚拟焦距 460 转归一化平面)，或跟踪特征数低
  • 输出 ROS sensor_msgs/PointCloud:归一化坐标 + 像素坐标 + 光流速度(供 td 估计)。注意老版用 Point 而非 Point32、u/v 是 row/column — 新版 PointCloud2 无此问题

3.3 IMU 预积分

IMU 测量模型(式 (1)(2)):加计/陀螺读数 = 真值 + bias + 重力项(加计)+ 高斯噪声;bias 服从随机游走 b˙a=nba\dot{b}_a = n_{b_a}, b˙w=nbw\dot{b}_w = n_{b_w}。

世界系直接积分(式 (3))每次优化更新状态后都要重积分。预积分把参考系换到 bkb_k(式 (5)(6))，分离出只依赖 IMU 测量与 bias 的三个量:

αbk+1bk=∬[tk,tk+1]Rtbk(a^t−bat)dt2,βbk+1bk=∫[tk,tk+1]Rtbk(a^t−bat)dt,γbk+1bk=∫[tk,tk+1]12Ω(ω^t−bwt)γtbk dt(6)\alpha^{b_k}_{b_{k+1}} = \iint_{[t_k, t_{k+1}]} R^{b_k}_{t}\left(\hat{a}_t - b_{a_t}\right) dt^2, \quad \beta^{b_k}_{b_{k+1}} = \int_{[t_k, t_{k+1}]} R^{b_k}_{t}\left(\hat{a}_t - b_{a_t}\right) dt, \quad \gamma^{b_k}_{b_{k+1}} = \int_{[t_k, t_{k+1}]} \tfrac{1}{2} \Omega\left(\hat{\omega}_t - b_{w_t}\right) \gamma^{b_k}_t\, dt \tag{6}

符号:α/β/γ\alpha/\beta/\gamma 分别为 [tk,tk+1][t_k, t_{k+1}] 区间内的相对位移/速度/旋转(四元数)预积分量，都表达在 bkb_k 机体系;RtbkR^{b_k}_t 为 tt 时刻机体系到 bkb_k 系的旋转;a^t,ω^t\hat{a}_t, \hat{\omega}_t 为加计/陀螺读数，bat,bwtb_{a_t}, b_{w_t} 为对应 bias;Ω(ω)\Omega(\omega) 为角速度的四元数导数矩阵。

离散递推(式 (7)，论文写 Euler，代码用中值):

α^i+1bk=α^ibk+β^ibk δt+12R(γ^ibk)(a^i−bai) δt2β^i+1bk=β^ibk+R(γ^ibk)(a^i−bai) δtγ^i+1bk=γ^ibk⊗[112(ω^i−bwi) δt](7)\begin{aligned} \hat{\alpha}^{b_k}_{i+1} &= \hat{\alpha}^{b_k}_{i} + \hat{\beta}^{b_k}_{i}\,\delta t + \tfrac{1}{2} R(\hat{\gamma}^{b_k}_{i})(\hat{a}_i - b_{a_i})\,\delta t^2 \\ \hat{\beta}^{b_k}_{i+1} &= \hat{\beta}^{b_k}_{i} + R(\hat{\gamma}^{b_k}_{i})(\hat{a}_i - b_{a_i})\,\delta t \\ \hat{\gamma}^{b_k}_{i+1} &= \hat{\gamma}^{b_k}_{i} \otimes \begin{bmatrix} 1 \\ \tfrac{1}{2}(\hat{\omega}_i - b_{w_i})\,\delta t \end{bmatrix} \end{aligned} \tag{7}

符号:ii 为区间内 IMU 样本序号，δt\delta t 为相邻样本间隔;R(γ^ibk)R(\hat{\gamma}^{b_k}_i) 为预积分旋转四元数对应的旋转矩阵;带 ⋅^\hat{\cdot} 表示用测量值(bias 取当前估计)计算的量。

一阶 bias 修正(式 (12)):优化中 bias 变化 δb\delta b 时不重传播，用预积分对 bias 的 Jacobian 线性修正:

αbk+1bk≈α^bk+1bk+Jbaα δbak+Jbwα δbwkβbk+1bk≈β^bk+1bk+Jbaβ δbak+Jbwβ δbwkγbk+1bk≈γ^bk+1bk⊗[112Jbwγ δbwk](12)\begin{aligned} \alpha^{b_k}_{b_{k+1}} &\approx \hat{\alpha}^{b_k}_{b_{k+1}} + J^{\alpha}_{b_a}\,\delta b_{a_k} + J^{\alpha}_{b_w}\,\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1}} &\approx \hat{\beta}^{b_k}_{b_{k+1}} + J^{\beta}_{b_a}\,\delta b_{a_k} + J^{\beta}_{b_w}\,\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1}} &\approx \hat{\gamma}^{b_k}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \tfrac{1}{2} J^{\gamma}_{b_w}\,\delta b_{w_k} \end{bmatrix} \end{aligned} \tag{12}

符号:JbaαJ^{\alpha}_{b_a} 等为预积分量对相应 bias 的 Jacobian(协方差传播时顺带递推得到);δbak,δbwk\delta b_{a_k}, \delta b_{w_k} 为加计/陀螺 bias 相对线性化点的变化量。协方差经 15×15 离散误差状态传播(P←FPFT+VQVTP \leftarrow F P F^T + V Q V^T，F/VF/V 为误差状态转移/噪声映射矩阵，QQ 为噪声协方差)，为 IMU 残差提供信息矩阵 — 完整推导见 VIO 预积分。

3.4 初始化 — 松耦合视觉惯性对齐

  1. 纯视觉 SfM:滑窗内找与最新帧视差 > 30 px 且共视 > 20 特征的帧，5 点法求相对位姿 → 三角化 → 其余帧用 EPnP 求解(算法见 PnP)— 得到以 c0c_0 为参考、up-to-scale 的位姿序列
  2. 陀螺 bias 标定:SfM 相对旋转 ↔ 预积分 γ\gamma 对齐，线性求 δbw\delta b_w，然后重传播
  3. 速度/重力/尺度(式 (16)):求解 XI=[vb0b0,…,vbnbn,gc0,s]\mathcal{X}_I = [v^{b_0}_{b_0}, \ldots, v^{b_n}_{b_n}, g^{c_0}, s] 的线性最小二乘;加计 bias 与重力耦合、短窗内不可观，初始化阶段忽略
  4. 重力方向迭代估计:固定模长 9.81，在切空间 2-DoF 参数化迭代;重力方向对齐世界系后旋转 yaw 对齐到 0

IMU 激励检查:窗内 Δv/Δt\Delta v / \Delta t 标准差 < 0.25 警告激励不足(代码层面仍继续尝试)。

3.5 滑窗紧耦合 VIO

滑窗结构 — 关键帧间 IMU 约束 + 共视特征，回环帧以固定位姿参与 (论文 Fig. 3)

全状态(式 (21)):n+1n+1 帧 IMU 状态 + 相机-IMU 外参 + m+1m+1 个特征逆深度:

X=[x0,…,xn,  xcb,  λ0,…,λm],xk=[pbkw,vbkw,qbkw,ba,bg],xcb=[pcb,qcb](21)\mathcal{X} = \left[ x_0, \ldots, x_n,\; x^b_c,\; \lambda_0, \ldots, \lambda_m \right], \quad x_k = \left[ p^w_{b_k}, v^w_{b_k}, q^w_{b_k}, b_a, b_g \right], \quad x^b_c = \left[ p^b_c, q^b_c \right] \tag{21}

符号:xkx_k 为第 kk 帧的 IMU 状态(世界系位置 pbkwp^w_{b_k}、速度 vbkwv^w_{b_k}、姿态四元数 qbkwq^w_{b_k}、加计/陀螺 bias);xcbx^b_c 为相机到 IMU 的外参(平移 + 旋转);λl\lambda_l 为第 ll 个特征在首观测帧下的逆深度。

MAP 目标(式 (22)):

min⁡X{∥rp−HpX∥2+∑k∈B∥rB(z^bk+1bk,X)∥Pbk+1bk2+∑(l,j)∈Cρ ⁣(∥rC(z^lcj,X)∥Plcj2)}(22)\min_{\mathcal{X}} \left\{ \left\| r_p - H_p \mathcal{X} \right\|^2 + \sum_{k \in \mathcal{B}} \left\| r_{\mathcal{B}}(\hat{z}^{b_k}_{b_{k+1}}, \mathcal{X}) \right\|^2_{P^{b_k}_{b_{k+1}}} + \sum_{(l,j) \in \mathcal{C}} \rho\!\left( \left\| r_{\mathcal{C}}(\hat{z}^{c_j}_l, \mathcal{X}) \right\|^2_{P^{c_j}_l} \right) \right\} \tag{22}

符号:第一项为边缘化先验(rpr_p 先验残差，HpH_p 先验信息);rBr_{\mathcal{B}} 为相邻帧 IMU 预积分残差，B\mathcal{B} 为全部 IMU 测量对，Pbk+1bkP^{b_k}_{b_{k+1}} 为预积分协方差(Mahalanobis 加权);rCr_{\mathcal{C}} 为视觉重投影残差，C\mathcal{C} 为全部(特征 ll， 观测帧 jj)对，ρ\rho 为鲁棒核。

IMU 残差(式 (24)，15 维 = 位置/速度/姿态切空间误差 + 两个 bias 游走):

rB=[Rwbk(pbk+1w−pbkw+12gwΔtk2−vbkwΔtk)−α^bk+1bkRwbk(vbk+1w+gwΔtk−vbkw)−β^bk+1bk2[(qbkw)−1⊗qbk+1w⊗(γ^bk+1bk)−1]xyzbak+1−bakbwk+1−bwk](24)r_{\mathcal{B}} = \begin{bmatrix} R^{b_k}_w \left( p^w_{b_{k+1}} - p^w_{b_k} + \tfrac{1}{2} g^w \Delta t_k^2 - v^w_{b_k} \Delta t_k \right) - \hat{\alpha}^{b_k}_{b_{k+1}} \\ R^{b_k}_w \left( v^w_{b_{k+1}} + g^w \Delta t_k - v^w_{b_k} \right) - \hat{\beta}^{b_k}_{b_{k+1}} \\ 2 \left[ (q^w_{b_k})^{-1} \otimes q^w_{b_{k+1}} \otimes (\hat{\gamma}^{b_k}_{b_{k+1}})^{-1} \right]_{xyz} \\ b_{a_{k+1}} - b_{a_k} \\ b_{w_{k+1}} - b_{w_k} \end{bmatrix} \tag{24}

符号:gwg^w 为世界系重力，Δtk\Delta t_k 为帧间时长;前三块依论文式 (24) 行序为 位置/速度/姿态，分别与预积分量 α^/β^/γ^\hat{\alpha}/\hat{\beta}/\hat{\gamma} 作差(姿态用 [⋅]xyz[\cdot]_{xyz} 取四元数虚部得切空间误差);后两块为 bias 随机游走残差。

视觉残差(式 (25)):特征 ll 在锚帧 cic_i 首观测、逆深度 λl\lambda_l，变换链 cam_i → imu_i → world → imu_j → cam_j 得预测 PlcjP^{c_j}_l，残差定义在单位球切平面上:

rC=[b1b2]T⋅(Pˉ^lcj−Plcj∥Plcj∥)(25)r_{\mathcal{C}} = \begin{bmatrix} b_1 & b_2 \end{bmatrix}^T \cdot \left( \hat{\bar{P}}^{c_j}_l - \frac{P^{c_j}_l}{\Vert P^{c_j}_l \Vert} \right) \tag{25}

符号:Pˉ^lcj\hat{\bar{P}}^{c_j}_l 为帧 jj 中实际观测反投影到单位球的射线;PlcjP^{c_j}_l 为按当前状态预测的特征位置(归一化后即预测射线);b1,b2b_1, b_2 为预测射线切平面的一组正交基 — 残差是两条射线之差在切平面上的投影(2 维)。

单位球面视觉残差 — 观测与预测射线之差投影到切平面 [b1 b2] (论文 Fig. 6)

切平面残差不依赖针孔模型 → 鱼眼/全景同样适用。逆深度参数化:远点 λ→0\lambda \to 0 数值稳定、每特征仅 1 维。Ceres 求解(DENSE_SCHUR + DogLeg)，视觉残差套鲁棒核(论文式 (22) 写 Huber，代码实现用 Cauchy，见 §7.6)，td 在线估计时换 ProjectionTdFactor(含 rolling shutter 行修正)。

3.6 边缘化 — 两段式策略 + FEJ

边缘化策略 — 次新帧是关键帧则 marg 最老帧;否则丢弃其视觉观测、保留 IMU 预积分 (论文 Fig. 7)

  • 次新帧是关键帧:边缘化最老帧及其锚定的路标，信息经 Schur 补压成先验 {rp,Hp}\{r_p, H_p\}
  • 次新帧不是关键帧:丢弃它的视觉观测(视差太小，信息近似重复)，IMU 预积分合并传给下一帧

Schur 消元:

[ΛaΛbΛbTΛc][δxaδxb]=[gagb]  ⇒  (Λc−ΛbTΛa−1Λb)δxb=gb−ΛbTΛa−1ga\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\; \left( \Lambda_c - \Lambda_b^T \Lambda_a^{-1} \Lambda_b \right) \delta x_b = g_b - \Lambda_b^T \Lambda_a^{-1} g_a

符号:δxa\delta x_a 为被边缘化的变量，δxb\delta x_b 为保留变量;Λ\Lambda 为信息矩阵(Hessian)分块，gg 为对应梯度;消元后括号项即保留变量的新信息矩阵(Schur 补)。先验的 Jacobian 在边缘化时刻固定(FEJ，First Estimates Jacobian)，之后只按 r=r0+J0 δxr = r_0 + J_0\, \delta x 一阶展开 — 保证信息矩阵一致性，可观性论证见 VIO 滑窗优化与边缘化。

3.7 重定位与 4-DoF 位姿图

  • 回环检测:DBoW2（视觉词袋库，二进制描述子的层次量化词典 + TF-IDF 图像检索，Gálvez-López & Tardós 2012）;除 VIO 的角点外，额外提 500 个 FAST（角点检测算法：判断像素邻域圆弧上连续多点亮度差超阈值即为角点）+ BRIEF（二进制鲁棒独立基础特征，随机采样像素对比较强度得到紧凑二进制描述子）专供回环(VIO 特征太少不够检索)
  • 特征找回:BoW 匹配 → 两步外点剔除(2D-2D 基础矩阵 RANSAC + 3D-2D PnP RANSAC)
  • 紧耦合重定位(式 (26)):回环帧观测作为第四类残差进滑窗优化，回环帧位姿固定 — 漂移在 VIO 内部被"拉回"，而非事后对齐
  • 4-DoF 位姿图(式 (27)-(29)):单目 + IMU 系统 roll/pitch 可观(重力)，只有 x/y/z/yaw 4 个自由度漂移。边残差(式 (28)):
ri,j=[R(ϕ^i,θ^i,ψi)−1(pjw−piw)−p^ij iψj−ψi−ψ^ij](28)r_{i,j} = \begin{bmatrix} R(\hat{\phi}_i, \hat{\theta}_i, \psi_i)^{-1} (p^w_j - p^w_i) - \hat{p}^{\,i}_{ij} \\ \psi_j - \psi_i - \hat{\psi}_{ij} \end{bmatrix} \tag{28}

符号:piw,ψip^w_i, \psi_i 为待优化的关键帧位置与 yaw;ϕ^i,θ^i\hat{\phi}_i, \hat{\theta}_i 为来自 VIO 的 roll/pitch(固定不优化);p^ij i,ψ^ij\hat{p}^{\,i}_{ij}, \hat{\psi}_{ij} 为 VIO(序列边)或回环检测(回环边)给出的相对平移/相对 yaw 量测。

序列边不加鲁棒核(VIO 已滤过)，回环边套 Huber 抑制误回环。位姿图支持下采样管理(大场景实验保留 2000 关键帧)。

4. Experiments & Results

论文有两个版本:arXiv v1 只给轨迹与误差曲线图;T-RO 2018 正式版补充了 EuRoC 全序列 RMSE 对比表(vs OKVIS/ROVIO)。下文数字以 arXiv v1 为准。

4.1 EuRoC

  • 全部序列稳定运行(含 V1_03_difficult);MH_03/MH_05 上轨迹与误差曲线对比 OKVIS:VINS+loop 平移误差最小
  • 论文坦承:OKVIS 的 roll/pitch 角精度更好：作者归因于 VINS 预积分的一阶近似(vs OKVIS 每次迭代重传播)

4.2 实测漂移(论文 §IX)

实验 路程 VINS (无回环) VINS+loop OKVIS
室内外混合 ~700 m 漂移 0.88% ≈ 0(终点误差 [−0.03， 0.09, 0.07] m) 2.36%
MAV 8 字飞行 62 m 0.29% — —
校园大场景 5.62 km / 1h34min — 闭环后全程一致 —
iPhone 7 AR ~264 m — 不漂 (Tango 开门场景漂移)

4.3 耗时(论文 Table I,i7-4790)

线程 模块 耗时 频率
1 特征检测 / KLT 15 / 5 ms 25 Hz
2 滑窗优化 50 ms 10 Hz
3 回环检测 / 位姿图 100 / 130 ms 15 Hz
— motion-only BA(移动端) ~5 ms 相机帧率

5. Ablation & Discussion

论文没有正式消融表，但实验本身构成对照:

  • 回环的价值:700 m 实验 0.88% → ≈0，直接量化 4-DoF 位姿图 + 紧耦合重定位的贡献
  • 一阶 bias 修正的代价:roll/pitch 精度输给 OKVIS — 实时性换精度的明确 trade-off
  • 为什么 4-DoF 而非 6-DoF 位姿图:roll/pitch 本就无漂移，优化它们反而把无漂移方向的误差摊进去 — 可观性结构决定优化结构
  • 为什么误差状态参数化(ESKF 思想，见 四元数 EKF):旋转误差用 3 维切空间量，不过参数化、远离奇异、二阶量可忽略

6. Strengths / Limitations / Future Work

Strengths:

  • 单目 + IMU 最小配置出公制尺度，完整 pipeline(初始化/VIO/回环/重定位/位姿图)开箱即用
  • 工程取舍清晰:一阶 bias 修正、两段式边缘化、4-DoF 位姿图 — 每一处都对应可观性/实时性论证
  • 跨平台验证:PC / 机载 i7-5500U 闭环控制 / iPhone

Limitations:

  • 一阶预积分近似 → 姿态角精度略逊 OKVIS
  • 单目固有:静止/匀速等退化运动下尺度不可观，初始化需要充分激励
  • 前端 KLT 光流在快速旋转/弱纹理下脆弱(无描述子重识别能力)

Future Work(论文原文):可观性在线评估与主动运动规划;面向海量消费设备的全参数在线标定;单目 VI 稠密建图。(后续演化:VINS-Fusion 加双目/GPS，VINS 系成为 GVINS 等工作的底座)

7. Code Walkthrough

三个 ROS 包:feature_tracker → vins_estimator → pose_graph，经 topic 串联(/feature_tracker/feature → /vins_estimator/keyframe_* → /pose_graph/match_points 回灌重定位)。

7.1 测量同步 — estimator_node.cpp

// getMeasurements(): 取出每帧图像 + 跨到图像时刻的全部 IMU (考虑 td)
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
    IMUs.emplace_back(imu_buf.front()), imu_buf.pop();
IMUs.emplace_back(imu_buf.front());   // 跨界样本,process() 里线性插值到图像时刻

imu_callback 同时做高频中值传播(tmp_P/Q/V)发 IMU 帧率里程计;process() 线程消费测量对，依次 processIMU → processImage → 发布。

7.2 预积分 — integration_base.h

// midPointIntegration(): 中值积分 (论文式 (7) 是 Euler,代码实际用中值)
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr   = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q    = delta_q * Quaterniond(1, un_gyr(0)*_dt/2, un_gyr(1)*_dt/2, un_gyr(2)*_dt/2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc   = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p    = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v    = delta_v + un_acc * _dt;

15×15 Jacobian/协方差随样本递推;evaluate() 输出式 (24) 的 15 维残差(含式 (12) 一阶 bias 修正);repropagate() 只在初始化阶段调用(solveGyroscopeBias() / visualInitialAlign() 修正陀螺 bias 后)— 优化循环里从不重传播，始终走一阶修正。

7.3 Ceres 因子

因子 SizedCostFunction 参数块
IMUFactor <15, 7, 9, 7, 9> pose_i, speedbias_i, pose_j, speedbias_j
ProjectionFactor <2, 7, 7, 7, 1> pose_i, pose_j， 外参， 逆深度
ProjectionTdFactor <2, 7, 7, 7, 1, 1> + td
MarginalizationFactor 动态 先验涉及的全部保留块
  • 全部手写解析 Jacobian;位姿用自定义 PoseLocalParameterization(四元数流形，GlobalSize 7 / LocalSize 6)
  • IMU 信息矩阵用 Cholesky 平方根预乘:sqrt_info = LLT(covariance.inverse()).matrixL().transpose() — 数学同 Σ−1\Sigma^{-1}，数值更稳
  • 投影链(ProjectionFactor::Evaluate，即式 (25) 的链):
Vector3d pts_camera_i = pts_i / inv_dep_i;            // 锚帧反投影
Vector3d pts_imu_i    = qic * pts_camera_i + tic;
Vector3d pts_w        = Qi * pts_imu_i + Pi;
Vector3d pts_imu_j    = Qj.inverse() * (pts_w - Pj);
Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
residual = (pts_camera_j / pts_camera_j.z()).head<2>() - pts_j.head<2>();

注意:默认编译用针孔平面残差;论文的单位球切平面残差在 #define UNIT_SPHERE_ERROR 后启用(parameters.h 中默认注释掉，EuRoC 用平面版)。残差对相机系点的 Jacobian 中间项:

∂r∂p=[1z0−xz201z−yz2]\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}

其中 (x,y,z)(x, y, z) 为特征在目标相机系下的 3D 坐标，rr 为归一化平面上的 2 维重投影残差 — 这是链式法则里"残差对相机系点"的一环。

四元数约定 Hamilton,Jacobian 旋转项用左/右乘矩阵 Utility::Qleft/Qright:

p⊗q=[p]L q=[q]R p,[q]L=qwI+[0−qvTqv[qv]×],[q]R=qwI+[0−qvTqv−[qv]×]p \otimes q = [p]_L\, q = [q]_R\, p, \qquad [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}

其中 qw,qvq_w, q_v 为四元数的实部/虚部，[qv]×[q_v]_\times 为虚部的反对称矩阵;[⋅]L/[⋅]R[\cdot]_L / [\cdot]_R 把四元数乘法写成对另一乘数的线性矩阵作用，便于求导。四元数代数见 旋转与刚体变换 §11。

7.4 优化与滑窗 — estimator.cpp

optimization():边缘化先验 + 相邻帧 IMUFactor + 共视特征 ProjectionFactor(+ 重定位帧投影);求解器 DENSE_SCHUR + DOGLEG，时间预算 max_solver_time=0.04s / max_num_iterations=8(MARGIN_OLD 路径再砍到 4/5);视觉残差套 CauchyLoss(1.0)(注释里的 Huber 被弃用)，IMU 因子不套鲁棒核。

slideWindow():MARGIN_OLD → 数组整体前移、重建 pre_integrations[WINDOW_SIZE]、特征重锚定;MARGIN_SECOND_NEW → 次新帧预积分合并进新帧、覆盖状态。marginalization_factor.cpp 用多线程组装 Hessian，特征值分解求 Schur 补并取平方根因子存成 linearized_jacobians/residuals(FEJ)。

7.5 关键参数 — euroc_config.yaml + parameters.h

参数 值 说明
WINDOW_SIZE 10(硬编码) 滑窗 11 帧位姿
FOCAL_LENGTH 460(硬编码) 虚拟焦距:视差阈值换算 + sqrt_info = 460/1.5 · I 固定视觉权重
max_cnt / min_dist 150 / 30 特征数 / 最小间距
freq 10 Hz 前端发布频率
keyframe_parallax 10.0 px ÷460 → 归一化视差阈值
acc_n / gyr_n 0.08 / 0.004 IMU 噪声密度
acc_w / gyr_w 4e-5 / 2e-6 bias 随机游走
estimate_extrinsic 0/1/2 固定 / 在线优化 / 从零标定
estimate_td / td 0 / 0.0 时间偏移在线估计

7.6 Paper-vs-Code 差异

# 差异 说明
1 积分方案 论文式 (7) 写 Euler，代码全程中值积分(Euler/RK4 注释掉)
2 视觉残差 论文主推单位球切平面;代码默认针孔平面残差，球面版编译开关关闭
3 bias 重传播 论文提"必要时重传播";代码只在初始化(陀螺 bias 标定后)重传播，优化路径从不重传播、只用一阶修正
4 鲁棒核 论文式 (22) 写 Huber;代码视觉因子实际用 CauchyLoss(1.0)
5 视觉信息矩阵 固定 FOCAL_LENGTH/1.5 各向同性权重，与实际相机标定无关
6 FOCAL_LENGTH=460 为 EuRoC(fx≈458)选定的常数，换相机系统不会自动适配
7 滑窗大小 论文未给数值;代码硬编码 10
8 2D-2D 基础矩阵 RANSAC 论文描述回环特征验证用基础矩阵 RANSAC；keyframe.cpp 中该调用被注释掉，实际走 PnP RANSAC 单轮验证

References

资源 链接
VINS-Mono 论文 arXiv:1708.03852, IEEE T-RO 2018
官方代码 (PC/ROS) HKUST-Aerial-Robotics/VINS-Mono
VINS-Mobile (iOS) HKUST-Aerial-Robotics/VINS-Mobile
预积分理论 Forster et al., On-Manifold Preintegration, IEEE T-RO 2017
DBoW2 Gálvez-López & Tardós, IEEE T-RO 2012
知乎: VINS-Mono 四元数推导 VINS-Mono 四元数雅可比推导
贺一家: 边缘化讲解 VINS-Mono 边缘化详解

相关笔记:理论体系见 VIO 预积分、VIO 滑窗与边缘化、VIO 初始化;边缘化、四元数 EKF、光流;同类系统对照 OKVIS(多相机滑窗 VIO)、ORB-SLAM2(纯视觉特征法)。