Image 和 IMU 时间戳同步
1
目录
时间戳同步介绍
时间戳同步初探
时间戳获取方式
时间戳延迟估计方法
姿态补偿
匀速模型在 Bundle Adjustment 中的应用
重投影残差
视觉特征匀速运动补偿特征坐标
重投影残差
基于特征匀速模型算法的实验结果
轨迹匀速模型 vs 特征匀速模型
时间戳同步算法扩展
其他时间戳延迟估计算法简介
时间戳同步介绍
时间戳同步问题及意义
什么是时间戳同步
为什么要时间戳同步
2
时间戳延迟估计方法
基于轨迹匀速模型的时间戳补偿算法
基于视觉特征匀速模型的时间戳补偿算法
两匀速模型算法对比
3
时间戳同步算法扩展
其他时间戳延迟估计算法
VIO 初始化阶段的时间戳估计
时间戳同步初探
时间戳用途：传感器数据被采集时刻 t 标注，用于数据对齐融合。
VIO 例子：如图1中上部分所示，1 时刻相机姿态已知，其 2 时刻的姿态可以通过 1 到 4 之间的 IMU 数据进行预测。错误的时
间戳将导致错误的姿态约束。
时间戳获取方式
1 硬件同步时间戳：IMU 产生数据的同时硬件中断触发相机曝光进行严格同步。
2 异步时间戳：传感器独立生产数据，时间戳为数据到达 cpu 的时刻，时间存在延迟。
timestamp :
t imu s = t + t imu d , tcam s = t + t cam d
time delay :
td
.
= t imu d − t cam d = t imu s − t cam
只需要以 IMU 时刻为准，用 td 将数据对齐，多传感器融合精度将更高
深入：时间戳不同步具体如何影响 VIO 精度？
如图所示，获取实际图像时相机坐标系为 c1，对应时间戳已同步的 IMU 坐标系 b1。
若将 c1 时间戳加上一个延迟 td，对应坐标系为 c ′ 1。三维点云将被投影到 c ′ 1 坐标下，
计算重投影误差时误差将增大。
VIO 在优化视觉重投影误差和 IMU 预积分约束的最小二乘问题时，能否多估计一个
td 呢
时间戳延迟估计方法
相机匀速运动模型补偿姿态
思路一：姿态由于时间戳延迟而有误差，那就补偿姿态
姿态补偿
假设 t 为对齐时刻，延迟量 td，延迟后时间戳为 t cam，
则 t 时刻的相机姿态为
Twc(t) = Twb (t cam − td) · Tbc (2)
具体地，假设相机匀速运动，t cam 时刻 IMU 坐标系速
度为 v w bi，角速度测量值为 ωbi (忽略 bias)，补偿公式为
Rwbi
,
−td ≈ Rwbi Exp (−ωbi td)
pwbi,−td ≈ pwbi − v w bi td (3)
补偿后的相机姿态为
Rwci = Rwbi,−tdRbc ≈ Rwbi Exp (−ωbi td) Rbc
pwci = Rwbi,−td pbc + pwbi,−td
≈ Rwbi Exp (−ωbi td) pbc + pwbi − v w bi td
匀速模型在 Bundle Adjustment 中的应用
VIO 的后端优化：通过最小化滑动窗口中的残差项来估计系统的状态 变量
时间戳延迟变量只对先验残差和视觉重投影误差有影响。IMU 预积分误差中由于不
涉及相机传感器数据，不受时间延迟的影响。
先验残差类似其他变量，处理信息矩阵 H 时多一个变量并进行对应 marg 就行。接
下来，主要讲解重投影残差部分。
重投影残差
不考虑时间戳延迟时，残差计算流程：
世界坐标系中的第 l 个地图点 f l w = [x, y, z] ⊤，先变换到相机坐标 系下
f l ci = RcbRwb ⊤ i (fl w − pwbi ) + pcb (6)
再投影到图像平面，并计算重投影残差：
rc = [ x z l i i l − u i l y i l z i l − vl i ]⊤ (7)
考虑时间戳延迟时，坐标系变换公式如下，
f l ci = Rcb Exp (ωbi td) Rwb ⊤ i ( fl w − pwbi + vb w i td ) + pcb
重投影残差雅克比计算
重投影残差对时间戳延迟 td 的雅克比由链式求导可得，J = ∂ ∂ f r ci l ∂f ci l ∂δtd
相机坐标系下地图点坐标可以近似为
视觉特征匀速运动补偿特征坐标
思路二：时间戳延迟使得特征的重投影坐标和真实坐标之间存在误差，那是否可以直
接补偿特征坐标
特征坐标补偿
假设视觉特征在图像平面上匀速移动，如下图示
特征的运动速度为：
重投影残差
同样地，我们只对 VIO 后端中的重投影残差受到时间戳延迟的影响进行介绍。
残差计算 无时间戳延迟时，残差计算流程：
1 世界坐标系中的第 l 个地图点
fl w = [x, y, z] ⊤，先变换到相机坐标系下
f l ci = RcbRwb ⊤ i (fl w − pwbi ) + pcb
2 再投影到图像平面，并计算重投影残差：
rc = [ x i l z i l − u i l y i l z i l − vl i ]⊤
考虑时间戳延迟时，特征坐标进行补偿
基于特征匀速模型算法的实验结果
将特征匀速模型算法应用于 vins-mono，在 EuRoc 数据集 MH03 上的测试，给图像
时间戳人为加上 [−40, 40]ms 的延迟。轨迹误差结果对比如下图所示
两个结论：
• Vins-Mono 轨迹误差随着延迟的增大而增大，而应用了时间戳补偿的算法，轨
迹误差几乎没有变化，验证了算法的有效性。
• 从误差的大小可以看出，几十 ms 的延迟，能让系统精度下降非常多
轨迹匀速模型 vs 特征匀速模型
算法优缺点总结
• 特征匀速模型：计算简单，代码改动小。然而视觉特征在图像平面是非线性运
动，特征匀速的假设精度低。
• 轨迹匀速模型：相比于特征匀速，轨迹匀速的假设更接近实际，模型精度高。
但雅克比矩阵等计算复杂，代码改动多
从图中可知，基于轨迹匀速模型的时间戳收敛速度以及估计的准确度都高于基于特征
匀速模型的算法
时间戳同步算法扩展
其他时间戳延迟估计算法简介
前面的算法都是通过时间戳延迟去补偿相机姿态，还有众多其他思路，部分论文如下：
• 补偿图像时间戳，改为补偿 IMU 时间戳：每优化一次 IMU 时间戳，IMU 预积
分需要重新计算一次6。没有匀速假设，模型精度更高，但是重新预积分复杂度
更高。
• IMU 积分对齐相机测量，改为相机姿态微分对齐 IMU 测量：基于角速度测量对
齐的算法7，基于 B 样条微分的算法8，B 样条雅克比解析解
VIO 系统初始时，时间延迟 td 如何估计？
前面算法为系统初始化后，通过后端优化实时估计时间延迟。那系统初始化过程中，
能否估计时间延迟呢？
初始化阶段，相机关键帧的姿态通过 VO/SFM 等可求解，有
Rwbi = Rwci,tdRcb,
Rwbj = Rwcj ,tdRcb
跟 IMU 预积分旋转分量构建误差，如下
（可回顾 vio 初始化章节）
eR = log ( ∆Ri,jRwb ⊤ iRwbj )
考虑陀螺仪 bias, 时间延迟 td 有：
eroti,j = log (( ∆Ri,j Exp ( J g ∆R δbg ))⊤ Rbc ·Exp (−ωci td) RciwRwci+1 Exp ( ωci+1 td ) Rcb)
多时刻的观测构建最小二乘求解