3. 相机模型与标定

做位姿估计，第一步是什么？

不是跑算法，不是调参数。是搞清楚你的摄像头到底长什么样。

我见过太多人，算法调得飞起，结果一换摄像头就崩。为什么？因为没做标定。说白了，相机模型就是摄像头看世界的“数学眼镜”。眼镜度数不对，看啥都是歪的。

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

先聊最经典的针孔模型。你想想看，小孔成像大家都做过实验吧？一个暗盒，前面戳个针眼，后面放张底片，外面的世界就倒着映上去了。

相机模型本质上就是把这个过程数学化。它把三维世界里的点，通过一个“投影中心”（就是那个针孔），映射到二维成像平面上。

数学上，这个过程用四个坐标系来描述：

• 世界坐标系：物体在真实世界的位置，单位是米。
• 相机坐标系：以相机光心为原点，Z轴朝前。
• 图像物理坐标系：在成像平面上，单位还是米，但原点在光轴与平面的交点。
• 图像像素坐标系：就是我们最终看到的图片，单位是像素，原点在左上角。

从世界坐标到像素坐标，中间经历了三次变换：

1. 刚体变换（外参）：从世界坐标系转到相机坐标系。就是旋转R和平移t。
2. 透视投影：从相机坐标系转到图像物理坐标系。这里用到了相似三角形。
3. 仿射变换（内参）：从物理坐标转到像素坐标。把米变成像素，还要考虑主点偏移。

我把这个流程画了张图，你看一眼就明白了：

内参矩阵长这样：

K = [fx,  0, cx]
    [ 0, fy, cy]
    [ 0,  0,  1]

其中 fx, fy 是焦距（像素单位），cx, cy 是主点坐标。我习惯把 fx 理解为“焦距 × 像素密度”，这样好记。

3.2 畸变模型：镜头不是完美的

理想很丰满，现实很骨感。针孔模型假设镜头是完美的，但实际镜头都有畸变。

你拍过那种边缘弯曲的照片吧？直线变曲线了。这就是畸变。

畸变主要分两种：

畸变类型	产生原因	视觉效果	数学模型
径向畸变	透镜形状不完美，光线在边缘偏折更大	桶形畸变（向外凸）或枕形畸变（向内凹）	k1, k2, k3 三个参数
切向畸变	透镜与成像平面不平行，有安装误差	图像像被“拧”了一下	p1, p2 两个参数

数学公式其实不复杂。假设 (x, y) 是归一化平面上的理想坐标，畸变后的坐标 (x_dist, y_dist) 这样算：

// 径向畸变
r² = x² + y²
x_radial = x * (1 + k1*r² + k2*r⁴ + k3*r⁶)
y_radial = y * (1 + k1*r² + k2*r⁴ + k3*r⁶)

// 切向畸变
x_tangential = 2*p1*x*y + p2*(r² + 2*x²)
y_tangential = p1*(r² + 2*y²) + 2*p2*x*y

// 总畸变
x_dist = x_radial + x_tangential
y_dist = y_radial + y_tangential

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

说到标定，绕不开张正友。1998年的，到现在还是工业界标配。为什么？因为它只需要拍几张棋盘格照片，就能把内参和畸变全搞定。

核心思路是这样的：

1. 拍棋盘格：从不同角度拍10-20张。棋盘格是平面，角点坐标已知。
2. 提取角点：用亚像素精度找到每个棋盘格的内角点。
3. 求解单应矩阵：每张照片，棋盘格平面到图像平面有一个单应矩阵 H。
4. 利用约束求内参：利用旋转矩阵的正交性，从多个 H 中解出内参。
5. 最大似然估计优化：用非线性优化（比如 Levenberg-Marquardt）微调所有参数。

我个人的习惯是：拍15张左右，棋盘格要占画面1/3以上，角度变化要大（倾斜、旋转、远近都要有）。

3.4 标定工具：OpenCV vs Matlab

理论懂了，接下来就是动手。市面上最常用的两个工具：OpenCV 和 Matlab。

对比项	OpenCV	Matlab
上手难度	需要写代码，但灵活	图形界面，点几下就行
标定精度	够用，默认用棋盘格	更高，支持多种标定板
畸变模型	支持径向+切向，最多5个参数	支持更多模型，包括鱼眼
批量处理	容易集成到自动化流程	需要手动操作
价格	免费开源	需要商业许可

我个人的建议是：

• 快速验证：用 Matlab，5分钟出结果。
• 工程部署：用 OpenCV，方便集成到代码里。
• 高精度需求：先用 Matlab 标，再用 OpenCV 做二次优化。

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)
        # 亚像素精度优化
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1),
                                     criteria)
        imgpoints.append(corners2)

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

print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

3.5 标定实战：从拍到用

最后，我分享一个完整的标定流程，这是我做项目时总结的：

1. 准备标定板：打印一张A4大小的棋盘格，贴在硬纸板上。格子数量建议9×6或12×9。
2. 拍摄照片：固定相机，移动标定板。拍15-20张，覆盖画面各个区域。
3. 初步标定：用 Matlab 或 OpenCV 跑一遍，看重投影误差。
4. 剔除坏图：如果某张图的重投影误差特别大，删掉它重新标。
5. 最终标定：用筛选后的图片再标一次，保存内参和畸变系数。
6. 验证：拍一张新的棋盘格，用标定结果去畸变，看直线是不是真的直了。

标定完成后，去畸变的代码也很简单：

# 去畸变
img_undistorted = cv2.undistort(img, mtx, dist, None, mtx)

# 或者用 remap 更高效（适合视频流）
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, mtx, 
                                          (w, h), cv2.CV_32FC1)
img_undistorted = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)