第二章：相机模型与标定

大家好，欢迎来到第二章。上一章我们聊了SLAM的整体框架，今天咱们把镜头拉近——聚焦在相机这个“眼睛”上。

做SLAM这么多年，我最大的体会是：相机标定没做好，后面一切算法都是空中楼阁。你想想看，如果相机本身都有“近视”或“散光”，那它看到的世界怎么可能准呢？

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

先说说最基础的针孔相机模型。说白了，它就是一个小黑盒，前面戳个针孔，光线穿过小孔在后面的感光元件上成像。

数学上怎么描述这个过程？我们用四个坐标系来串联：

• 世界坐标系：物体在真实世界中的位置 (Xw, Yw, Zw)
• 相机坐标系：以相机光心为原点 (Xc, Yc, Zc)
• 图像物理坐标系：成像平面上的实际位置 (x, y)
• 像素坐标系：最终图像上的像素位置 (u, v)

从世界坐标到像素坐标，其实就是一连串的矩阵乘法：

// 投影方程：从3D世界到2D像素
s * [u, v, 1]^T = K * [R|t] * [Xw, Yw, Zw, 1]^T

其中 K 是相机内参矩阵，[R|t] 是外参矩阵。内参是相机自己的“性格”，外参是相机在空间中的“位置和姿态”。

我个人习惯把内参理解为相机的“出厂设置”——它决定了空间中的一个点，最终落在图像的哪个像素上。

2.2 畸变模型——为什么照片边缘会弯曲？

理想很丰满，现实很骨感。针孔模型是理想情况，但真实相机都有镜头，镜头就会带来畸变。

我在项目中遇到过最典型的场景：用广角镜头拍棋盘格，边缘的格子都弯成了弧形。这就是径向畸变在作怪。

畸变主要分两类：

• 径向畸变：光线经过透镜边缘时弯曲更厉害，产生“桶形”或“枕形”变形
• 切向畸变：镜头和成像平面不平行，产生“倾斜”效果

数学上怎么矫正？我们用多项式来拟合畸变：

// 径向畸变矫正
x_corrected = x * (1 + k1*r^2 + k2*r^4 + k3*r^6)
y_corrected = y * (1 + k1*r^2 + k2*r^4 + k3*r^6)

// 切向畸变矫正
x_corrected = x + [2*p1*x*y + p2*(r^2 + 2*x^2)]
y_corrected = y + [p1*(r^2 + 2*y^2) + 2*p2*x*y]

其中 r 是像素到光心的距离，k1、k2、k3 是径向畸变系数，p1、p2 是切向畸变系数。

2.3 相机标定原理与方法——给相机做“体检”

标定说白了就是求解上面那些参数：内参矩阵 K 和畸变系数 (k1, k2, p1, p2, k3)。

最经典的方法是张正友标定法。它的核心思路是：拍一组不同角度的棋盘格照片，利用棋盘格上角点的已知几何关系，反推出相机参数。

为什么用棋盘格？因为棋盘格的角点检测非常稳定，而且角点之间的物理距离是已知的（比如格子边长30mm）。

标定的流程大致是：

1. 打印一张棋盘格，贴在平面上
2. 从不同角度拍摄10-20张照片
3. 检测每张照片中的角点
4. 利用角点坐标和已知物理坐标，求解单应性矩阵
5. 分解单应性矩阵得到内参和外参
6. 用非线性优化（比如LM算法）优化所有参数

下面我用SVG画一张标定流程的示意图，帮你理清思路：

2.4 OpenCV标定实战——动手做一遍

理论讲完了，咱们来点实际的。OpenCV里封装好了标定的全套工具，用起来非常方便。

先准备棋盘格：我建议用 9×6 的内角点棋盘格，格子边长30mm。打印出来贴在硬纸板上。

拍照时注意：

• 至少拍15张，角度要丰富
• 棋盘格要占画面1/4以上
• 避免反光和模糊

下面是完整的标定代码：

import cv2
import numpy as np
import glob

# 1. 准备棋盘格参数
chessboard_size = (9, 6)  # 内角点数量
square_size = 30.0        # 格子边长（mm）

# 2. 生成世界坐标系中的棋盘格角点坐标
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 
                       0:chessboard_size[1]].T.reshape(-1, 2)
objp *= square_size

# 3. 存储所有图片的角点
objpoints = []  # 世界坐标
imgpoints = []  # 像素坐标

images = glob.glob('calib_images/*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    # 检测角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
    
    if ret:
        objpoints.append(objp)
        
        # 亚像素精确化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners2)
        
        # 可视化
        cv2.drawChessboardCorners(img, chessboard_size, corners2, ret)
        cv2.imshow('Corners', img)
        cv2.waitKey(100)

cv2.destroyAllWindows()

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

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

# 5. 评估标定精度
mean_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    mean_error += error

print("重投影误差: ", mean_error / len(objpoints))

标定完成后，别忘了做畸变矫正：

# 畸变矫正
img = cv2.imread('test.jpg')
h, w = img.shape[:2]

# 获取最优相机矩阵（可以裁剪掉畸变严重的边缘）
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))

# 矫正
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

# 裁剪
x, y, w, h = roi
dst = dst[y:y+h, x:x+w]

cv2.imshow('Corrected', dst)
cv2.waitKey(0)

2.5 标定中的常见坑

做了这么多年标定，我踩过的坑可以写本书了。这里挑几个最常见的：

问题	现象	解决方法
棋盘格照片太少	标定结果不稳定，每次跑都不一样	至少拍15张，角度要覆盖整个视野
棋盘格贴得不平	重投影误差很大	用玻璃板夹住，或者买专业的标定板
照片模糊	角点检测不准	保证快门速度足够，使用三脚架
只拍了中心区域	边缘畸变矫正效果差	让棋盘格出现在画面的各个角落

嗯，这里还要提醒一点：标定参数是跟分辨率绑定的。如果你用1920×1080标定，后来改成640×480，那内参矩阵要重新算，不能直接套用。

好了，这一章的内容就到这里。相机标定是SLAM的基石，花时间把它做扎实了，后面的路会顺畅很多。下一章我们聊聊特征点提取与匹配——也就是SLAM系统怎么“认路”的。

---

公众号:蓝海资料掘金营，微信deep3321