开门见山说一句：如果把VIO系统比作一个人的话，视觉是眼睛，IMU就是内耳前庭——负责感知运动。手写VIO走到第二节，imu.cpp就是那个把IMU原始数据变成系统能用的“运动知觉”的关键文件。这一篇我从头到尾拆一遍imu.cpp里到底写了什么、为什么要这么写、以及我实际手写时踩过的坑。项目还没开始、正在啃代码、还是已经跑通想回头梳理的，都能在这篇里找到对得上号的东西。

先交代一下背景：我整理这套手写VIO系列时，把imu.cpp定成第二节是有意的——视觉前端哪怕直接调现成的特征提取，代码也能跑，但IMU数据如果不在一开始就做对，后续整个VIO的初始化、预积分、紧耦合优化全都是错的。换句话说，视觉错了能看见，IMU错了往往看不见，直到结果发散才回头查，那时候定位一个bug的成本就高得多了。

1. 先弄清楚imu.cpp在系统里的职责边界

很多人一上来就盯着代码看，看了半天不知道每个函数为什么要存在。我的习惯是先画职责边界，再落代码。

1.1 一个VIO系统中IMU相关任务的全景拆分

IMU在VIO里无非干五件事：数据接入、预处理、运动积分、预积分、在线标定辅助。我手写的这个VIO里，imu.cpp承担了前四件事，在线标定辅助只留了接口，后面接完整优化时才填。

• 数据接入：订阅IMU消息，或者从数据集读文件，拿到时间戳、三轴角速度、三轴加速度。
• 预处理：去零偏、去尺度误差、坐标系对齐、异常数据过滤。
• 运动积分：在两个时刻之间对IMU做一次积分，输出增量，供状态预测和前端视觉追踪用。
• 预积分：在两个关键帧（或两帧图像）之间对IMU测量做相对积分，缓存原始数据，支持零偏更新后重新传播。
• 辅助标定：提供初始重力方向估计，以及IMU与相机的外参初值传递。

这五件事如果写在一个超长文件里也能跑，但手写VIO的目的一半是为了理解，一半是为了以后扩展。所以我建议imu.cpp内部仍然按模块拆：文件入口是回调函数或者数据转发，核心逻辑封装成专门的类或结构体。

1.2 我手写的imu.cpp模块划分和对应关系

以我现在的实现为例，文件内部结构大致是：

// imu.cpp 主要职责模块 // 模块1: IMU数据缓存与时间戳管理 // 模块2: 原始测量预处理（去偏、坐标变换） // 模块3: 单步离散积分（欧拉/中值） // 模块4: 预积分器（含协方差与雅可比传递） // 模块5: 对外输出接口（给前端/后端/初始化）

这里有个容易忽略的点：很多参考实现会把IMU积分代码单独放到一个头文件里，imu.cpp只做数据入口。我一开始也这么干，后来发现调试时来回跳文件太麻烦，反而放一起改起来直观。这个取舍因人而异，但我的建议是：文件可以合，逻辑边界必须清晰，否则写到后面你自己都分不清哪里在更新状态。

1.3 从整体架构理解imu.cpp为什么“绕”

另一个新手经常问的问题：IMU积分不就是模板里几行公式吗，为什么实际代码又长又绕？原因在于实时VIO里，IMU数据的到达时间不可能和图像帧严格对齐。imu.cpp里大量代码其实是在做“排队、对齐、补采”这类琐碎工作。比如图像帧率30Hz，IMU频率200Hz，那么在连续两帧之间大约有6到7个IMU测量。imu.cpp要把这些测量按时间顺序缓存好，以图像帧的时刻为界切分，再交给积分器。

想清楚这一点，再看代码就不会被那些时间戳比较、数组push_back、迭代器判断绕晕了。实际上它们都在回答一个问题：这一小段IMU数据，应该算到哪两帧之间。

2. 手写imu.cpp的第一步：数据接入与预处理

这一节处理的是“原始数据进来之后，先做什么”。别看步骤简单，这里犯的错往往最隐蔽，因为很多错不会立刻让程序崩，而是让结果偏差一点点。

2.1 回调函数与时间戳处理的最佳实践

先看一个常见的数据接入骨架：

// 假设IMU数据通过回调/读取逐帧进入 void imuCallback(double t, const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr) { // 1. 原始数据缓存 imu_buf.push_back({t, acc, gyr}); // 2. 时间戳单调性检查 if (imu_buf.size() >= 2) { double dt = imu_buf.back().t - imu_buf[imu_buf.size()-2].t; if (dt <= 0) { // 说明时间戳乱序或重复，需要处理 imu_buf.pop_back(); return; } } // 3. 触发积分/预积分 processIMU(t, acc, gyr); }

这里最核心的注意点：时间戳必须是double或者更高精度，不能用int秒。很多人第一次手写时会忽略IMU高频数据的时间精度问题，用整型时间导致所有dt计算都为0，积分完全不动。我吃过这个亏，后来统一一律用double，并且做一次单调性检查。

另外关于乱序问题：实测中IMU数据偶尔会乱序，尤其是把IMU和相机分线程处理时。处理策略不是粗暴丢弃，而是判断一下如果乱序偏移很小（比如采样间隔的1/10），交换一下就行；偏移大了才丢弃，否则会引入时间误差。

2.2 单位、零偏补偿和坐标系约定

预处理这一节，要处理的物理量包括三部分：单位换算、零偏补偿、坐标系对齐。这三件事的顺序不能乱，通常先单位换算，再减零偏，最后做坐标旋转。

单位换算是很多代码里写死的东西：陀螺仪常用单位是deg/s，而算法内部统一用rad/s，因此要乘pi/180。加速度计一般已经是m/s^2，但不同IMU模组可能输出g（重力加速度倍数），需要乘9.81。这个换算如果弄错，最典型的现象是初始化时重力估计完全不靠谱，或者系统运行时漂移快得离谱。

零偏补偿就更关键了：

// 零偏补偿示意 Eigen::Vector3d gyr_corrected = gyr_raw - gyro_bias; Eigen::Vector3d acc_corrected = acc_raw - acc_bias;

零偏的来源有两个：一是IMU上电后静态温度漂移，二是VIO在线估计出的bias更新。手写阶段没有在线估计，所以至少要有开机静态取平均的初始零偏。我在测试时发现，如果跳过这一步，10秒内姿态就会明显漂移。别嫌这一步简单，它直接决定后面所有积分的质量。

坐标系约定这里要特别展开说一下。IMU的坐标系右手系是共识，但z轴朝上还是x轴朝前，不同硬件并不一样。VIO系统里最终用的坐标系通常遵循“右前上”或“前右下”，相机坐标也有自己约定。所以在imu.cpp预处理里，一定要把IMU原始坐标系转换到系统统一坐标系。

一个非常隐蔽的point：如果你的IMU和相机是近似刚性安装，但外参旋转不是精确单位阵，那么在预处理阶段也可以做一次近似旋转对齐，后续再做精细外参标定。如果代码里没有这一步，做纯单目+IMU试验时，初始化会消耗极长时间，甚至直接失败。

2.3 初始重力对齐和加速度计数据的方向约定

IMU积分之所以能在VIO里起作用，是因为加速度计直接感受到了重力分量。初始化过程中，一个关键任务是利用静止或缓慢运动时的加速度测量，估计出重力在IMU坐标系的表示。这个初值会被用来把IMU坐标系粗略对齐到世界坐标系。

实现上很简单，取一段静态数据求平均就行。但有一个细节：加速度计输出的“静止”测量，实际上是抵消了重力支撑力，所以测到的矢量方向和重力是反着的。如果初始重力方向写反，整个VIO的z轴会翻转，姿态解算全是错的。我建议在imu.cpp里写一个简单的自检函数：静止状态下，加速度模长应当接近9.81，若偏差超过0.5m/s^2，就要检查单位或者数据方向。

3. 核心中的核心：IMU离散积分与预积分实现

到这里才算真正进入imu.cpp的深水区。很多人啃代码啃到这里开始卡壳，因为公式和代码总对不上。我把这一节按“从简单到复杂”的顺序拆：先讲单步积分，再讲预积分整体，再讲协方差和雅可比。

3.1 欧拉法还是中值法？从实际表现谈选型

单步离散积分的目的是，已知上一时刻的PVQ状态（位置、速度、姿态四元数），根据这一小段的角速度和加速度，计算出当前时刻的PVQ。两种常见方法：

欧拉法直接用当前测量值更新：

// 欧拉法示意 Eigen::Quaterniond delta_q; delta_q = Eigen::Quaterniond(1, 0.5 * w.x() * dt, 0.5 * w.y() * dt, 0.5 * w.z() * dt); // 近似，需归一化 // 或使用 Exp(w * dt)

中值法则用上一时刻和当前时刻测量值的平均：

// 中值法示意：先缓存上一时刻 acc_0, gyr_0 Eigen::Vector3d w_mid = 0.5 * (gyr_0 + gyr_1) - bias_g; Eigen::Vector3d a_mid = 0.5 * (acc_0 + acc_1) - bias_a;

实测下来，IMU频率在100Hz以下时，欧拉法在剧烈运动情况下误差明显；中值法在100Hz下已经能做到不错的效果。手写VIO阶段，我不建议直接上更高阶的RK4，因为代码复杂度和调试难度都不小，中值法足够撑起整个系统。

我的建议是：默认中值法，并且在imu.cpp里留下一个接口，方便后面想切回欧拉法做对照实验。这对理解积分误差来源非常有帮助。

3.2 预积分整体框架与代码骨架

预积分解决的核心问题是：在后端优化调整位姿时，IMU测量产生的积分量不应该被反复重算。预积分的思想是，把两帧图像之间的IMU测量，相对于“起始时刻的PVQ状态”做一个相对积分，得到相对旋转、相对速度增量、相对位置增量。这样在后端优化中，IMU误差项只依赖于预积分值和起始/结束状态，而不需要重新遍历所有IMU数据。

一个典型的预积分器类：

class IMUPreintegration { public: // 缓存原始IMU测量，用于零偏更新后的重新传播 std::vector<std::pair<double, std::pair<Eigen::Vector3d, Eigen::Vector3d>>> measurements; // 预积分量 Eigen::Quaterniond delta_q; // 相对旋转 Eigen::Vector3d delta_v; // 速度增量 Eigen::Vector3d delta_p; // 位置增量 // 协方差与雅可比 Eigen::Matrix<double, 9, 9> covariance; Eigen::Matrix<double, 15, 15> jacobian; // 状态对bias等的雅可比 // 当前零偏 Eigen::Vector3d linearized_ba, linearized_bg; void push_back(double dt, const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr); void midPointIntegration(double dt, const Eigen::Vector3d& acc_0, const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_1, const Eigen::Vector3d& gyr_1); void repropagate(const Eigen::Vector3d& new_ba, const Eigen::Vector3d& new_bg); };

注意几点：预积分量不关心绝对位姿，它关心的是相对量。所以在代码里看到的delta_q、delta_v、delta_p全部是相对值。这让预积分的数据可以被反复用于不同绝对状态假设下的误差计算。

另一件事：预积分器必须保留原始测量（measurements数组），因为当零偏估计发生变化时，预积分量需要用新零偏重新计算。如果没缓存原始数据，后面优化迭代时就没法更新，只能推倒重来。

3.3 中值预积分核心更新的代码实现

给出一个简化但结构完整的中值预积分实现骨架，关键在更新顺序：

void IMUPreintegration::push_back(double dt, const Eigen::Vector3d& acc, const Eigen::Vector3d& gyr) { measurements.push_back({dt, {acc, gyr}}); // 这里调用单步积分 if (measurements.size() == 1) { // 初始化状态，存下当前值作为 acc_0/gyr_0 acc_0 = acc; gyr_0 = gyr; return; } // 用当前值和上一次的值做中值积分 midPointIntegration(dt, acc_0, gyr_0, acc, gyr); acc_0 = acc; gyr_0 = gyr; } void IMUPreintegration::midPointIntegration( double dt, const Eigen::Vector3d& acc_0, const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_1, const Eigen::Vector3d& gyr_1) { // 中值去偏 Eigen::Vector3d w_mid = 0.5 * (gyr_0 + gyr_1) - linearized_bg; Eigen::Vector3d a_mid = 0.5 * (acc_0 + acc_1) - linearized_ba; // 旋转增量: Exp(w_mid * dt) double theta = w_mid.norm() * dt; Eigen::Vector3d axis = w_mid.normalized(); Eigen::Quaterniond dq; if (theta > 1e-6) { dq = Eigen::Quaterniond( std::cos(0.5 * theta), std::sin(0.5 * theta) * axis.x(), std::sin(0.5 * theta) * axis.y(), std::sin(0.5 * theta) * axis.z()); } else { dq = Eigen::Quaterniond(1, 0, 0, 0); } dq.normalize(); // 关键:先更新旋转，再在旋转后的坐标系中处理加速度 Eigen::Quaterniond new_delta_q = delta_q * dq; new_delta_q.normalize(); // 计算当前IMU坐标系下的加速度增量 // 注意:这里的 a_mid 是在当前IMU坐标系的测量 delta_p += delta_v * dt + 0.5 * delta_q * a_mid * dt * dt; delta_v += delta_q * a_mid * dt; delta_q = new_delta_q; // 更新协方差和雅可比（见下节） updateCovAndJacobian(dt, a_mid, w_mid, dq); }

更新顺序为什么是先更新delta_p再更新delta_v？因为delta_p增量里用的是当前的delta_v，而不是更新后的delta_v。这个顺序写错会导致位置增量比实际超前半个积分步，系统长时间运行后会有肉眼可见的漂移。

另一个细节：delta_q * a_mid 是把加速度从当前IMU坐标系旋转到预积分起始时刻的坐标系（也可以理解为相对于起始姿态下的加速度）。这个旋转用的必须是旧的delta_q（即积分开始时的累计旋转），而不是新的new_delta_q。我在实现时一度搞混，结果高动态下预积分量对不上，日志里看delta_p和delta_v的比值明显不对，才排查出来。

3.4 协方差传递与雅可比更新，到底在算什么

协方差传递回答的问题是：IMU测量噪声经过积分之后，在预积分量中累积成多大的不确定性。这个不确定性在后端优化里作为信息矩阵的权重，直接影响IMU误差项的置信度。

离散形式的协方差更新一般写成：

void IMUPreintegration::updateCovAndJacobian(...) { // 状态误差: [rot_err, velocity_err, position_err, ba_err, bg_err] // 简化的F矩阵: 描述误差如何传递 Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity(); // 旋转误差块 F.block<3,3>(0,0) = Eigen::Matrix3d::Identity() - skew(w_mid * dt); F.block<3,3>(0,3) = ...; // 对ba的雅可比 // 速度块 F.block<3,3>(3,0) = -delta_q.toRotationMatrix() * skew(a_mid) * dt; // 位置块 F.block<3,3>(6,0) = -0.5 * delta_q.toRotationMatrix() * skew(a_mid) * dt * dt; // 噪声到误差的传递矩阵 V Eigen::Matrix<double, 15, 12> V = ...; // 离散协方差更新: P = F * P * F^T + V * Q * V^T covariance = F * covariance * F.transpose() + V * noiseQ * V.transpose(); // 雅可比更新: J = F * J（对bias的敏感度） }

这里不要被一堆矩阵吓到，理解思路就够了：F矩阵描述的是“如果当前误差是X，下一步误差会变成什么样”，V矩阵描述的是“如果当前测量噪声是N，误差会加多少”。协方差更新就是这两个传递关系的组合。

实操中最容易出错的点是噪声协方差矩阵Q的取值。角速度噪声和加速度噪声的单位不同，量级差很多，必须从IMU硬件手册或实测数据获取。如果Q矩阵里某个元素设得特别大，相当于告诉系统“IMU测量非常不可靠”，后端会过度信任视觉；反之亦然。我在调试时发现，一个合理的经验法则是先用厂家手册给的值，然后做一次Allan方差分析校准，可以显著提升初始化收敛速度。

另一个需要说明的点：雅可比J更新用于预积分对零偏的一阶修正。后端优化更新了零偏之后，我们并不总是需要从头重新传播整个预积分，可以先用雅可比做线性修正，只有当零偏变化量超过阈值时才做精确的repropagate。这是一个重要的性能优化点，也是imu.cpp里比较精妙的地方。

4. 我在手写过程中踩过的坑与调试心得

这一段不是教科书内容，是我实际把imu.cpp调通的过程中一点点积累的。写出来给大家省点时间。

4.1 最难排查的几个bug

第一是四元数未归一化。四元数使用时间长了会出现数值漂移，尤其在连续乘了大量小角度增量后，模长会慢慢偏离1。如果不归一化，旋转矩阵里会出现微小尺度变化，视觉重投影和IMU残差对不上。刚开始根本没往这想，因为姿态角度看起来还正常。后来用一个长序列测试，对比旋转矩阵的行列式，发现它从1.0000慢慢变成0.9998，这才定位。我现在的规则是：凡是更新delta_q后，立刻归一化，没有例外。

第二是时间戳只进了增量缓存但没更新积分状态。有些写法是先push_back缓存，然后在另一个函数里做积分，中间如果订阅频率不均匀，很容易出现“积分频率和缓存频率不一致”的问题。表现是运行速度不慢，但精度时好时坏。排查方式是在每个积分步打印时间间隔，和IMU理论采样周期做对比。

第三是世界坐标系的gravity定义位置错了。预积分公式里，如果不显式处理重力，而是在后续残差计算时再统一补偿，那么在预积分内部一定不要额外减重力。很多网上的代码在预积分里减了重力，又在后端加回来，等于加减抵消，但中间涉及的坐标系一多，符号就乱了。我建议在imu.cpp里严格区分：预积分计算delta_p、delta_v时不把重力项计入，重力只在残差模型里出现。这样公式简洁，也便于核对。

4.2 验证积分正确性的调试手段

我调试时养成了一个习惯：把IMU数据输入固定之后，分别用欧拉法、中值法、甚至纯解析解（比如恒定角速度场景）跑一遍，对比delta_p、delta_v的差异。这个方法实现简单，但能暴露出绝大多数积分实现问题。

第二个验证手段是运动恢复测试。把IMU静置放在桌面上，记录1分钟数据，跑完整预积分，最后输出的delta_p应该接近零、delta_v接近零、delta_q接近单位四元数。如果发现delta_p明显不为零，说明预处理阶段的重力剔除或零偏处理有问题。注意这里“重力剔除”指的是在预积分公式中选择是否包含重力项，需要和上一节说的保持一致。

第三个手段比较高级但也推荐：使用仿真数据。我在自己搭建测试时，先用手写程序生成一条运动轨迹，再通过正向模型生成IMU测量，然后将这些测量喂给imu.cpp，看预积分量能否恢复出真实相对运动。仿真数据的好处是Ground Truth已知，可以定量评估误差。

4.3 关键参数调参建议清单

下面这张表是我手写VIO时常用的参数设定经验，基于常见IMU配置，不同硬件需要自己微调。

还要提一个很多人忽略的参数：IMU数据滞后补偿。在真实系统中，IMU和相机传感器的内部处理会带来几毫秒到十几毫秒的时间延迟，这个延迟不校正，会导致视觉和IMU之间出现系统性偏差。手写VIO阶段可以先忽略，但如果你发现系统初始化非常敏感，稍微改一下时间就变了，就要考虑加一个可调的时间偏移参数，在imu.cpp里对每个IMU数据的时间戳统一做偏移修正。

5. 从imu.cpp出发，接下来该怎么走

这一节不是总结，而是给后面的路标。手写VIO是个大工程，imu.cpp只是地基，但地基决定了上层能盖多高。

5.1 imu.cpp怎么扩展才不算过度设计

我见过有人把imu.cpp写得巨大无比，塞进了在线标定、多IMU融合、故障检测，结果第二周自己都看不懂。我的建议是，保持文件的核心功能单一，其他内容通过接口向外扩展。

值得扩展的方向有三个：一是外加一个“Bias校正器”，后期对接后端优化时，把估计出的零偏反馈到预处理里；二是增加“IMU序列管理模块”，支持滑窗机制，当关键帧滑动时，丢弃过期IMU数据而不用重建所有量；三是支持仿真模式，在读取数据时增加一个开关，直接生成IMU测量，方便纯算法调试。

5.2 手写顺序建议：不要一上来就啃协方差

如果要自己从零实现，我的建议顺序是：先写一个最朴素的“类欧拉积分”版本，只更新PVQ，能跑通数据流，看到姿态翻滚有反应；然后换成中值法，对比效果；再加入预积分量和缓存；最后才上协方差和雅可比。理由是“先让系统转起来，再优化精度”，如果你第一步就陷入协方差矩阵推导，很可能卡住三天连数据流都没理顺。

最后分享一段我在代码注释里给自己写的提醒：“预积分的本质是提前算好，不是离线算好，它保留的是测量信息，而不是历史状态。”想明白这句话，再看那些看似冗余的缓存代码，就都顺理成章了。手写VIO是一条很长的路，imu.cpp这关过了，后面初始化、滑窗、边缘化才会真正好懂。祝各位顺利。