3. 单目相机模型

单目视觉 SLAM 的第一步，是把图像上的像素坐标与真实三维世界建立可计算的数学联系。这个联系由相机模型描述：针孔模型给出理想投影关系，畸变模型修正真实镜头的非线性偏差，内参标定确定相机自身的投影参数，外参标定确定相机在机器人或世界坐标系中的位姿。四者共同构成 SLAM 前端观测方程的基础。

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

针孔模型假设光线通过一个理想小孔后在成像平面投影。设相机坐标系下一点 P_c = (X, Y, Z)，归一化平面坐标为 (X/Z, 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 是主点坐标。针孔模型是线性的，但真实镜头存在制造与装配误差，因此必须引入畸变模型。

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

畸变主要分为径向畸变和切向畸变。径向畸变由镜头形状引起，表现为桶形或枕形；切向畸变由镜头与成像平面不平行引起。常用 Brown-Conrady 模型：

r² = x² + y²
x_dist = x(1 + k1 r² + k2 r⁴ + k3 r⁶) + 2 p1 x y + p2(r² + 2x²)
y_dist = y(1 + k1 r² + k2 r⁴ + k3 r⁶) + p1(r² + 2y²) + 2 p2 x y

其中 k1、k2、k3 为径向畸变系数，p1、p2 为切向畸变系数。SLAM 中通常先对图像去畸变，再提取特征点，避免畸变破坏对极几何与重投影误差模型。

三、内参标定：确定 K 与畸变系数

内参标定通过已知尺寸的标定板（棋盘格、圆点板或 AprilTag）建立多组三维点与像素点的对应关系，求解 fx、fy、cx、cy、k1、k2、p1、p2。常用工具包括 OpenCV、Kalibr 和 MATLAB Camera Calibrator。标定流程如下：

1. 采集 15～30 张不同角度、不同距离的标定板图像，覆盖画面边缘区域。
2. 检测角点，建立标定板坐标系下的三维点与图像二维点的对应。
3. 用张正友标定法求初值，再以最小二乘或 LM 优化重投影误差。
4. 检查平均重投影误差，通常要求小于 0.3 像素；过大说明图像模糊或角点检测不准。

# OpenCV 标定示例
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    object_points, image_points, image_size, None, None)
print("重投影误差:", ret)
print("内参 K:\n", K)
print("畸变系数:", dist.ravel())

四、外参标定与相机坐标系变换

外参描述相机相对世界坐标系或机器人本体的位姿，包括旋转 R 与平移 t。世界点 P_w 到相机点 P_c 的变换为：

P_c = R_cw * P_w + t_cw
T_cw = [ R_cw  t_cw
          0     1  ]

在 SLAM 中，外参分为两类：一是相机与 IMU、轮速计、激光雷达之间的固定外参，需通过联合标定获得；二是相机在每一帧的位姿，由 SLAM 跟踪与优化实时估计。前者是常量，后者是状态量，二者不可混淆。

五、核心关系对比

六、案例与常见误区

假设一台单目相机内参为 fx=fy=520, cx=320, cy=240，图像尺寸 640×480。某特征点像素为 (400, 300)，若已知该点深度 Z=2m，则相机坐标可反投影为：

X = (u - cx) * Z / fx = (400-320)*2/520 ≈ 0.308 m
Y = (v - cy) * Z / fy = (300-240)*2/520 ≈ 0.231 m
P_c ≈ (0.308, 0.231, 2.000)

常见误区包括：只标定一次就长期使用，忽略温度与碰撞导致的内参漂移；把外参与内参混为一谈，导致重投影误差异常；去畸变时只使用 k1 而忽略 k2、k3，边缘区域误差明显；标定图像集中在画面中心，导致边缘畸变系数估计不准。工程上建议定期复标，并在 SLAM 初始化阶段用重投影误差监控标定质量。

掌握上述模型后，SLAM 中的重投影误差、PnP 位姿求解、三角化深度估计才有统一坐标基础。工程实现时，应把内参、畸变系数和外参分别配置管理，并在系统启动时校验标定文件版本与图像分辨率是否匹配，避免因参数错配导致定位漂移。