手写VIO：从IMU运动学与视觉几何基础入门视觉惯性里程计

1. 项目缘起：为什么从“手写VIO”开始？

如果你正在接触机器人、无人机或者自动驾驶，那么“VIO”这个词大概率已经在你眼前晃过无数次了。VIO，视觉惯性里程计，简单说就是让机器同时用“眼睛”（相机）和“感觉”（IMU）来知道自己在哪里、往哪走。听起来很酷，对吧？但当你真正打开那些开源框架，比如VINS-Mono、OKVIS，或者ORB-SLAM3里集成的VIO模块，面对动辄数万行的代码和层层叠叠的数学公式时，很容易就懵了。这种感觉我太熟悉了，就像给你一本武功秘籍，但全是文言文，还缺了最关键的心法口诀。

所以，这个“手写VIO”系列，我想做的不是另一个代码搬运教程。它的核心目的，是**“拆解黑盒，重建直觉”**。我们不用任何现成的VIO库，就从最原始的传感器数据开始，一行行代码，把整个VIO系统搭起来。为什么非要“手写”？因为只有当你亲手把IMU的角速度、加速度积分成位姿，当你亲自推导并实现视觉重投影误差，当你调试一个bug调到头秃最后发现是四元数更新写反了的时候，那些书本上的公式、论文里的优化理论，才会真正变成你肌肉记忆的一部分。这不是为了造轮子，而是为了彻底理解轮子是怎么转的。

基于这个想法，第一章的目标非常明确：打下坚实的地基。这一章不涉及复杂的多传感器融合，我们只聚焦两件事：IMU运动学与视觉几何基础。我会带你重新审视IMU数据，理解它究竟告诉了我们什么，以及如何从这些带有噪声的测量中，一步步推演出设备的运动轨迹。同时，我们会梳理相机模型和特征点匹配，这是视觉信息能用于定位的前提。你会发现，很多后续紧耦合优化中让人头疼的问题，比如IMU偏差、时间同步、特征参数化，其根源都在这一章的基础概念里。

2. IMU运动学：从数据流到位姿推演

VIO中，IMU是高频（通常200-1000Hz）提供运动信息的传感器。它输出的是三轴角速度（陀螺仪）和三轴线加速度（加速度计）。但请注意，加速度计测量的并不是我们通常理解的“速度变化率”，而是比力——即物体所受的除引力外所有力的合力所产生的“非引力加速度”。这是理解IMU数据的第一步，也是最容易出错的一步。

2.1 IMU测量模型与噪声拆解

我们拿到的原始IMU数据远非完美。一个更贴近现实的测量模型如下：

角速度测量：ω_meas = ω_true + b_g + η_g其中，ω_true是真实角速度，b_g是缓慢变化的陀螺仪零偏（Bias），η_g是白噪声。

加速度测量：a_meas = a_true + b_a + η_a + g^b这里多了一项g^b，它表示在IMU本体坐标系（b系）下观察到的重力加速度。a_true是本体相对于惯性系的真实加速度（即比力）。

关键理解：b_a和b_g这些零偏并不是常数，它们会随着温度、时间缓慢漂移。如果我们忽略它们，直接积分，误差会迅速累积，轨迹很快就会“飞”掉。这也是为什么低成本的消费级IMU（如手机里的）很难独立进行长时间定位的原因。

噪声η我们通常建模为高斯白噪声，而零偏b则建模为随机游走过程（其导数是白噪声）。这引出了IMU数据处理中的一个核心概念：噪声的“颜色”。白噪声是高频的、不相关的，而随机游走（零偏）是低频的、相关的。在后续的滤波器（如ESKF）或优化器中，我们会用不同的噪声协方差矩阵Q来分别描述它们对状态估计的影响。

2.2 姿态、速度、位置的递推（积分）

有了测量值，如何得到位姿？核心是积分。假设在时间间隔Δt内，角速度ω和加速度a保持不变（实际上用中值积分或龙格-库塔法会更精确），递推过程在连续时间下可以描述为：

1. 姿态更新（旋转）：姿态通常用旋转矩阵R或四元数q表示。其微分方程为：Ṙ = R * [ω]×或q̇ = 0.5 * q ⊗ [0, ω]其中[ω]×是角速度的反对称矩阵。对微分方程进行积分，即可从t时刻的姿态R_t，得到t+Δt时刻的姿态R_{t+Δt}。对于四元数，常用一阶近似或更精确的指数映射进行更新。

2. 速度更新：速度的微分方程直接来源于牛顿第二定律在本体坐标系下的表达：v̇ = R * a + g这里a是加速度计测量值减去零偏和重力（a = a_meas - b_a - R^T * g），g是世界坐标系下的重力矢量（通常为[0,0,-9.8]^T）。对v̇积分得到速度变化。

3. 位置更新：位置p的微分最简单：ṗ = v对速度积分即得到位置变化。

手写作业启示：在作业中实现这个递推流程，你会立刻遇到两个问题： 第一，离散化误差。上述是连续形式，代码里必须离散化。最简单的欧拉法误差大，中值积分（取Δt区间内测量值的平均）是更常用且简单的选择。 第二，重力处理。加速度计测量值包含重力，必须在正确的坐标系下减去。通常我们在世界系（w系）进行积分，所以需要先将本体系（b系）下的加速度测量值旋转到世界系：a_w = R * (a_meas - b_a)，然后减去世界系重力g_w，得到真实的运动加速度a_motion = a_w - g_w，再用它来更新速度。

2.3 一个简单的预积分概念铺垫

直接按上述步骤积分，会带来一个严重问题：每次优化更新了t时刻的姿态R_t后，t之后所有的位姿都需要重新积分，计算量巨大。这就是预积分技术要解决的核心问题。虽然第一章不深入，但可以建立直觉：预积分的思想是，将两个关键帧i和j之间的所有IMU测量值，预先积分为一个与i时刻姿态无关的相对运动量ΔR_ij, Δv_ij, Δp_ij。这样，当i时刻的姿态优化后，只需一次旋转即可更新j时刻的状态，而无需重新遍历所有IMU数据。理解好基础的积分，是后续理解预积分的前提。

3. 视觉几何基础：相机如何观测世界

视觉为VIO提供了绝对尺度（IMU无法提供）和回环检测以消除累积误差的能力。第一章的视觉部分，重点是理解单个相机如何将3D点映射到2D像素。

3.1 相机模型：从3D到2D的映射链

一个3D空间点P_w = [X, Y, Z]^T是如何变成照片上一个像素点[u, v]^T的？这个过程涉及一系列坐标系变换：

1. 世界坐标系 -> 相机坐标系：通过相机的外参（旋转R_cw和平移t_cw）完成。P_c = R_cw * P_w + t_cw

2. 相机坐标系 -> 归一化平面坐标系：这是一个理想化的投影。将P_c = [X_c, Y_c, Z_c]^T除以深度Z_c，得到归一化坐标：p_n = [X_c/Z_c, Y_c/Z_c, 1]^T = [x, y, 1]^T这个[x, y]位于Z=1的平面上，与焦距无关。

3. 归一化平面 -> 像素平面：这一步由相机内参K完成，它包含了焦距fx, fy和主点cx, cy，同时处理了透镜畸变。首先处理畸变：对于径向畸变和切向畸变，有相应的模型（如Brown-Conrady模型）。用归一化坐标[x, y]计算畸变后的坐标[x_d, y_d]。然后进行投影：[u, v, 1]^T = K * [x_d, y_d, 1]^T，其中K = [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]。

实操心得：在代码中，我强烈建议将“去畸变”和“投影”写成独立的函数。很多开源代码将两者耦合，但在调试时，你经常需要检查畸变模型是否正确，或者单独验证投影部分。清晰的模块划分能节省大量调试时间。

3.2 特征提取与匹配：数据的关联

VIO不直接处理稠密的图像像素，而是处理特征点（如角点、边缘）。第一章你需要实践的是：

• 特征提取：使用如FAST、ORB、SIFT等算法从图像中提取关键点。
• 特征描述：为每个关键点计算一个描述子（如ORB的256位二进制描述子），使其对光照、视角变化有一定鲁棒性。
• 特征匹配：通过比较描述子（如汉明距离），找到相邻图像间属于同一个3D点的特征点对。

这里有一个关键陷阱：误匹配。由于纹理重复、光照变化等，总会有错误的匹配对。这些误匹配如果进入后续的状态估计，会直接污染优化结果，导致估计发散。因此，匹配后必须进行外点剔除。最常用的方法是：

1. 基础矩阵（F）或本质矩阵（E）的RANSAC：利用对极几何约束，随机采样少量匹配点对计算矩阵，然后统计满足该矩阵的内点数量。迭代多次后，选择内点最多的模型，并剔除不符合该模型的外点。
2. 交叉验证：对于双目相机，还可以利用左右目间的极线约束进行验证。

在作业中实现一个简单的特征匹配和RANSAC剔除外点流程，会让你对视觉数据的不确定性有第一手的认识。你会发现，即使经过RANSAC，剩下的“内点”里也可能隐藏着一些难以察觉的误匹配，这为后续紧耦合优化中设计鲁棒的损失函数（如Huber核函数）埋下了伏笔。

4. 松耦合与紧耦合：两种融合哲学的初探

虽然第一章不实现融合，但必须理解这两种架构的根本区别，这决定了后续所有工作的方向。

4.1 松耦合：独立估计，结果拼接

松耦合把IMU和视觉当作两个独立的里程计。典型流程是：

• 视觉里程计（VO）：单独运行，根据图像特征点估计出相机在t和t+1时刻间的相对运动ΔR_vo, Δt_vo。
• IMU积分：在相同时间段内，对IMU数据进行积分，得到相对运动估计ΔR_imu, Δv_imu, Δp_imu。
• 融合：在一个滤波器（最常见的是扩展卡尔曼滤波器EKF）中，将VO估计的位移（Δt_vo）与IMU积分的位置（Δp_imu）进行融合。由于VO提供了绝对尺度，可以借此来校正IMU积分因加速度计零偏和尺度因子造成的尺度漂移。

优点：系统结构简单，模块化好，VO和IMU可以独立调试。如果VO暂时失效（如快速运动导致图像模糊），IMU仍能短时间提供运动估计。缺点：损失了信息。VO在计算ΔR_vo, Δt_vo时，已经丢弃了特征点的原始观测信息，只用了它们的统计结果（如通过对极几何或PnP算出的位姿）。这个过程中，观测的不确定性被简化了。而且，VO本身在纹理缺失或动态物体场景下容易失败，这个失败的结果会直接作为“坏数据”输入给滤波器。

4.2 紧耦合：原始数据层级的融合

紧耦合是当前主流VIO方案的选择。它的核心思想是：将IMU的原始测量值和视觉特征点的原始像素坐标，共同放入一个优化框架中，去估计系统状态。

在紧耦合中，我们构建一个整体的代价函数：J = Σ ||z_imu - h_imu(x)||^2_{Σ_imu} + Σ ||z_vision - h_vision(x)||^2_{Σ_vision}其中：

• x是待优化的状态变量（包括位姿、速度、IMU零偏、3D路标点坐标等）。
• z_imu是IMU的原始角速度和加速度测量值（或预积分量）。
• h_imu(x)是根据状态x预测的IMU测量值。
• z_vision是特征点的像素坐标观测值。
• h_vision(x)是根据状态x和3D路标点位置，将路标点投影到像素平面的预测坐标（即重投影）。
• ||·||^2_Σ表示马氏距离，由各自测量噪声的协方差矩阵Σ加权。

然后，我们使用非线性优化方法（如高斯-牛顿、列文伯格-马夸尔特）最小化这个代价函数J，一次性得到最优的状态估计。

优点：

1. 精度高：充分利用了所有原始观测信息及其不确定性。
2. 鲁棒性强：即使部分特征点被误匹配或暂时遮挡，其他约束（IMU和其他正确匹配的点）仍然可以维持状态估计不崩溃。优化框架可以自然地处理部分信息丢失的情况。
3. 能在线估计IMU零偏：IMU零偏作为状态变量的一部分被共同优化。

缺点：系统复杂，计算量大，调试困难。状态变量维度高（包含所有路标点），需要借助滑动窗口或边缘化等技巧来限制计算复杂度。

第一章的定位：我们手写的第一个VIO，毫无疑问应该选择紧耦合路线。因为只有走通这条路，你才能真正理解状态估计、因子图优化、滑动窗口这些VIO核心技术的精髓。松耦合作为一个概念对比，帮助我们理解为什么社区选择了更复杂的道路。

5. 作业实战：从理论到代码的跨越

理论懂了，不写代码等于没懂。第一章的作业，我建议按以下步骤实践，这比直接看答案有效十倍。

5.1 IMU数据积分轨迹生成

任务：给定一组IMU的角速度和加速度数据（含时间戳），以及初始位姿，积分生成轨迹。关键步骤：

1. 数据读取与同步：确保IMU数据流是时间有序的。通常需要按时间戳排序。
2. 选择积分方法：实现欧拉法和中值积分法。欧拉法用t时刻的测量值预测t+Δt的状态；中值积分法用t和t+Δt时刻测量值的平均值。对比两者结果，直观感受中值积分如何更好地近似连续时间运动。
3. 重力处理：明确你的世界坐标系g_w方向（通常是[0,0,-9.8]）。在速度更新步骤中，务必在正确的坐标系下减去重力。
4. 可视化：将积分得到的轨迹（位置序列）在3D空间中画出来。同时，把速度、姿态（可以转换为欧拉角观察）也绘制成时间序列图。

你会遇到的坑：

• 四元数更新顺序：四元数乘法不可交换。q_{t+1} = q_t ⊗ δq，其中δq是由ω和Δt计算出的增量四元数。搞反顺序会导致姿态完全错误。
• 坐标系混淆：IMU测量值是在本体坐标系（b系）。积分时，我们通常在世界坐标系（w系）下进行。因此，在更新速度时，需要将a_meas从b系转换到w系（R * a_meas），然后再减去w系的重力。很多初学者会错误地在b系下减重力。
• 零偏的影响：尝试在数据中人为加入一个固定的零偏b_a（如[0.1, 0.05, -0.05] m/s^2），再积分。观察轨迹如何迅速发散。这能让你深刻理解零偏在线估计的必要性。

5.2 特征点视觉里程计（VO）

任务：给定一个图像序列，提取特征点并进行匹配，利用对极几何或PnP计算相邻帧间的相对运动。关键步骤：

1. 特征处理流水线：实现或调用OpenCV函数完成提取(ORB) -> 描述(ORB) -> 匹配(BFMatcher)。
2. 外点剔除：使用匹配点对，调用cv::findFundamentalMat或cv::findEssentialMat函数，并设置RANSAC标志。该函数会返回一个内点掩码（mask），利用它剔除外点。
3. 运动估计：
   • 初始化（第一、二帧）：从本质矩阵E或基础矩阵F中恢复出相对旋转R和平移t（带尺度不确定性）。这里会得到4种可能的[R|t]组合，需要通过三角化一点并检查深度为正来选出唯一正确的解。
   • 后续帧：有了前一帧的位姿和一组3D点（通过三角化得到），对于新帧，就可以用PnP（Perspective-n-Point，如EPnP、SolvePnP）直接求解当前帧位姿。这比继续用对极几何更稳定、更高效。
4. 三角化：利用估计出的相对位姿，将匹配的特征点对三角化成3D空间点。这是构建地图的基础。

你会遇到的坑：

• 尺度不确定性：从E或F恢复的t是单位向量，没有真实尺度。这就是单目视觉的尺度不确定性。你需要通过其他方式确定尺度，比如IMU（在VIO中），或者假设一个初始运动距离（在纯VO中）。在作业中，你可以简单地将平移向量t归一化，这相当于假设第一次运动的平移量为1个单位。这会导致整个轨迹的尺度是任意的。
• 三角化点的深度：恢复位姿时，必须检查三角化出的3D点的深度（在相机坐标系下的Z值）是否为正。深度为负的点意味着该点在相机后方，这在物理上对于前向运动的相机是不可能的，对应的[R|t]解就是错误的。
• 匹配质量：RANSAC的阈值设置很关键。阈值太小，可能把正确的匹配也剔除了；阈值太大，则无法有效剔除外点。通常需要根据图像分辨率和噪声水平进行调整。

5.3 结果分析与思考

完成两项作业后，不要只看结果图就结束。请进行以下分析：

1. 对比：将纯IMU积分轨迹和纯视觉VO轨迹画在一起（注意视觉轨迹需要对齐到IMU轨迹的初始位置）。观察两者的漂移趋势有何不同？IMU轨迹是否快速发散？视觉轨迹的尺度是否一致？
2. 思考松耦合：如果现在想做一个最简单的松耦合，你会怎么做？一个直观的想法是：用VO估计的位移幅度，来标定IMU积分轨迹的尺度。尝试写几行代码，计算一下VO相邻帧位移的平均模长，然后用它来缩放IMU的位移积分。看看融合后的轨迹是否更稳定？
3. 预见紧耦合：思考当前两个独立模块的缺点。对于IMU，零偏无法估计；对于VO，尺度未知且容易受误匹配影响。如果有一个优化框架，将IMU的角速度/加速度读数、特征点的像素坐标都作为观测值，把位姿、速度、IMU零偏、3D点坐标都作为变量一起优化，是不是理论上能同时解决所有问题？这个想法，就是紧耦合VIO的起点。

通过这一章的手写实践，你获得的不只是两段代码，而是对VIO两大信息源最本真的物理直觉和数据处理经验。当你在后续章节面对预积分、滑动窗口优化、边缘化这些复杂概念时，你会清楚地知道，它们都是为了更好地处理你在这里亲手触摸过的IMU噪声和特征像素。地基打得越深，上层建筑才能建得越稳。