4. 相机模型与视觉几何

在视觉惯性融合定位（VIO）系统中，视觉前端负责从图像中提取可用的几何约束，而惯性测量单元（IMU）提供高频的运动先验。要让两者在同一个优化框架下协同工作，必须首先建立精确的相机成像模型，并理解多视图几何中的核心关系。本节将围绕针孔模型、畸变模型、对极几何、三角化、PnP以及光束法平差（Bundle Adjustment, BA）六个关键概念展开，它们是VIO工程中视觉约束构建与求解的基石。

一、针孔模型：从三维点到像素的映射

针孔模型是最常用的相机成像近似模型。它假设光线通过一个理想的小孔，在成像平面上形成倒立的投影。在工程中，我们通常将成像平面虚拟到相机前方，得到正立的像。设相机坐标系下一点 P_c = [X, Y, Z]^T，其归一化坐标为 [X/Z, Y/Z, 1]^T，经过内参矩阵 K 投影到像素平面：

p = K * [X/Z, Y/Z, 1]^T
K = [ fx  0  cx
       0 fy  cy
       0  0   1 ]

其中 fx, fy 为焦距（以像素为单位），cx, cy 为主点坐标。针孔模型忽略了镜头畸变，适用于视场角较小、畸变已校正的场景。在VIO中，我们通常先对图像去畸变，再使用针孔模型进行后续几何计算。

二、畸变模型：径向与切向畸变

真实镜头存在制造误差和装配误差，导致图像产生畸变。最常见的两种是径向畸变和切向畸变。径向畸变由镜头形状引起，表现为桶形或枕形失真；切向畸变由镜头与成像平面不平行引起。工程中常用Brown-Conrady模型：

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，k1, k2, k3 为径向畸变系数，p1, p2 为切向畸变系数。在VIO系统中，畸变参数通常通过离线标定获得，并在图像预处理阶段进行去畸变，或者直接在优化中作为参数估计。对于广角或鱼眼相机，还需使用更复杂的畸变模型（如KB模型）。

三、对极几何：两视图间的约束关系

对极几何描述了两个相机视图之间的内在射影关系。设两个相机中心分别为 O1 和 O2，空间点 P 在两个图像上的投影分别为 p1 和 p2。对极约束指出，p1 对应的极线 l2 必然经过 p2，且满足：

p2^T * F * p1 = 0
F = K2^-T * E * K1^-1
E = [t]_x * R

其中 F 为基础矩阵，E 为本质矩阵，R 和 t 为两相机间的旋转和平移。对极几何在VIO中用于特征匹配的几何验证、剔除误匹配，以及初始化阶段的相对位姿估计。它不依赖于场景结构，仅由相机运动决定。

四、三角化：从二维观测恢复三维点

三角化是利用多帧图像中的同名点观测，恢复空间点三维坐标的过程。给定两个相机位姿 T1, T2 和归一化观测 x1, x2，三角化求解满足投影关系的三维点 X。常用方法包括线性三角化（DLT）和基于优化的非线性三角化。线性方法构造 AX = 0 的齐次方程，通过SVD求解；非线性方法则最小化重投影误差：

X* = argmin_X Σ ||π(T_i * X) - x_i||^2

三角化的精度受基线长度、观测噪声和相机分辨率影响。在VIO中，三角化用于初始化地图点，并为后续的BA提供初值。基线过短会导致深度不确定性急剧增大，因此需要足够的运动激励。

五、PnP：已知三维点求解相机位姿

Perspective-n-Point（PnP）问题是指：已知 n 个三维空间点及其在图像上的二维投影，求解相机的位姿 R, t。当 n ≥ 3 时，问题可解。常用方法包括P3P、EPnP、迭代最近点等。在VIO中，PnP常用于重定位、闭环检测后的位姿恢复，以及基于已知地图点的帧间位姿估计。其目标函数同样是最小化重投影误差：

{R*, t*} = argmin_{R,t} Σ ||π(K(R*X_i + t)) - x_i||^2

实际工程中，通常结合RANSAC处理外点，提高位姿估计的鲁棒性。

六、光束法平差：联合优化所有参数

光束法平差（Bundle Adjustment, BA）是视觉SLAM和VIO中的核心优化技术。它同时优化相机位姿、三维点坐标以及可能的相机内参和畸变参数，最小化所有观测的重投影误差：

min_{T_i, X_j} Σ_{i,j} ρ(||π(T_i, X_j) - x_ij||^2)

其中 ρ 为鲁棒核函数（如Huber），用于抑制外点影响。BA通常使用Levenberg-Marquardt算法求解，利用稀疏性加速。在VIO中，BA与IMU预积分联合优化，形成视觉惯性BA，是精度最高的融合方式。工程中常采用滑动窗口BA，仅优化最近若干帧的状态，以控制计算量。

七、案例与场景：VIO初始化中的几何处理

以单目VIO初始化为例：系统首先使用对极几何估计初始帧间的相对位姿，然后三角化得到一批地图点。接着，利用这些地图点和后续帧的观测，通过PnP估计新帧位姿。最后，将视觉重投影误差与IMU预积分误差联合，进行光束法平差，完成尺度、重力方向、速度等状态的初始化。若畸变未校正，重投影误差会显著增大，导致初始化失败或精度下降。

八、常见误区与复盘

• 忽略畸变：在广角镜头下直接使用针孔模型，会导致边缘区域重投影误差巨大，BA难以收敛。
• 三角化基线过短：纯旋转或微小平移时，三角化深度不确定度极高，应避免将此类点加入地图。
• 对极几何误用：基础矩阵估计对噪声敏感，需配合RANSAC和足够的内点数量。
• BA中固定内参：若相机内参标定不准，应在BA中放开内参优化，或至少加入先验约束。
• 忽略鲁棒核：未使用Huber等核函数时，少量外点会严重拉偏优化结果。

九、关键参数与工具清单

参数/工具	说明	典型值/方法
fx, fy	焦距（像素）	标定获得，如 500~1500
cx, cy	主点坐标	图像中心附近
k1, k2, p1, p2	畸变系数	标定获得，k1常为负
基础矩阵F	对极几何约束	8点法+RANSAC
三角化	恢复三维点	DLT或非线性优化
PnP	位姿估计	EPnP+RANSAC
BA	联合优化	LM算法，滑动窗口

掌握上述六个核心概念，是构建稳定、高精度VIO系统的前提。在实际工程中，需要根据相机类型、场景尺度和计算资源，灵活选择模型与算法，并通过实验验证每一步的精度与鲁棒性。