如何使用GTSAM完成Camera-IMU时间偏移的因子图建模与估计

核心建模逻辑 #

两类传感器的时间偏移本质是一个（或一组随时间慢变的）待估标定参数，完全可以融入因子图和其他状态量做联合优化，不需要前置单独标定：

• 首先定义时间偏移参数td，满足关系：相机采样时刻 = IMU采样时刻 + td，固定偏移场景下td是全局常量，工业级传感器一般偏移在几十ms量级，消费级设备可能到上百ms。
• 改造常规IMU预积分因子：原预积分是按相机原始时间戳截取IMU序列做帧间积分，存在时间偏移时，实际和相机帧对齐的IMU时刻要偏移td，积分区间边界落在两个离散IMU采样点之间时，对相邻IMU测量做线性插值补全边界值，再完成预积分计算。
• 把td作为独立变量节点加入因子图，所有帧间IMU预积分因子都关联对应时刻的td节点，推导预积分残差对td的雅可比，优化时同步更新位姿、速度、IMU零偏、外参、td所有状态量。
• 如果传感器时钟存在漂移、偏移不是固定值，可以按时间窗口拆分td为多个节点（比如每10个关键帧对应一个td节点），相邻td节点之间加弱约束的随机游走因子，避免估计值跳变。

GTSAM落地步骤 #

不需要魔改GTSAM核心代码，基于原生接口扩展即可：

• 变量定义：位姿用gtsam::Pose3、速度用gtsam::Vector3、IMU零偏用gtsam::imuBias::ConstantBias，时间偏移直接用double类型对应符号即可，比如用字符t作为td的key前缀，全局固定偏移只需要建一个td节点。
• 预积分模块改造：可以继承GTSAM原生的gtsam::PreintegratedImuMeasurements类扩展：
  1. 每次构造帧间预积分时，根据当前td的估计值，计算前后两帧相机对应的实际IMU对齐时刻，从IMU缓存中截取对应区间的测量值
  2. 对区间首尾不落在IMU离散采样点的时刻，用相邻两个IMU的角速度、加速度做线性插值，得到边界时刻的测量值后再按常规流程计算预积分量、噪声协方差
  3. 额外计算预积分残差对td的雅可比：本质是td变化带来积分区间边界移动，边界处IMU测量值变化导致的预积分结果变化，推导难度低，常规VIO的在线标定模块都有现成推导可以复用。
• 自定义因子类：继承gtsam::NoiseModelFactor，因子关联的变量包括前后两帧的位姿、速度、IMU零偏，以及对应时刻的td节点，在evaluateError接口中实现残差计算，返回残差对所有关联变量的雅可比即可。
• 优化配置：td初始值一般设0即可，如果提前知道偏移方向可以直接给粗略初始值；给td加一个弱先验因子，噪声模型选标准差50~100ms的高斯模型即可，既避免td无约束发散，也不会限制参数收敛；优化器选常规的Levenberg-Marquardt或者Dogleg都可以，正常迭代优化即可得到收敛的td估计值。

实践经验与踩坑提示 #

• 这套思路的逻辑和VINS-Mono/VINS-Fusion里的在线时间标定完全一致，只是把滑动窗口优化的实现换成GTSAM因子图框架，核心的预积分补偿、雅可比推导部分可以直接参考成熟开源实现的逻辑，不需要从零推公式。
• 几个容易出问题的点：
  • 时间偏移和IMU零偏、相机-IMU外参存在耦合，只有当运动存在足够的角加速度、线加速度激励时，td才是可观的，静止、匀速运动场景下估计出的td没有参考意义。
  • 优化迭代过程中如果td更新量比较大，不要一直用第一次计算的预积分量，可以每迭代2~3次根据最新的td重新计算预积分，避免一阶雅可比近似误差过大导致不收敛。
  • 如果使用卷帘快门相机，要把传感器间时间偏移和卷帘快门行延迟分开建模，不要把行读出带来的时间差算到td里，否则估计结果会有固定偏差。
  • td的先验噪声不要设太小，否则相当于固定td为初始值失去标定意义，也不要设太大（比如超过1s标准差），否则容易和其他参数耦合导致估计飘移。

内容的提问来源于stack exchange，提问作者milin