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保持不变实际上用中值积分或龙格-库塔法会更精确递推过程在连续时间下可以描述为姿态更新旋转姿态通常用旋转矩阵R或四元数q表示。其微分方程为Ṙ R * [ω]×或q̇ 0.5 * q ⊗ [0, ω]其中[ω]×是角速度的反对称矩阵。对微分方程进行积分即可从t时刻的姿态R_t得到tΔt时刻的姿态R_{tΔt}。对于四元数常用一阶近似或更精确的指数映射进行更新。速度更新速度的微分方程直接来源于牛顿第二定律在本体坐标系下的表达v̇ R * a g这里a是加速度计测量值减去零偏和重力a a_meas - b_a - R^T * gg是世界坐标系下的重力矢量通常为[0,0,-9.8]^T。对v̇积分得到速度变化。位置更新位置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的这个过程涉及一系列坐标系变换世界坐标系 - 相机坐标系通过相机的外参旋转R_cw和平移t_cw完成。P_c R_cw * P_w t_cw相机坐标系 - 归一化平面坐标系这是一个理想化的投影。将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]位于Z1的平面上与焦距无关。归一化平面 - 像素平面这一步由相机内参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点的特征点对。这里有一个关键陷阱误匹配。由于纹理重复、光照变化等总会有错误的匹配对。这些误匹配如果进入后续的状态估计会直接污染优化结果导致估计发散。因此匹配后必须进行外点剔除。最常用的方法是基础矩阵F或本质矩阵E的RANSAC利用对极几何约束随机采样少量匹配点对计算矩阵然后统计满足该矩阵的内点数量。迭代多次后选择内点最多的模型并剔除不符合该模型的外点。交叉验证对于双目相机还可以利用左右目间的极线约束进行验证。在作业中实现一个简单的特征匹配和RANSAC剔除外点流程会让你对视觉数据的不确定性有第一手的认识。你会发现即使经过RANSAC剩下的“内点”里也可能隐藏着一些难以察觉的误匹配这为后续紧耦合优化中设计鲁棒的损失函数如Huber核函数埋下了伏笔。4. 松耦合与紧耦合两种融合哲学的初探虽然第一章不实现融合但必须理解这两种架构的根本区别这决定了后续所有工作的方向。4.1 松耦合独立估计结果拼接松耦合把IMU和视觉当作两个独立的里程计。典型流程是视觉里程计VO单独运行根据图像特征点估计出相机在t和t1时刻间的相对运动Δ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一次性得到最优的状态估计。优点精度高充分利用了所有原始观测信息及其不确定性。鲁棒性强即使部分特征点被误匹配或暂时遮挡其他约束IMU和其他正确匹配的点仍然可以维持状态估计不崩溃。优化框架可以自然地处理部分信息丢失的情况。能在线估计IMU零偏IMU零偏作为状态变量的一部分被共同优化。缺点系统复杂计算量大调试困难。状态变量维度高包含所有路标点需要借助滑动窗口或边缘化等技巧来限制计算复杂度。第一章的定位我们手写的第一个VIO毫无疑问应该选择紧耦合路线。因为只有走通这条路你才能真正理解状态估计、因子图优化、滑动窗口这些VIO核心技术的精髓。松耦合作为一个概念对比帮助我们理解为什么社区选择了更复杂的道路。5. 作业实战从理论到代码的跨越理论懂了不写代码等于没懂。第一章的作业我建议按以下步骤实践这比直接看答案有效十倍。5.1 IMU数据积分轨迹生成任务给定一组IMU的角速度和加速度数据含时间戳以及初始位姿积分生成轨迹。关键步骤数据读取与同步确保IMU数据流是时间有序的。通常需要按时间戳排序。选择积分方法实现欧拉法和中值积分法。欧拉法用t时刻的测量值预测tΔt的状态中值积分法用t和tΔt时刻测量值的平均值。对比两者结果直观感受中值积分如何更好地近似连续时间运动。重力处理明确你的世界坐标系g_w方向通常是[0,0,-9.8]。在速度更新步骤中务必在正确的坐标系下减去重力。可视化将积分得到的轨迹位置序列在3D空间中画出来。同时把速度、姿态可以转换为欧拉角观察也绘制成时间序列图。你会遇到的坑四元数更新顺序四元数乘法不可交换。q_{t1} 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计算相邻帧间的相对运动。关键步骤特征处理流水线实现或调用OpenCV函数完成提取(ORB) - 描述(ORB) - 匹配(BFMatcher)。外点剔除使用匹配点对调用cv::findFundamentalMat或cv::findEssentialMat函数并设置RANSAC标志。该函数会返回一个内点掩码mask利用它剔除外点。运动估计初始化第一、二帧从本质矩阵E或基础矩阵F中恢复出相对旋转R和平移t带尺度不确定性。这里会得到4种可能的[R|t]组合需要通过三角化一点并检查深度为正来选出唯一正确的解。后续帧有了前一帧的位姿和一组3D点通过三角化得到对于新帧就可以用PnPPerspective-n-Point如EPnP、SolvePnP直接求解当前帧位姿。这比继续用对极几何更稳定、更高效。三角化利用估计出的相对位姿将匹配的特征点对三角化成3D空间点。这是构建地图的基础。你会遇到的坑尺度不确定性从E或F恢复的t是单位向量没有真实尺度。这就是单目视觉的尺度不确定性。你需要通过其他方式确定尺度比如IMU在VIO中或者假设一个初始运动距离在纯VO中。在作业中你可以简单地将平移向量t归一化这相当于假设第一次运动的平移量为1个单位。这会导致整个轨迹的尺度是任意的。三角化点的深度恢复位姿时必须检查三角化出的3D点的深度在相机坐标系下的Z值是否为正。深度为负的点意味着该点在相机后方这在物理上对于前向运动的相机是不可能的对应的[R|t]解就是错误的。匹配质量RANSAC的阈值设置很关键。阈值太小可能把正确的匹配也剔除了阈值太大则无法有效剔除外点。通常需要根据图像分辨率和噪声水平进行调整。5.3 结果分析与思考完成两项作业后不要只看结果图就结束。请进行以下分析对比将纯IMU积分轨迹和纯视觉VO轨迹画在一起注意视觉轨迹需要对齐到IMU轨迹的初始位置。观察两者的漂移趋势有何不同IMU轨迹是否快速发散视觉轨迹的尺度是否一致思考松耦合如果现在想做一个最简单的松耦合你会怎么做一个直观的想法是用VO估计的位移幅度来标定IMU积分轨迹的尺度。尝试写几行代码计算一下VO相邻帧位移的平均模长然后用它来缩放IMU的位移积分。看看融合后的轨迹是否更稳定预见紧耦合思考当前两个独立模块的缺点。对于IMU零偏无法估计对于VO尺度未知且容易受误匹配影响。如果有一个优化框架将IMU的角速度/加速度读数、特征点的像素坐标都作为观测值把位姿、速度、IMU零偏、3D点坐标都作为变量一起优化是不是理论上能同时解决所有问题这个想法就是紧耦合VIO的起点。通过这一章的手写实践你获得的不只是两段代码而是对VIO两大信息源最本真的物理直觉和数据处理经验。当你在后续章节面对预积分、滑动窗口优化、边缘化这些复杂概念时你会清楚地知道它们都是为了更好地处理你在这里亲手触摸过的IMU噪声和特征像素。地基打得越深上层建筑才能建得越稳。