2. 相机模型与标定：从理论到ROS实战

相机标定这件事，我做了不下几十次了。每次带新人做项目，我都会让他们先亲手标定一次相机。为什么？因为这是视觉SLAM的基石，基础不牢，后面全是坑。

今天我们就来聊聊相机模型和标定。说白了，就是搞清楚相机是怎么把三维世界变成二维图像的，以及怎么纠正它带来的各种变形。

2.1 针孔相机模型：最朴素的成像原理

先说说最简单的针孔相机模型。你想想看，一个小孔，光线穿过它，在后面的平面上成像。这就是最原始的相机。

数学上怎么描述？很简单，一个点P(X, Y, Z)在图像上的投影位置p(u, v)满足：

u = fx * X / Z + cx
v = fy * Y / Z + cy

这里的fx、fy是焦距参数，cx、cy是光心偏移。我习惯把它们写成矩阵形式：

[u]   [fx  0  cx] [X]
[v] = [ 0 fy  cy] [Y]
[1]   [ 0  0   1] [Z]

这个3x3矩阵就是相机内参矩阵K。嗯，这里要注意，实际相机不会这么理想，但理解这个模型是第一步。

2.2 畸变模型：镜头带来的烦恼

我在项目中遇到过最头疼的事，就是标定完发现畸变参数不对，导致建图全是弯曲的。为什么会这样？因为镜头畸变分两种：

• 径向畸变：镜头形状引起的，越靠近边缘越明显。用k1, k2, k3三个参数描述。
• 切向畸变：镜头安装不平行引起的。用p1, p2两个参数描述。

畸变矫正的公式长这样：

x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y

看着复杂？其实你只需要知道，标定就是求出这5个畸变参数（k1, k2, p1, p2, k3）。

2.3 相机内参与外参：谁在哪儿，看到了什么

内参是相机自己的属性，焦距、光心、畸变参数。外参是相机在世界坐标系中的位置和朝向。

我习惯这样理解：

• 内参：相机本身的"眼睛"参数，出厂就定了
• 外参：相机放在哪、朝哪看，每次安装都会变

外参用旋转矩阵R和平移向量t表示。在SLAM里，我们经常需要把世界坐标系的点转换到相机坐标系：

P_camera = R * P_world + t

然后通过内参投影到图像平面。这就是整个成像过程。

2.4 张正友标定法：经典中的经典

张正友标定法，说白了就是用棋盘格照片来算内参和畸变。为什么用棋盘格？因为角点好检测，而且几何关系明确。

步骤其实不复杂：

1. 打印一张棋盘格，贴在平面上
2. 从不同角度拍10-20张照片
3. 检测每张照片的角点
4. 用最小二乘法求解内参和畸变参数
5. 用最大似然估计优化结果

我记得第一次手动实现这个算法时，花了整整两天。现在有了OpenCV，几行代码就搞定：

import cv2
import numpy as np

# 准备棋盘格角点坐标
pattern_size = (9, 6)  # 内角点数
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)

# 收集所有图片的角点
objpoints = []  # 世界坐标系中的点
imgpoints = []  # 图像坐标系中的点

for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None)

2.5 使用ROS进行相机标定：自动化流程

ROS提供了非常方便的标定工具——camera_calibration包。我建议新手直接用这个，省时省力。

具体步骤：

1. 启动相机驱动节点
2. 运行标定节点
3. 移动棋盘格，采集数据
4. 点击"Calibrate"计算参数
5. 点击"Save"保存结果

命令行操作：

# 启动相机（以USB摄像头为例）
roslaunch usb_cam usb_cam-test.launch

# 启动标定节点
rosrun camera_calibration cameracalibrator.py \
    --size 8x6 \
    --square 0.025 \
    image:=/usb_cam/image_raw \
    camera:=/usb_cam

参数说明：

• --size：棋盘格内角点数（宽x高）
• --square：棋盘格方格边长，单位米
• image：图像话题
• camera：相机命名空间

标定结果会生成一个YAML文件，内容类似：

image_width: 640
image_height: 480
camera_name: usb_cam
camera_matrix:
  rows: 3
  cols: 3
  data: [530.82, 0, 320.5, 0, 531.23, 240.3, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.283, 0.052, 0.001, -0.002, 0.0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 3
  data: [530.82, 0, 320.5, 0, 531.23, 240.3, 0, 0, 1]

标定完成后，记得验证一下。我习惯拍一张棋盘格照片，用标定结果去畸变，然后看角点是否对齐。如果偏差超过1个像素，建议重新标定。

好了，相机模型和标定就聊到这儿。下一章我们会讲特征点提取与匹配，到时候这些标定参数就要派上大用场了。