1. 从一张框图说起IMU预积分模块到底在解决什么问题在VINS-Mono里待过一阵子的人应该都有这种感觉整个系统跑通不难但想读懂IMU预积分那部分代码确实要费点劲。网上资料虽然多但大多只讲数学公式真正落到代码层面、能带着你把processIMU()、integrationBase类、imu_factor.h这三个东西串起来讲的很少。先聊点背景。视觉惯性里程计的核心问题之一是IMU的频率通常有100Hz到200Hz而图像帧只有10Hz到30Hz。如果按传统方式每一帧图像到来时都要把两帧图像之间所有IMU测量值当作一个整体去重新积分那么每优化一次位姿就要重新积一次分计算的量非常大而且高频IMU数据在非线性优化里会变成巨大的待优化变量优化器根本扛不住。预积分的核心动机就是把这个积分操作“挪到”优化之前完成。它把两帧之间的IMU相对运动旋转增量、速度增量、位置增量事先算好作为优化中的“测量值”而优化过程中只对帧的位姿、速度、零偏这些状态量做调整不去动原始IMU数据。理解了这点再看代码就会清晰很多。整个模块其实就三个层次processIMU()负责接收IMU数据判断当前数据属于哪个图像帧区间并驱动预积分器做增量积分计算。integrationBase类预积分器的核心实现里面存了增量旋转、增量速度、增量位置、协方差矩阵以及Jacobian矩阵中值积分逻辑也都在这里。imu_factor.h把预积分结果封装成一个Ceres因子供后端非线性优化调用。残差的计算、Jacobian矩阵的填充、信息矩阵的传递都在这个文件里。这套接口设计其实非常顺手。如果你只想跑通VINS-Mono未必需要动这部分代码但如果你想改传感器配置、换IMU型号、改预积分策略或者干脆想移植到自己的系统里那么这三个文件就是绕不开的坎。这篇文章我按数据流动的顺序从processIMU()一路讲到imu_factor.h把里面每一处关键代码的意图和隐含的数学原理都过一遍。2. processIMU()深度拆解预积分的输入入口2.1 调用时机与前置规则processIMU()在VINS-Mono里是VIO状态机的核心入口之一它不属于integrationBase类而是在estimator流程里以独立函数存在。每次拿到一帧IMU数据不管是来自数据集还是RTK实采系统都会调用这个函数。调用时的核心任务可以归纳成一句话判断当前IMU数据应该属于哪一组相邻关键帧然后把该数据对应的时间间隔内的加速度和角速度塞进对应的预积分器。这里有个重要的前置规则就是第一帧图像之前的IMU数据怎么处理。VINS-Mono里会有一个first_imu标志位用来处理IMU数据流还没有对应图像帧的情况——也就是系统刚开始跑、还没初始化完成的那几秒钟。这段路上的IMU数据只用来构建初始的旋转估计不进入预积分过程。而一旦图像帧建立起来后面的IMU数据就按照图像帧的时间戳划分成一段一段的。实现上并不复杂void Estimator::processIMU(double t, double gx, double gy, double gz, double ax, double ay, double az) { // 如果是第一次收到IMU数据记录时间戳不进入积分逻辑 if (!first_imu) { first_imu true; acc_0 Vector3d(ax, ay, az); gyr_0 Vector3d(gx, gy, gz); } // 判断当前IMU数据是否落在当前帧与上一帧之间 if (!pre_integrations[frame_count]-push_back(dt, acc_1, gyr_1, acc_0, gyr_0)) { // 如果超出当前帧区间说明需要建立新的预积分器 frame_count; pre_integrations.push_back(new IntegrationBase(...)); } // 更新当前加速度和角速度 acc_0 Vector3d(ax, ay, az); gyr_0 Vector3d(gx, gy, gz); }关键就在于push_back()返回值的判断逻辑。它的字面意思是“把这段IMU数据积累到当前预积分器里”实际上内部会检查如果当前IMU时间戳没有超过当前图像帧的时间戳就正常积累如果已经超了说明这段IMU数据已经不属于当前帧区间就需要新建一个预积分器给下一帧。这个设计保证了每个预积分器恰好覆盖两帧图像之间的所有IMU数据不多不少。2.2 时间对齐与帧间判断时间对齐是IMU预积分最容易出错的地方尤其是用数据集的时候。VINS-Mono默认的时间戳单位是秒使用EuRoC数据集时是同步好的但换到自己采集的数据或者TUM数据集时时间戳乱一点整个VIO就跑飞。processIMU里没有做复杂的时间插值而是假设IMU数据和图像帧时间戳已经是对齐了的只做区间归属判断。因此在实际使用中你必须保证喂进去的数据时间戳准确否则预积分的结果就是错的而且这种错非常隐蔽——不会崩但精度会慢慢漂。帧间判断的逻辑可以这样理解系统里有一个frame_count表示当前正在处理的图像帧索引每个图像帧都对应一个独立的pre_integrations[frame_count]。IMU数据来了以后先看它的时间戳t是否小于当前帧时间戳frame_stamp[frame_count]。如果是就正常把它放入当前预积分器如果不是就说明这一帧图像之间的IMU数据已经收齐程序会把frame_count加一然后为新的帧区间创建新的预积分器。这里其实还有一个容易被忽略的细节就是dt的计算。VINS-Mono在processIMU里取了当前IMU时间戳与上一帧IMU时间戳的差值。如果IMU数据频率稳定这个dt值基本稳定在0.005秒或0.01秒左右但IMU数据本身有抖动时dt也会跟着抖动这会对中值积分的精度有轻微影响。2.3 中值积分的两次更新细节进入pre_integrations[frame_count]-push_back()以后真正的预积分计算才开始。VINS-Mono默认使用中值积分也就是每个IMU采样间隔内取当前时刻和下一时刻的加速度、角速度的平均值作为该间隔内的常量值。这个过程通常涉及两次更新。第一次是先用当前时刻的测量值做一次积分可以理解为“预测”第二次是把下一时刻的测量值平均进去再做一次校正。这样做的效果相当于在采样间隔内用一个更平滑的角速度和加速度来替代原来的分段常量假设精度比欧拉积分高很多而计算量只增加了一倍性价比非常高。中值积分的具体实现放在push_back()函数里它会根据当前保存的加速度acc_1、角速度gyr_1以及新进来的acc_0、gyr_0计算平均值然后调用midPointIntegration()函数更新预积分量、协方差和Jacobian。这个函数是integrationBase类里最核心的函数也是下面要重点拆解的对象。3. integrationBase类预积分量、协方差与Jacobian的载体3.1 类内部的关键成员变量integrationBase这个类是整个预积分模块的“数据仓库”。如果你打开vins_estimator的代码目录找到这个类的定义第一眼会被它的一堆成员变量吓到。但只要搞清楚哪些是预积分量、哪些是协方差、哪些是Jacobian、哪些是中间变量整个类就清晰了。核心预积分量有四个delta_q当前帧相对于参考帧的旋转增量用四元数表示对应数学上的ΔR。delta_v速度增量对应Δv。delta_p位置增量对应Δp。linearized_acc和linearized_gyr最近一次的加速度和角速度零偏值。协方差和Jacobian相关的成员包括jacobian和covariance这两个矩阵在优化中非常重要。需要特别注意的是代码里还有一个sum_dt用于累计预积分间隔的总时间。这个变量看似不起眼但实际上在残差计算中会用到因为位置残差和速度残差都要乘以时间间隔。另外integrationBase里还保存了acc_0、gyr_0等用来做中值积分的上一时刻测量值。这些值不是IMU的原始数据而是参与预积分计算时记录下来的前一帧IMU数据目的就是为了下一帧数据到来时能求平均值。3.2 push_back与中值积分实现push_back()函数的结构很清晰接收两帧IMU数据判断时间间隔是否有效然后更新预积分。它的本质工作是调用midPointIntegration()但在这之前还需要维护一些状态量。假设某次调用中上一帧IMU加速度是acc_1角速度是gyr_1当前帧的IMU测量值是acc_0和gyr_0。那么中值积分会先计算acc_mid (acc_1 acc_0) / 2 gyr_mid (gyr_1 gyr_0) / 2然后假设这个acc_mid和gyr_mid在dt时间内保持不变进行一次积分。也就是说预积分量的更新公式大致是delta_q delta_q * quaternion_delta(gyr_mid * dt) delta_v delta_v delta_q * acc_mid * dt delta_p delta_p delta_v * dt 0.5 * delta_q * acc_mid * dt^2当然这只是忽略零偏和重力项后的简化表达。真实的代码实现里需要把零偏的影响也考虑进去如果是带零偏更新的版本还要额外对Jacobian和协方差做递推。这里有一个值得一提的细节中值积分在旋转上的更新不是简单的加法。因为旋转本身是非线性的我们需要把角速度积分得到的旋转增量转换成四元数再用四元数乘法来更新delta_q。这正好解释了为什么delta_q的类型是Quaterniond而不是Vector3d。3.3 协方差与Jacobian的递推方程协方差和Jacobian的更新是midPointIntegration()的核心。初学者经常会问一个问题为什么要维护这两个矩阵这要从优化角度来理解。协方差的物理意义是预积分量随时间推演的不确定度累积。IMU测量有噪声每一次积分都会引入新的误差误差会随着时间累积。如果不给优化器提供协方差信息优化器就无法判断“这个预积分量到底可信多少”进而影响信息矩阵的权重分配。VINS-Mono默认把协方差的初值设为零矩阵然后递推更新。Jacobian的意义则更直接。在优化过程中我们要计算预积分残差对状态变量的导数。如果每次计算导数时都用数值微分去算效率太低而且数值不稳定。所以VINS-Mono采用了一种更聪明的办法先在预积分阶段就把Jacobian的递推算好优化时直接读取。公式上Jacobian的递推可以写成J_{tdt} F * J_t其中F是状态转移矩阵它描述了当前时刻的状态误差如何影响下一时刻的状态误差。协方差递推则是P_{tdt} F * P_t * F^T G * Q * G^T其中G是噪声驱动矩阵Q是IMU噪声协方差矩阵。在代码里这个F矩阵是一个15×15的大矩阵这里有个值得注意的细节——这15个维度其实包含了位置、速度、旋转、加速度零偏、角速度零偏这五组状态量每组3个。这个维度的选择并不是随意的它对应了后端优化时IMU因子关联的状态变量集合。3.4 midPointIntegration的两个版本对比如果你仔细读过integrationBase的代码会发现midPointIntegration在代码里有过不同的版本早期版本和后期版本在“零偏更新”的处理方式上有差异。老版本的做法是在预积分量更新时把最近的零偏值带入积分公式每次IMU数据进来都用最新的零偏值去重新计算。这样做的问题在于零偏一旦变化整个预积分量都需要重新计算而且前期积分的Jacobian也会受影响对优化收敛非常不友好。VINS-Mono最终版本选择了“先按固定零偏积分再通过Jacobian做修正”的策略。具体来说midPointIntegration在预积分推进时使用固定的零偏值同时把零偏误差的影响以Jacobian的形式记录下来。后端优化时如果零偏估计值发生变化预积分残差会通过Jacobian来近似修正而不需要重新积分。这个设计的巧妙之处在于预积分量本身只需要随IMU数据积累一次优化过程中的零偏修正完全靠线性化来近似计算效率大幅提升。当然这种近似在零偏变化很大时会失效所以代码里还设置了一个阈值比如零偏变化超过某个范围时强制重新积分。4. 残差公式与Jacobian推导imu_factor.h的数学内核4.1 预积分残差的定义与形式imu_factor.h是整个IMU预积分模块与Ceres优化器的衔接点。要理解这个文件首先要理解它定义的残差是什么。预积分残差的思路很直白预积分量是“两帧之间的相对运动测量值”而状态变量可以预测出一个“两帧之间的相对运动”。这两个值的差就是残差。更具体一点假设当前关键帧的状态为位置p_i、速度v_i、旋转q_i下一关键帧的状态为p_j、v_j、q_j。预积分模块已经算出了两帧之间的旋转增量delta_q、速度增量delta_v、位置增量delta_p。那么残差可以写成r_p q_i^inv * (p_j - p_i - v_i * dt - 0.5 * g * dt^2) - delta_p r_v q_i^inv * (v_j - v_i - g * dt) - delta_v r_q 2 * (delta_q^inv * (q_i^inv * q_j)).vec()其中g是重力向量.vec()表示取四元数的虚部。这三个残差分别对应位置、速度、旋转。不过在imu_factor.h里残差并不止这三项。因为优化还要估计加速度计零偏ba和陀螺仪零偏bg所以残差里还隐含了对零偏的约束。VINS-Mono的处理方式是把零偏作为优化变量的一部分在残差计算时根据当前估计的零偏值与预积分时使用的零偏值的差异通过Jacobian的预积分修正来调整delta_p、delta_v、delta_q。这也是为什么imu_factor.h里会看到类似eigen_velocity、eigen_q、eigen_vector这样的中间变量。4.2 对优化变量的Jacobian推导残差有了接下来就是要对状态变量求导。VINS-Mono中IMU因子的优化变量一共有七个按顺序是位置p_i3维姿态q_i4维四元数速度v_i3维加速度零偏ba_i3维角速度零偏bg_i3维位置p_j3维姿态q_j4维四元数其中速度和零偏只涉及本帧不涉及另一帧所以相关的Jacobian块会相对简单。在实际代码中我们需要计算残差对每个优化变量的一阶偏导数这些偏导数拼成一个大矩阵就是imu_factor.h里最重要的jacobian矩阵。以位置残差为例对p_i和p_j的导数尤其直观。残差里包含q_i^inv * (p_j - p_i - v_i * dt - 0.5 * g * dt^2)对p_i求导得到-q_i^inv对p_j求导得到q_i^inv。对v_i求导则得到-q_i^inv * dt。这些结果其实和普通惯性导航方程里的推导一致。绕一点的是对姿态q_i的Jacobian因为姿态以四元数形式出现在残差里而且和预积分旋转增量耦合在一起。代码里一般通过扰动模型来推导先给q_i一个微小的旋转扰动然后看残差的变化量提取出线性项作为Jacobian。4.3 零偏更新的扰动模型零偏的Jacobian是IMU因子里最复杂、也最容易写错的部分。它的困难在于预积分量delta_p、delta_v、delta_q本来是在固定零偏假设下算出来的但现在优化过程中零偏会变所以必须通过Jacobian来补偿这个变化。具体的做法可以这样理解。在预积分阶段integrationBase已经算好了delta_q对bg的Jacobian记为J_q_bg以及delta_v和delta_p对ba、bg的Jacobian记为J_v_ba、J_v_bg、J_p_ba、J_p_bg。这些Jacobian在midPointIntegration里已经递推好了直接存放在类的成员变量里。在imu_factor.h的残差计算中当后端优化把零偏从ba_i、bg_i更新成新的值以后程序会用这些Jacobian对预积分量做一阶修正比如delta_p_corrected delta_p J_p_ba * (ba_i - linearized_ba) J_p_bg * (bg_i - linearized_bg)然后残差的真正形式是r_p q_i^inv * (p_j - p_i - v_i * dt - 0.5 * g * dt^2) - delta_p_corrected这里用到的linearized_ba和linearized_bg就是预积分时保存下来的零偏值。这样做的好处是后端优化时不需要重新积分IMU数据只需要用Jacobian做一个线性修正就能近似得到新的预积分量。需要注意的一点是残差对零偏的Jacobian不只是上面那个预积分Jacobian还要加上修正项本身对零偏的导数。完整的Jacobian是这两部分的组合在代码中以sqrt_info乘上各分块矩阵的形式体现。5. imu_factor.h接入ceres从公式到代码的最后一公里5.1 Factor类结构与Evaluate()函数imu_factor.h里的IMU因子类继承自Ceres的SizedCostFunction模板参数是残差维度15维和各优化变量的维度。Evaluate()是整个因子最核心的入口函数Ceres每次迭代都会调用它。Evaluate()函数的输入是优化变量的当前值数组parameters输出是残差residuals和Jacobian矩阵jacobians。整体流程可以分为几个阶段第一步是解析参数。代码里会把parameters[0]到parameters[6]分别解析成位置、姿态、速度、零偏等Eigen类型的变量。这里要特别小心Ceres里四元数的存储顺序是w, x, y, z而VINS-Mono内部很多地方使用的是x, y, z, w的顺序在传递参数时如果不做转化姿态就会完全错乱。第二步是计算修正后的预积分量。代码会根据当前零偏值和预积分时保存的零偏值用Jacobian修正delta_p、delta_v、delta_q。相关代码在IntegrationBase::evaluate()函数中封装好了。第三步是计算残差向量。按照上一节列出的公式逐个填充这15维残差。顺序通常是位置残差3维、速度残差3维、旋转残差3维、零偏残差33维。VINS-Mono里还有一个值得注意的操作在残差计算完成后会乘以sqrt_info矩阵也就是信息矩阵的平方根。这相当于对残差做了白化处理让残差的协方差接近单位阵有利于Ceres的数值稳定性。5.2 信息矩阵的构造与鲁棒核sqrt_info矩阵的计算是IMU因子中比较细节但也非常关键的一步。它的来源是预积分协方差矩阵covariance。在预积分完成之后系统会根据协方差矩阵计算出信息矩阵sqrt_info Eigen::LLTEigen::Matrixdouble, 15, 15(covariance.inverse()).matrixL().transpose();这里的逻辑是协方差矩阵的逆是信息矩阵信息矩阵做Cholesky分解取上三角矩阵作为残差的加权矩阵。这样处理后的残差其协方差在理想情况下接近单位矩阵优化器对不同残差项的权重分配就自然合理了。还有一个细节是鲁棒核函数的使用。VINS-Mono默认没有给IMU因子加鲁棒核而是把鲁棒核加在了视觉因子上。这个设计符合实际IMU预积分的残差一般比较平滑异常值少而视觉匹配则容易出现外点。如果你在实际应用中遇到IMU数据跳变比如传感器受到剧烈冲击可以考虑给IMU因子也加一个Huber核但要小心不要过度压制正常的动态响应。5.3 与视觉因子拼接时的注意事项在VINS-Mono的滑动窗口优化中IMU因子和视觉因子是同时在Ceres里求解的。IMU因子连接的是相邻两个关键帧的全部状态而视觉因子连接的是多个关键帧的位姿和路标点。一个常见的坑是视觉因子的残差模块通常使用ceres::Problem::AddResidualBlock添加每个视觉因子只关联一个位姿和一个路标点而IMU因子则一步到位关联两个关键帧的7个状态块。如果状态块的编号或者维度不一致Ceres会在求解时直接报错“parameter block size mismatch”。另外VINS-Mono在移除滑动窗口中的旧帧时需要同步移除对应的IMU因子和视觉因子。这个移除操作依赖Ceres的Problem::RemoveResidualBlock接口而调用这个接口时需要传入残差块的ID。因此代码里通常会维护一个残差块ID的列表在边缘化时逐个处理。如果你自己改动窗口大小或者边缘化策略一定要检查这个列表是否同步更新否则会出现Ceres内部状态不一致的诡异问题。6. 我在调试IMU预积分时踩过的坑6.1 零偏更新导致的不一致问题我第一次给VINS-Mono换IMU模型时遇到过一个特别诡异的现象初始化阶段表现正常但跑到几十秒后开始漂移而且越是快速运动漂移越严重。查了很久发现问题出在零偏初值上。integrationBase在构造时默认把零偏设为零但在VINS-Mono的初始化流程里processIMU()会在初始化完成前先估计一次陀螺仪零偏然后用估计到的值去构造后续的预积分器。如果你在改代码时不小心让预积分器的零偏值和后端优化里的零偏初值不一致就会导致残差计算时修正量出现常量偏差而这个偏差会在积分中不断累积。解决方式其实很简单在初始化完成后确保所有预积分器都使用相同的零偏初值并且linearized_ba和linearized_bg与后端的先验一致。这类问题不会导致系统崩溃但会表现为精度逐渐变差排查起来很花时间。6.2 协方差初值与尺度问题协方差矩阵的初值设置对优化收敛有一定影响但很多人容易忽略。VINS-Mono里预积分协方差初值是零矩阵这个选择意味着初始时刻认为预积分量是完全准确的然后随IMU噪声累积逐步增加不确定性。如果你的IMU噪声参数设置得过大协方差递推出来的数值会很大导致sqrt_info矩阵的数值很小最终IMU因子在优化中的权重被压低系统就更依赖视觉。反过来噪声参数设得太小IMU的权重被抬高一旦IMU数据质量不好系统就会跟着遭殃。一个比较实用的调参经验是先用量测数据离线计算一下IMU的Allan方差把噪声密度和随机游走参数标定出来再填进配置。如果手头没有Allan方差工具至少也要参考IMU芯片手册上的参数不要随便填。6.3 时间戳对齐的暗坑时间戳问题我前面提过这里再强调一次。VINS-Mono对IMU时间戳要求得很严格它假设IMU数据按时间顺序到达并且两帧IMU数据之间的时间间隔就是dt。如果数据里有重复时间戳或者乱序时间戳dt可能为负导致预积分出现NaN。更隐蔽的问题是IMU和相机的时间戳如果存在固定的时间偏移预积分量和视觉观测之间的几何一致性会被破坏但程序不会报错只会表现为精度下降。解决这类问题的方法也比较成熟就是用kalibr或者类似的工具做一次时间戳标定获取相机和IMU之间的延时然后在喂数据时统一补偿。6.4 一些调参经验最后说几条我在实践中总结出来的经验。如果室内的快速旋转场景下VINS-Mono容易飘优先看陀螺仪噪声和零偏随机游走参数多半是gyr_noise和gyr_bias设得太乐观。如果低纹理环境下经常初始化失败可以适当降低IMU因子的协方差也就是增大IMU权重让系统在视觉信息不足时依然能约束住状态。如果跑自己采集的数据集一定要注意IMU单位。有些IMU输出的加速度单位是g而VINS-Mono默认使用m/s²不换算的话预积分结果会偏差一个重力加速度量级整个系统直接废掉。预积分里sum_dt不要自己去改它表示该段预积分的总时间跨度残差计算里依赖它。如果你用了不等间隔的IMU数据务必确认这里的累加逻辑是准确的。IMU预积分这部分代码第一次读可能觉得矩阵又多又杂但本质上就是一整套“用固定零偏积分、用Jacobian修正”的思路。把这个思路理清楚再看processIMU()、integrationBase和imu_factor.h就不会迷路了。