第2章相机模型与坐标系

相机模型与坐标系：从像素到三维的桥梁

做SLAM这么多年，我越来越觉得，理解相机模型和坐标系转换，是入门视觉SLAM的第一道坎。说白了，你看到的每一张图片，都是一堆像素点。但我们要做的，是从这些像素里，反推出真实世界的三维结构。这中间，就需要一套严谨的数学模型来搭桥。

今天我们就来聊聊这个桥是怎么建的。我会从针孔相机模型讲起，然后聊聊畸变、标定，最后把四个坐标系的转换捋清楚。嗯，这些内容看起来有点枯燥，但相信我，搞懂了它们，后面的回环检测和图优化，你才能玩得转。

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

先问个问题：相机是怎么把三维世界拍到二维照片上的？

最经典的模型就是针孔相机模型。你想象一个密闭的盒子，前面开个小孔，光线穿过小孔，在后面的感光元件上成像。这就是最原始的相机。

数学上怎么描述？很简单，看这个公式：

// 三维点 P(X, Y, Z) 投影到成像平面上的点 p(x, y)
x = f * X / Z
y = f * Y / Z

其中 f 是焦距，也就是小孔到成像平面的距离。Z 是物体到相机的深度。

这里有个细节要注意：成像其实是倒立的。不过我们通常会在数学上把成像平面对称到相机前方，这样计算起来更方便。我个人习惯用这种「虚拟成像平面」来思考，不容易搞混。

核心要点：针孔模型是线性模型，它假设光线是直线传播的。但实际镜头有畸变，所以后面还要加畸变模型。

畸变模型：现实总是不完美的

实际镜头不是针孔，而是透镜。透镜的曲率、装配误差，都会导致图像变形。这就是畸变。

畸变主要分两种：

径向畸变：光线在透镜边缘弯曲得更厉害，导致图像边缘的直线变弯。比如拍一张棋盘格，边缘的格子会变成桶形或枕形。
切向畸变：透镜和成像平面不平行，导致图像倾斜。这个在手机相机里比较常见。

数学上，我们用多项式来矫正畸变：

// 径向畸变矫正
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 是切向畸变参数。

避坑指南：我曾经在项目里只用了 k1, k2 两个参数，结果在广角镜头上矫正效果很差。后来才发现，对于大畸变的镜头，必须加上 k3。所以标定时，建议把 k3 也加上，哪怕它最后被标定为0，也比漏掉强。

相机标定：给相机做一次「体检」

标定的目的，就是求出相机的内参（焦距、主点、畸变参数）和外参（相机在世界坐标系中的位置和姿态）。

最常用的方法是张正友标定法。你只需要打印一张棋盘格，从不同角度拍几张照片，算法就能自动算出参数。

标定的步骤大致如下：

打印棋盘格，贴在平面上
用相机从不同角度拍摄棋盘格（至少10-20张）
检测每张图片中的角点
用最小二乘法求解内参和畸变参数
用非线性优化（比如Levenberg-Marquardt）进一步精化

OpenCV里封装好了标定函数，用起来很方便：

import cv2
import numpy as np

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

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

# 遍历图片，检测角点
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

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

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

小技巧：标定的时候，棋盘格要占满画面的不同区域。我习惯让棋盘格出现在画面的四个角和中心，这样标定出来的畸变参数更准确。另外，棋盘格不要离相机太远，否则角点检测会不准确。

四个坐标系的转换：从世界到像素

这是SLAM里最基础、也最容易搞混的部分。我们一共涉及四个坐标系：

世界坐标系：真实世界的三维坐标，单位是米。我们通常把第一帧相机的位置设为原点。
相机坐标系：以相机光心为原点，Z轴指向相机前方，X轴向右，Y轴向下。
图像坐标系：在成像平面上，以光轴与成像平面的交点为原点，单位是毫米。
像素坐标系：在图像上，以左上角为原点，单位是像素。

转换过程分三步：

第一步：世界坐标系 → 相机坐标系

这是一个刚体变换，包括旋转和平移：

P_cam = R * P_world + t

其中 R 是3x3旋转矩阵，t 是3x1平移向量。

第二步：相机坐标系 → 图像坐标系

这就是针孔模型：

x = f * X_cam / Z_cam
y = f * Y_cam / Z_cam

第三步：图像坐标系 → 像素坐标系

把物理单位（毫米）转换成像素单位：

u = x / dx + u0
v = y / dy + v0

其中 dx, dy 是每个像素的物理尺寸，u0, v0 是主点（光轴与成像平面的交点）的像素坐标。

把这三步合并，就是完整的投影公式：

// 齐次坐标形式
[ u ]   [ fx  0  u0  0 ] [ R  t ] [ X_w ]
[ v ] = [ 0  fy v0  0 ] [ 0  1 ] [ Y_w ]
[ 1 ]   [ 0   0   1  0 ]         [ Z_w ]
                                  [ 1   ]

其中 fx = f/dx, fy = f/dy，是焦距的像素表示。

关键理解：这个3x4的矩阵，就是相机的投影矩阵 P = K[R|t]。K是内参矩阵，[R|t]是外参矩阵。SLAM里，我们估计的就是这个矩阵。

知识体系总览

下面这张图，把本章的核心逻辑串起来了。你可以看到，从三维世界到二维像素，每一步都有对应的数学模型。而相机标定，就是把这些模型的参数求出来。

你看，整个流程其实很清晰。从世界坐标到像素坐标，每一步都有明确的数学关系。而相机标定，就是把这些关系中的未知参数求出来。

我个人建议，刚开始学的时候，不要急着去调SLAM算法。先把相机模型和坐标系转换搞透。你可以拿自己的手机拍几张棋盘格，用OpenCV标定一下，看看内参矩阵长什么样。亲手做一遍，比看十遍书都管用。

嗯，这一章的内容就到这里。记住，相机模型是SLAM的基石。后面的回环检测、图优化，都是在这个基础上搭建的。打好基础，后面才能走得更远。

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

📚 课程目录

相机模型与坐标系：从像素到三维的桥梁

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

畸变模型：现实总是不完美的

相机标定：给相机做一次「体检」

四个坐标系的转换：从世界到像素

知识体系总览