相机模型与投影几何：针孔相机模型、畸变模型、相机标定基础

说到SLAM前端，特征提取和匹配是重头戏。但有个前提你绕不开——相机模型。说白了，你得知道相机是怎么把三维世界变成二维图像的。这个搞不清楚，后面的三角化、位姿估计全是空中楼阁。

我个人习惯，每次拿到一个新传感器，第一件事就是先做标定。为什么？因为相机模型里的参数，直接决定了你后续所有计算的精度。你想想看，一个像素偏差，在10米外可能就是几十厘米的误差，这在SLAM里是致命的。

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

针孔相机模型，是所有相机模型的基础。它假设光线通过一个小孔，在后面的成像平面上形成倒像。虽然真实相机复杂得多，但这个模型足够我们理解核心的几何关系。

核心公式其实就一个投影关系：

// 从世界坐标到像素坐标的映射
// [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是光心偏移。我在项目中遇到过，很多人以为cx、cy就是图像中心，其实不是。因为装配误差，光心往往偏几个像素。你如果直接用图像中心代替，短期看不出问题，但建图久了，累积误差会让你怀疑人生。

畸变模型：现实世界的修正

真实相机不是针孔。镜头有曲率，光线经过透镜会发生折射。这就导致了畸变。最常见的两种：径向畸变和切向畸变。

• 径向畸变：光线在透镜边缘弯曲更厉害。表现为图像边缘的直线变弯。分为桶形畸变（负）和枕形畸变（正）。
• 切向畸变：透镜和成像平面不平行。表现为图像看起来像被“拉伸”了。

畸变模型通常用多项式来拟合：

// 径向畸变参数：k1, k2, k3
// 切向畸变参数：p1, p2

// 畸变后的坐标 (x_distorted, y_distorted)：
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

嗯，这里要注意。k3一般只在畸变很大的镜头（比如鱼眼）才用。普通相机标定，用k1、k2就够了。我曾经在项目里偷懒，只标了k1，结果边缘特征点匹配总是飘。后来加上k2，问题就解决了。

相机标定基础：从理论到实践

相机标定，说白了就是求解内参和畸变参数。最经典的方法是张正友标定法。它用棋盘格作为标定物，通过多张不同角度的图像，解算出相机参数。

标定的核心步骤：

1. 采集图像：打印一张棋盘格，从不同角度拍摄15-30张照片。
2. 角点检测：用OpenCV的findChessboardCorners找到棋盘格角点。
3. 求解内参：利用单应性矩阵，解出内参矩阵K。
4. 优化畸变：用非线性最小二乘法（比如Levenberg-Marquardt）优化所有参数。

代码示例（OpenCV）：

import cv2
import numpy as np

# 棋盘格尺寸
pattern_size = (9, 6)  # 内角点数量
square_size = 0.025    # 每个格子边长（米）

# 准备世界坐标系中的点
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)
objp *= square_size

# 存储所有图像的点
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)
        
        # 亚像素精确化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
        imgpoints[-1] = corners2

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

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

投影几何：从3D到2D的桥梁

投影几何，说白了就是研究三维空间中的点怎么映射到二维图像上。这里面有几个关键概念：

• 齐次坐标：用n+1维向量表示n维点。好处是可以用矩阵乘法统一处理平移和旋转。
• 单应性矩阵：描述两个平面之间的投影变换。在SLAM里，常用于纯旋转或平面场景的特征匹配。
• 本质矩阵与基础矩阵：描述两帧图像之间的对极几何约束。是特征匹配和位姿估计的核心。

我个人觉得，理解投影几何的关键在于坐标系变换。世界坐标系→相机坐标系→图像坐标系→像素坐标系，每一步都是一个矩阵乘法。你只要把这个链条理清楚，后面的所有算法都顺了。

好了，这一章的内容就到这里。相机模型和投影几何是SLAM的基石，理解透了，后面的特征提取和匹配才能做得扎实。下一章我们会深入特征提取的具体方法，到时候见。