4. 视觉传感器与特征
在视觉与IMU融合姿态估计系统中，视觉前端负责从图像中提取可跟踪的几何信息，而IMU提供高频的角速度和加速度观测。要让两者在同一坐标系下协同工作，必须先理解相机如何把三维世界投影为二维像素，以及如何从像素中稳定地提取和匹配特征点。本节围绕相机模型、针孔成像、畸变校正、特征点检测与匹配四个核心环节展开，为后续的视觉惯性联合优化打下基础。
一、针孔成像与相机模型
针孔模型是最常用的相机成像近似。它假设光线通过一个理想小孔，在像平面上形成倒立的投影。设相机坐标系下一点为 P_c = [X, Y, Z]^T，归一化平面坐标为 x = X/Z、y = Y/Z，再经内参矩阵 K 映射到像素坐标：
u = fx * (X/Z) + cx
v = fy * (Y/Z) + cy
K = [ fx 0 cx
0 fy cy
0 0 1 ]其中 fx、fy 为焦距（像素单位），cx、cy 为主点。内参由标定获得，外参则描述相机相对世界或IMU的位姿。针孔模型忽略了镜头厚度与光线弯曲，因此实际图像还需引入畸变模型。
二、畸变校正：从畸变像素到理想像素
真实镜头存在径向畸变和切向畸变。径向畸变使直线弯曲，常用 k1、k2、k3 描述；切向畸变由镜头与成像平面不平行引起，用 p1、p2 描述。去畸变时通常先在归一化平面计算畸变后的坐标，再反解或迭代求理想坐标：
x_dist = x(1 + k1*r^2 + k2*r^4 + k3*r^6)
+ 2*p1*x*y + p2*(r^2 + 2*x^2)
y_dist = y(1 + k1*r^2 + k2*r^4 + k3*r^6)
+ p1*(r^2 + 2*y^2) + 2*p2*x*y
r^2 = x^2 + y^2工程上常使用 OpenCV 的 initUndistortRectifyMap 与 remap 预计算映射表，实现实时去畸变。若忽略畸变，特征点匹配会出现系统性偏移，尤其在图像边缘区域，会直接污染视觉惯性里程计的位姿估计。
三、特征点检测与匹配基础
特征点是图像中具有可重复性、显著性和可区分性的局部结构，常见有角点、斑点与边缘交点。经典方法包括 Harris、FAST、Shi-Tomasi、SIFT、SURF、ORB 等。视觉惯性系统中更偏好计算量低、分布均匀的 FAST 或 ORB 特征，并配合光流跟踪或描述子匹配。
匹配的基本流程为：检测关键点 → 计算描述子 → 依据描述子距离建立候选匹配 → 使用比率测试、交叉验证或 RANSAC 剔除误匹配。对于单目视觉惯性系统，还需通过本质矩阵或基础矩阵约束进一步筛选。
四、案例与场景：VIO 前端的典型处理
以单目视觉惯性里程计为例，前端通常按以下顺序处理一帧图像：
- 读取原始图像，利用标定参数进行去畸变；
- 在图像金字塔上提取 FAST 角点，并计算 ORB 描述子；
- 与上一帧或局部地图进行描述子匹配，同时用光流做跟踪补充；
- 通过 RANSAC 与对极几何剔除误匹配；
- 将保留的特征点作为视觉观测，与IMU预积分残差一起送入滑动窗口优化。
在纹理丰富、光照稳定的室内场景中，ORB 特征通常能提供足够的匹配对；而在弱纹理或快速运动场景中，光流跟踪与IMU预测可以显著提升鲁棒性。
五、常见误区与复盘
| 误区 | 后果 | 改进方式 |
|---|---|---|
| 忽略畸变校正 | 边缘特征偏移，重投影误差增大 | 标定后统一去畸变，检查边缘误差 |
| 只做描述子匹配，不做几何验证 | 误匹配进入优化，位姿漂移 | 加入 RANSAC、对极几何或 PnP 验证 |
| 特征点分布过于集中 | 退化场景下约束不足 | 网格化提取，限制单格特征数量 |
| 内外参混用或时间戳不同步 | 视觉与IMU观测不一致 | 统一坐标系，严格时间同步与标定 |
掌握相机模型、畸变校正与特征匹配基础后，后续就可以进一步讨论视觉观测如何与IMU预积分联合构建残差，并在滑动窗口中完成姿态估计。