2. 相机成像模型与坐标系：针孔相机模型、世界坐标系、相机坐标系、图像坐标系、像素坐标系
做运动对焦追踪，说白了就是让相机知道「目标在哪」以及「镜头该往哪转」。这背后绕不开一个基础问题——坐标系怎么统一。
我刚开始接触这个领域时，觉得坐标系转换就是一堆矩阵乘法，没啥意思。直到有一次在无人机项目上，目标明明在画面中心，云台却朝反方向转……查了两天才发现是坐标系定义反了。嗯，从那以后我再也不敢小看这几个坐标系了。
2.1 针孔相机模型——成像的物理基础
先聊聊相机是怎么看到东西的。你想想看，最简单的模型就是针孔相机——一个小孔，一个暗箱，外面景物倒着投在底片上。
数学上，针孔模型把三维空间点 \(P(X, Y, Z)\) 映射到成像平面上的点 \(p(x, y)\)。公式很简单：
x = f * (X / Z)
y = f * (Y / Z)其中 \(f\) 是焦距。注意，这里有个关键假设：光线直线传播，没有畸变。但实际镜头都有畸变，我们后面会讲怎么校正。
2.2 四个坐标系——从世界到像素的「翻译链」
一个三维点要变成屏幕上的像素，需要经过四次坐标变换。我画了张图帮你理清关系：
2.3 各坐标系详解
2.3.1 世界坐标系 (World Coordinate System)
这是你定义场景的「参考系」。比如在无人机追踪中，我习惯把起飞点设为原点，正北为X轴，正东为Y轴，高度为Z轴。所有目标位置、相机位姿都用这个坐标系描述。
2.3.2 相机坐标系 (Camera Coordinate System)
原点在相机光心，Z轴沿光轴指向场景前方，X轴向右，Y轴向下（符合图像行扫描方向）。
世界坐标系到相机坐标系的变换，就是外参矩阵：
[Xc] [R11 R12 R13 Tx] [Xw]
[Yc] = [R21 R22 R23 Ty] [Yw]
[Zc] [R31 R32 R33 Tz] [Zw]
[1 ] [0 0 0 1 ] [1 ]2.3.3 图像坐标系 (Image Coordinate System)
这是成像平面上的物理坐标系，单位是毫米。原点在光轴与成像面的交点（主点），x轴向右，y轴向下。
从相机坐标系到图像坐标系，就是针孔投影：
x = f * Xc / Zc
y = f * Yc / Zc2.3.4 像素坐标系 (Pixel Coordinate System)
这是你最终看到的图像坐标，单位是像素。原点在图像左上角，u轴向右，v轴向下。
从图像坐标到像素坐标，需要知道每个像素的物理尺寸：
u = x / dx + u0
v = y / dy + v0其中 \(dx, dy\) 是单个像素的宽和高（mm/pixel），\((u0, v0)\) 是主点像素坐标。
2.4 内参矩阵与外参矩阵——一张表说清楚
| 参数类型 | 包含内容 | 物理含义 | 标定方式 |
|---|---|---|---|
| 内参矩阵 K | fx, fy, u0, v0, s | 焦距、主点、倾斜因子 | 棋盘格标定（张正友法） |
| 外参矩阵 [R|t] | 旋转矩阵 R，平移向量 t | 相机在世界坐标系中的位姿 | PnP 求解（已知3D-2D点对） |
| 畸变参数 | k1, k2, k3, p1, p2 | 径向畸变、切向畸变 | 与内参同时标定 |
核心公式（齐次坐标形式）：
s * [u, v, 1]^T = K * [R|t] * [Xw, Yw, Zw, 1]^T这个公式贯穿整个运动对焦追踪。你只要知道目标的世界坐标，就能算出它在图像上的像素位置；反过来，知道像素位置和深度，也能反推世界坐标。
2.5 畸变校正——让针孔模型更贴近现实
实际镜头不是完美针孔，会有畸变。我遇到过最坑的情况：用广角镜头做追踪，目标在画面边缘时，像素坐标偏差了十几个像素，导致对焦环一直来回抽动。
畸变主要分两种：
- 径向畸变：光线通过透镜边缘时弯曲更大，表现为「桶形」或「枕形」失真。用 \(k1, k2, k3\) 描述。
- 切向畸变：透镜与成像面不平行，用 \(p1, p2\) 描述。
校正公式（先归一化，再畸变，最后投影）：
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*y2.6 实战：从像素坐标反推世界坐标
做追踪时，你经常需要反过来——知道目标在图像上的像素位置，想知道它在世界空间中的方位。这需要知道深度信息。
步骤很简单：
- 像素坐标 \((u, v)\) 转图像坐标：\(x = (u - u0) * dx, y = (v - v0) * dy\)
- 图像坐标转相机坐标（需要深度 Zc）：\(Xc = x * Zc / f, Yc = y * Zc / f\)
- 相机坐标转世界坐标：\([Xw, Yw, Zw, 1]^T = [R|t]^{-1} * [Xc, Yc, Zc, 1]^T\)
深度 Zc 怎么来？可以用激光测距、双目视觉、或者单目深度估计。我个人在无人机项目上习惯用激光测距仪——稳定、延迟低，对焦追踪最怕的就是深度跳变。