3. 相机模型与标定：针孔相机模型、畸变校正、张正友标定法实战

相机，说白了就是视觉SLAM的「眼睛」。眼睛不准，后面算再多都是白搭。我刚开始做视觉SLAM时，就吃过这个亏——用了个没标定过的摄像头，结果建出来的地图歪七扭八，飞控还一个劲往墙上撞。嗯，从那以后，我每次拿到新相机，第一件事就是做标定。

3.1 针孔相机模型：从3D到2D的数学魔法

先聊聊最经典的针孔相机模型。你想想看，一个三维世界里的点，是怎么落到相机传感器上的？

简单说，就是光沿着直线传播，穿过一个小孔（光心），投射到成像平面上。这个过程可以用一个公式描述：

// 从世界坐标到像素坐标的映射
// [u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T

// 其中K是内参矩阵：
K = [fx,  0, cx
      0, fy, cy
      0,  0,  1]

这里fx、fy是焦距（以像素为单位），cx、cy是光心在像素坐标系中的位置。R和t是相机的外参，描述相机在世界坐标系中的位姿。

我个人习惯把内参理解为「相机自己的脾气」——每个相机都不一样，出厂就定死了。外参则是「相机在哪儿、朝哪儿看」，每次安装位置变了，外参就得重新算。

3.2 畸变校正：镜头不是完美的

现实中的镜头，尤其是广角镜头，会让直线变弯。这就是畸变。主要分两种：

• 径向畸变：光线在镜头边缘弯曲得更厉害，表现为「桶形畸变」或「枕形畸变」
• 切向畸变：镜头和成像平面不平行，导致图像倾斜

我在项目中遇到过最头疼的情况——用鱼眼镜头做室内SLAM，畸变大到墙角都变成圆弧了。如果不校正，特征匹配基本全错。

畸变校正的数学模型长这样：

// 径向畸变校正
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]

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

3.3 张正友标定法：棋盘格里的大学问

张正友标定法，说白了就是「拍棋盘格，算相机参数」。这个方法巧妙在哪儿？它不需要你知道棋盘格在三维空间中的精确位置，只需要知道棋盘格是平面的，且格子大小已知。

具体步骤我总结为四步：

1. 拍棋盘格：从不同角度拍10-20张照片。角度要丰富，有俯仰、有旋转、有远近
2. 提取角点：用OpenCV的findChessboardCorners()找到棋盘格内角点
3. 计算单应矩阵：每张照片都能算出一个单应矩阵H，它描述了棋盘格平面到图像平面的映射
4. 求解内参：利用多个单应矩阵的约束，解出内参矩阵K和畸变系数

3.4 实战：用OpenCV做相机标定

直接上代码。这是我常用的标定流程，已经跑过几十个相机了：

import cv2
import numpy as np
import glob

# 1. 准备棋盘格参数
CHECKERBOARD = (9, 6)  # 内角点数
square_size = 25.0     # 格子边长，单位mm

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

# 3. 存储所有图片的角点
objpoints = []  # 3D点
imgpoints = []  # 2D点

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, CHECKERBOARD, 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)

# 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))

3.5 标定结果怎么用？

标定完，拿到内参矩阵和畸变系数，接下来就是校正图像：

# 校正单张图像
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]

这里有个细节：getOptimalNewCameraMatrix的alpha参数控制是否保留所有像素。alpha=1保留全部（会有黑边），alpha=0裁剪到最大有效区域。我一般用alpha=0.5，折中一下。

3.6 知识体系总览

下面这张图，是我自己梳理的相机模型与标定的知识脉络：

从这张图可以看得很清楚：针孔模型是理论基础，畸变模型是现实修正，张正友标定法是把理论变成实际参数的工具。三者缺一不可。

---

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