企业官网建设流程全解析

1. 从零手写VIO：为什么值得啃这块硬骨头

视觉惯性里程计（Visual-Inertial Odometry，简称VIO）这几年在机器人、无人机、AR/VR领域的热度一直居高不下。原因很直接：单靠视觉，快速运动或纹理缺失时容易跟丢；单靠惯性测量单元（IMU），零偏和噪声积分一会儿就飘到天上去。两者一结合，视觉提供长期稳定的位姿约束，IMU提供高频运动先验，互补之后系统鲁棒性直接上一个台阶。

但VIO的学习曲线确实陡。很多人第一次翻开相关教材或论文，看到预积分、流形上的雅可比、舒尔补边缘化这些概念，第一反应就是“每个字都认识，连起来不知道在说什么”。市面上的开源方案虽然多，但直接读源码往往陷入“知道它在算，但不知道为什么这么算”的困境。

这个项目标题——“从零开始手写VIO第一章作业（含详细推导过程）”——本质上就是冲着这个痛点来的。它不满足于调用现成库，而是要求你从最基础的数学推导开始，一步步把VIO的核心模块亲手搭出来。第一章作业通常聚焦在IMU运动学建模与预积分这个地基上，这是整个VIO系统最底层、也最容易劝退的部分。

这篇文章适合谁看？如果你已经学过线性代数和刚体运动基础，能看懂旋转矩阵和四元数，但一碰到“IMU预积分在流形上的更新”就卡壳，那这篇内容就是为你准备的。我会把第一章作业里涉及的推导逻辑、代码实现要点、以及我实际踩过的坑，全部摊开来讲。不绕弯子，不堆术语，争取让你看完能自己动手把这一章作业复现出来。

2. 第一章作业的整体设计思路拆解

2.1 为什么第一章偏偏要啃IMU预积分

很多教程一上来就讲特征点提取、光流跟踪，把IMU部分往后放。但这个作业的设计反其道而行，第一章直接硬刚IMU预积分。我一开始也觉得奇怪，后来自己动手推了一遍才明白：VIO的误差来源和优化框架，本质上都建立在IMU预积分的数学结构之上。

如果你不理解预积分量对零偏的雅可比怎么来的，后面做联合优化时，根本不知道信息矩阵该怎么设、残差该怎么线性化。更现实的一点是，视觉部分的代码调试起来直观——画个图就能看出特征点跟没跟丢；但IMU预积分如果符号推错一个负号，整个系统可能只是“慢慢飘”，你连哪里错了都找不到。所以第一章把这块最抽象、最容易埋雷的内容先解决掉，后面反而越走越顺。

这个作业的核心目标可以拆成三个层次：

• 第一层：理解IMU的连续时间运动学模型，知道角速度和加速度是怎么积分出位姿的。
• 第二层：掌握预积分的离散化实现，包括欧拉积分、中值积分、龙格库塔积分三种方式的区别和精度对比。
• 第三层：推导预积分量对零偏的雅可比矩阵，并理解它在优化中如何更新。

这三层是一环扣一环的。第一层没搞懂，第二层的离散化就是抄公式；第二层没吃透，第三层的雅可比推导就是天书。

2.2 方案选型：为什么用四元数而不是旋转矩阵

在推导IMU运动学之前，第一个要做的决策就是：姿态用什么表示？旋转矩阵、欧拉角、四元数、李代数，选哪个？

这个作业里我最终选择了四元数作为主要表示，原因有几个。旋转矩阵有9个参数但只有3个自由度，优化时如果直接更新矩阵元素，会破坏正交性约束，还得额外做正交化，很麻烦。欧拉角有万向锁问题，而且三角函数求导在推导时容易出错。李代数（so(3)）在理论推导上最优雅，但第一章作业如果直接从李代数切入，对新手来说门槛太高，容易在“左扰动还是右扰动”这种问题上卡好几天。

四元数的好处是：参数只有4个，约束简单（模长为1），乘法运算规则固定，求导也不算复杂。虽然它也有“双倍覆盖”的问题（q和-q表示同一旋转），但在预积分这种短时间窗口内，只要初始化时选好符号，基本不会出问题。

注意：用四元数做优化时，更新量通常是三维的（因为模长约束去掉一个自由度），所以雅可比矩阵是4×3或者3×3的，这个维度问题在后面推导时一定要盯紧，我当初就是在这里把维度搞混，导致代码里矩阵乘法一直报错。

2.3 离散化方案的选择逻辑

连续时间的IMU运动学方程写起来很漂亮，但实际IMU给的是离散采样点，所以必须做离散化。作业里要求实现三种积分方式，这不是为了凑字数，而是让你通过对比理解精度和计算量的权衡。

欧拉积分最简单，用当前时刻的测量值直接乘时间间隔。但它是一阶精度，IMU采样率不够高时误差累积很快。中值积分用前后两个时刻测量值的平均，精度提升到二阶，计算量只增加一点点，所以实际VIO系统里用得最多。龙格库塔积分（RK4）精度更高，但需要多次计算中间斜率，计算量明显上升，在IMU频率本来就很高（通常100Hz以上）的情况下，性价比不高。

我实测下来的感受是：如果你的IMU频率在200Hz以上，中值积分已经足够；如果只有100Hz左右，且运动比较剧烈，可以考虑RK4，但更推荐的做法是提高IMU采样率或者做好零偏估计，而不是单纯堆积分阶数。

3. 核心推导细节与实操要点

3.1 IMU连续时间运动学模型的完整推导

IMU测量模型是整章作业的起点。一个三轴IMU通常输出角速度ω和加速度a，但这两个量都带有零偏b和噪声n：

ω_measured = ω_true + b_g + n_g a_measured = a_true + b_a + n_a

这里第一个容易踩的坑是坐标系问题。加速度计测量的是比力，不是纯加速度。在重力存在的情况下，静止时加速度计读数约为+9.8 m/s²（方向指向天），但实际物体加速度为零。所以运动学方程里必须把重力减掉：

a_true = R * (a_measured - b_a - n_a) + g

其中R是机体到世界系的旋转矩阵，g是重力向量（通常取[0, 0, -9.8]）。这个符号我改过三次才搞对，因为不同教材对重力方向的约定不一样，有的把g写成正9.8，有的写成负9.8，一定要和你自己的世界系定义统一。

连续时间下，位姿更新方程为：

p_dot = v v_dot = R * (a_measured - b_a) + g q_dot = 0.5 * q ⊗ [0, ω_measured - b_g]

四元数求导这里用的是四元数乘法，⊗表示四元数乘法。展开后可以得到一个4×4的矩阵形式，这个形式在代码里实现起来最方便。

实操心得：推导时建议把四元数乘法显式写出来，不要跳步。我当初为了省事直接用了简化公式，结果在代码里发现旋转方向反了，排查了一整晚才发现是四元数乘法顺序搞错。四元数不满足交换律，q1⊗q2和q2⊗q1差一个符号，这个符号在预积分里会被放大。

3.2 预积分量的定义与物理意义

预积分最核心的思想是：把与状态量无关的部分先算出来，避免在优化迭代中重复积分。具体来说，两个关键帧之间的IMU积分，如果每次都从世界系重新算，那优化时每改变一次零偏或速度，就要重新积分一遍，计算量爆炸。

预积分把积分拆成两部分：一部分只依赖IMU测量和零偏，另一部分依赖起始状态。定义预积分量：

Δp_ij = ∫∫ R_i^T * (a_measured - b_a) dt² Δv_ij = ∫ R_i^T * (a_measured - b_a) dt Δq_ij = ∫ q_i^T ⊗ q_dot dt

这里的R_i^T是把世界系的量转到起始帧的机体坐标系。这样定义之后，预积分量就只和IMU测量、零偏有关，和起始位姿无关。优化时如果零偏变化不大，可以直接用一阶泰勒展开修正，不用重新积分。

这个“零偏变化不大”的假设是预积分能加速优化的关键。实际系统中，零偏通常在优化中被当作状态量估计，但每次迭代零偏变化很小，所以用雅可比做一阶修正就够了。如果零偏变化很大，说明初始化没做好，或者IMU质量太差，这时候预积分的加速效果会打折扣。

3.3 离散化实现的三种方式对比

连续公式推完之后，就要落地到离散实现。作业里要求实现三种积分方式，我把它们的核心区别整理成表：

中值积分的具体实现里，有一个细节很容易忽略：角速度的中值积分和加速度的中值积分，时间对齐方式不一样。角速度影响的是旋转，旋转又影响加速度的积分方向，所以严格来说应该先用中值角速度更新旋转，再用更新后的旋转去积分加速度。我第一版代码里把两者并行计算，结果在高动态下误差明显偏大。

// 中值积分核心代码片段（示意） Vec3 omega_mid = 0.5 * (omega_k + omega_k1); Vec3 acc_mid = 0.5 * (acc_k + acc_k1); Quat dq = Quat::Identity() + 0.5 * Quat(0, omega_mid) * dt; dq.normalize(); Vec3 acc_world = R_k * acc_mid + g; p_k1 = p_k + v_k * dt + 0.5 * acc_world * dt * dt; v_k1 = v_k + acc_world * dt;

注意：四元数更新后一定要做归一化。数值积分会引入微小误差，不归一化的话，跑几千步之后四元数模长可能偏离1，导致旋转矩阵不正交，整个系统就崩了。这个坑我在调试时遇到过，表现是位姿慢慢漂移，但看不出明显错误，最后打印四元数模长才发现问题。

3.4 零偏雅可比矩阵的推导要点

预积分量对零偏的雅可比，是第一章作业里最硬的部分。推导思路是：对预积分公式两边关于零偏求偏导，利用链式法则逐项展开。

以旋转预积分对陀螺仪零偏的雅可比为例：

∂Δq_ij / ∂b_g = Σ (∂Δq_ij / ∂ω_k) * (∂ω_k / ∂b_g)

其中∂ω_k / ∂b_g = -I（因为ω_measured = ω_true + b_g，所以ω_true = ω_measured - b_g）。旋转部分对ω的偏导，需要用到四元数右乘雅可比矩阵，这个矩阵在四元数乘法中反复出现，建议单独写一个函数封装。

加速度零偏的雅可比更复杂，因为加速度影响速度和位置，而速度又影响位置，所以位置预积分对加速度零偏的雅可比包含两项：一项是直接的二次积分项，另一项是通过速度间接影响的项。我当初推导时漏掉了第二项，导致优化时零偏估计收敛很慢，后来对照公式逐项检查才发现。

实操心得：推导雅可比时，建议先用数值求导验证符号。具体做法是给零偏一个微小扰动，用数值差分算雅可比，再和解析推导的结果对比。如果符号相反，说明推导中某一步的符号搞错了。这个方法虽然笨，但能帮你快速定位错误，比盯着公式反复看有效得多。

4. 完整实操流程与核心环节实现

4.1 工程环境搭建与依赖选择

第一章作业虽然只涉及IMU预积分，但工程环境还是要搭好，不然后面章节扩展时会很痛苦。我用的环境是Ubuntu 20.04 + CMake + Eigen + Sophus。Eigen负责矩阵运算，Sophus负责李群和李代数操作（虽然第一章用四元数，但后面章节会用到SE(3)）。

CMakeLists.txt里关键配置：

cmake_minimum_required(VERSION 3.10) project(vio_chapter1) set(CMAKE_CXX_STANDARD 14) find_package(Eigen3 REQUIRED) find_package(Sophus REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR} ${Sophus_INCLUDE_DIR}) add_executable(test_imu_preintegration test_imu_preintegration.cpp) target_link_libraries(test_imu_preintegration ${Sophus_LIBRARIES})

Sophus的安装有个小坑：它依赖fmt库，如果系统里没有fmt，编译会报错。我当初直接clone源码编译，卡在fmt找不到的问题上，后来先装了libfmt-dev才通过。如果你不想折腾，也可以只用Eigen自己实现四元数运算，但Sophus的API更简洁，后面做SE(3)优化时省事很多。

4.2 IMU数据读取与时间对齐

实际IMU数据通常是CSV或bag包格式。第一章作业一般会提供一段仿真数据或公开数据集。读取时要注意两个问题：时间戳单位和数据对齐。

时间戳单位常见的有秒、毫秒、纳秒，如果不统一，积分出来的位姿会差几个数量级。我习惯在读取后统一转成秒，并在代码里加一个断言检查时间间隔是否合理（比如dt应该在0.001到0.1秒之间）。

数据对齐指的是IMU和视觉帧的时间戳对齐。第一章虽然只做IMU预积分，但后面章节要做视觉惯性联合，所以最好从一开始就养成习惯：把IMU数据按时间排序，并记录每个视觉关键帧对应的IMU区间。

struct ImuData { double timestamp; Vec3 omega; Vec3 acc; }; std::vector<ImuData> imu_data; // 读取后按timestamp排序 std::sort(imu_data.begin(), imu_data.end(), [](const ImuData& a, const ImuData& b) { return a.timestamp < b.timestamp; });

4.3 预积分类的设计与实现

预积分模块我封装成了一个类，核心成员包括预积分量（Δp, Δv, Δq）、零偏雅可比、以及积分时间。类接口设计如下：

class PreIntegrator { public: void reset(); void integrate(const ImuData& data); void repropagate(const Vec3& new_bg, const Vec3& new_ba); Mat3 getDeltaR() const; Vec3 getDeltaV() const; Vec3 getDeltaP() const; Mat3 getJacobianRToBg() const; // ... 其他雅可比接口 private: Vec3 bg_, ba_; Vec3 delta_p_, delta_v_; Quat delta_q_; Mat3 J_R_bg_, J_V_bg_, J_V_ba_, J_P_bg_, J_P_ba_; double dt_; };

integrate函数里实现中值积分，同时更新雅可比。雅可比更新用的是递推方式：每一步积分后，用当前步的雅可比更新公式刷新。这样比最后统一算更高效，也更容易调试。

repropagate函数用于零偏更新后重新积分。如果零偏变化很小，可以直接用雅可比做一阶修正；如果变化超过阈值（比如0.01），就重新积分。这个阈值我试过几个值，0.01在大多数场景下够用，太小会导致频繁重积分，太大则一阶修正误差明显。

4.4 验证方案：用仿真数据对比三种积分方式

代码写完之后，怎么验证对不对？我的做法是：用同一段仿真数据，分别跑三种积分方式，对比最终位姿差异。

仿真数据生成方式：给定真实的运动轨迹（比如匀速圆周运动），用真实角速度和加速度生成IMU测量，再加上零偏和高斯噪声。然后分别用欧拉、中值、RK4积分，看哪种方式最接近真实轨迹。

我实测的结果是：在IMU频率100Hz、运动角速度1 rad/s的条件下，欧拉积分的位姿误差在10秒后达到0.5米左右，中值积分约0.05米，RK4约0.01米。但RK4的计算时间是中值积分的3倍多。所以中值积分确实是性价比最高的选择。

注意：验证时一定要把重力补偿加进去。我第一版仿真忘了加重力，结果三种积分方式误差都很小，还以为自己写对了，后来发现是仿真数据本身没包含重力，实际IMU数据里重力是必然存在的。

5. 常见问题与排查技巧实录

5.1 预积分结果飘移的排查思路

预积分飘移是最常见的问题，表现是静止时预积分量应该接近零，但实际跑出来一直在变。排查顺序建议如下：

1. 检查零偏是否已扣除。如果测量值里还带着零偏，积分必然飘。可以先把零偏设为零，看飘移是否减小。
2. 检查重力方向。重力符号搞反的话，速度会一直往一个方向加速。打印速度预积分量，如果单调递增，基本就是重力问题。
3. 检查四元数归一化。前面提过，不归一化会导致旋转矩阵不正交，进而影响加速度积分方向。
4. 检查时间戳单位。如果dt大了1000倍，积分结果会完全离谱。

我当初遇到的问题是第2条，重力方向搞反了，但表现不是速度单调递增，而是速度先增后减，因为旋转在变化。后来打印重力向量和旋转矩阵的乘积才定位到。

5.2 雅可比矩阵维度不匹配的解决

雅可比矩阵维度错误通常发生在四元数更新上。四元数是4维，但更新量是3维（因为模长约束），所以雅可比矩阵应该是4×3或者3×3。如果你写成4×4，编译能过，但数值会错。

我的做法是：在代码里加静态断言，检查矩阵维度。比如：

static_assert(J_R_bg_.rows() == 3 && J_R_bg_.cols() == 3, "Jacobian dimension mismatch");

另外，四元数更新时用的是三维旋转向量，更新公式是：

Quat dq = Quat::Identity() + 0.5 * Quat(0, dtheta); delta_q_ = delta_q_ * dq; delta_q_.normalize();

这里的dtheta是三维的，不是四维。我当初把dtheta写成四维，结果四元数更新后模长直接爆炸。

5.3 零偏更新后预积分量修正的注意事项

零偏更新后，预积分量需要修正。一阶修正公式是：

Δp_corrected = Δp + J_P_bg * δbg + J_P_ba * δba Δv_corrected = Δv + J_V_bg * δbg + J_V_ba * δba Δq_corrected = Δq ⊗ exp(J_R_bg * δbg)

注意旋转的修正是在流形上做的，不是简单相加。这里exp是四元数的指数映射，把三维旋转向量转成四元数。我当初直接写成四元数加法，结果旋转修正完全错误，优化时旋转量一直不收敛。

实操心得：零偏修正后，建议重新计算一次预积分量的模长和方向，和修正前对比。如果修正后变化太大（比如旋转角变化超过0.1弧度），说明零偏变化太大，一阶修正不准确，应该触发重新积分。

5.4 常见问题速查表

6. 从第一章作业延伸出的后续学习路径

第一章作业把IMU预积分这块地基打牢之后，后面章节的展开就顺理成章了。第二章通常会做视觉特征跟踪和三角化，第三章做视觉惯性联合初始化，第四章做滑动窗口优化和边缘化。每一步都依赖第一章的预积分量和雅可比。

我个人建议是：第一章作业不要只满足于“跑通”，要把推导过程手写一遍，尤其是雅可比部分。我当初偷懒直接抄了公式，结果第三章做联合优化时，残差对零偏的雅可比一直对不上，回头重新推第一章才解决问题。手写推导虽然慢，但后面省的时间更多。

另外，预积分代码建议封装成独立模块，接口设计得通用一些。后面做滑动窗口时，会有多个关键帧之间的预积分，如果接口设计得不好，代码会变得很乱。我现在的做法是：每个预积分对象只负责一对关键帧之间的积分，滑动窗口管理这些对象，边缘化时把旧对象的预积分量转移到新对象上。

这个内容后续还可以这样扩展：把预积分模块和视觉重投影误差结合起来，做一个简单的视觉惯性BA，用g2o或Ceres做优化。这样能把第一章的知识真正用起来，也能提前感受VIO系统的整体流程。