4. 视觉观测模型

在VIO系统中，IMU提供高频的自身运动约束，而视觉提供的是与外部环境之间的几何观测。两者融合的前提，是必须把“三维空间中的路标点”与“图像平面上的像素观测”用一套可微、可优化的数学模型连接起来。这套模型就是视觉观测模型。它决定了残差如何构造、雅可比如何推导、外参如何进入优化，也决定了前端提取到的特征点能否被后端正确使用。

一、相机投影模型：从三维点到像素

相机投影模型描述一个相机坐标系下的三维点 P_c = [X, Y, Z]^T 如何投影到像素坐标 [u, v]^T。最常用的是针孔模型加径向畸变：

归一化平面： x = X / Z,  y = Y / Z
去畸变前：    r² = x² + y²
径向畸变：    x_d = x(1 + k1 r² + k2 r⁴)
              y_d = y(1 + k1 r² + k2 r⁴)
像素投影：    u = fx * x_d + cx
              v = fy * y_d + cy

其中 fx, fy, cx, cy 为内参，k1, k2 为径向畸变系数。工程中通常把内参和畸变统一写成标定文件，VIO前端在去畸变后再送入跟踪，避免畸变模型反复进入后端优化。需要强调的是，投影模型是非线性的，Z 出现在分母上，因此深度越小，像素对深度的敏感度越高，这也是近处特征点对VIO精度贡献更大的原因。

二、特征点与光流：视觉前端的两种观测方式

视觉前端负责从图像中产生可跟踪的观测。常见路线有两条：特征点法和光流法。特征点法先检测角点（如FAST、Shi-Tomasi），再计算描述子（如ORB、BRIEF），通过描述子匹配建立帧间对应关系；光流法则不计算描述子，而是假设亮度恒定，直接追踪同一像素在下一帧中的位置。

光流的核心假设是灰度不变：

I(x+dx, y+dy, t+dt) ≈ I(x, y, t)
一阶展开后得到光流约束方程：
Ix * u + Iy * v + It = 0

其中 u, v 是像素运动速度。单个像素方程欠定，因此实际使用Lucas-Kanade方法，在一个小窗口内假设运动一致，用最小二乘求解。VIO中广泛使用光流，是因为它速度快、对纹理要求低，且能直接得到亚像素级的跟踪点，配合IMU预积分可以在后端形成紧耦合的视觉残差。

三、重投影误差：视觉残差的统一形式

重投影误差是视觉观测模型进入优化的最终形式。给定世界坐标系下的路标 P_w、相机位姿 T_cw 和观测像素 z_obs，残差定义为：

P_c = T_cw * P_w
z_pred = π(P_c)
r_vis = z_obs - z_pred

这个残差同时依赖相机位姿、路标位置、外参和内参。在VIO中，位姿由IMU预积分提供先验，路标由前端跟踪维持，外参通常作为待优化量或固定量参与。重投影误差的雅可比需要分别对位姿、路标和外参求导，其中对位姿的导数会用到投影模型对 P_c 的偏导，再乘以位姿扰动模型。工程上，这一部分最容易出错的地方是坐标系方向：T_cw 表示从世界到相机，还是从相机到世界，必须与代码中的定义严格一致。

四、外参标定：相机与IMU的几何桥梁

VIO中的外参 T_ic 描述相机与IMU之间的相对位姿。它把IMU坐标系下的运动预测与相机坐标系下的视觉观测统一到同一参考系。外参误差会直接表现为重投影误差的系统性偏移，尤其在快速旋转或大加速度场景下，外参不准会导致视觉残差与IMU残差相互矛盾。

外参标定通常分两步：先离线标定，使用标定板或标定场，通过多帧观测求解相机-IMU相对位姿；再在线估计，把外参作为状态量加入优化，利用VIO运行中的观测不断修正。工程中常见做法是离线标定给出初值，在线优化只做小范围修正，避免外参和位姿、路标之间产生不可观测的耦合。

五、视觉前端概述与工程要点

视觉前端是VIO的观测入口，主要任务包括：图像去畸变、特征检测、光流跟踪、关键帧选择、外点剔除和观测管理。一个典型的VIO前端流程如下：

1. 读取图像，按标定参数去畸变；
2. 在上一帧特征点附近做光流跟踪，得到当前帧像素位置；
3. 对跟踪失败的区域补充新特征点，维持特征数量；
4. 用RANSAC或卡方检验剔除误跟踪；
5. 根据视差、跟踪时长和IMU激励选择关键帧；
6. 把特征观测、外参和内参送入后端，构造重投影残差。

下表总结了视觉观测模型中各模块的输入、输出与常见工程问题：

复盘一个典型场景：在室内手持VIO设备快速旋转时，若前端光流跟踪窗口过大，快速运动导致窗口内像素运动不一致，跟踪点会向纹理边缘滑动，重投影误差随之增大；此时若外参又有小角度偏差，后端会错误地把误差归因于位姿，导致姿态估计出现周期性抖动。解决思路是：缩小光流窗口、提高跟踪金字塔层数、在旋转段提高IMU权重，并检查外参旋转分量是否与标定值一致。

总体来看，视觉观测模型是VIO中连接图像与状态的桥梁。相机投影模型给出几何映射，特征点和光流提供观测，重投影误差把观测变成可优化的残差，外参标定保证相机与IMU在同一几何框架下对话，视觉前端则负责稳定、持续地产生高质量观测。理解这五者之间的数据流和依赖关系，是后续紧耦合优化和工程调参的基础。