3、坐标系与变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系，刚体变换与投影变换
做三维重建这些年，我最大的感触就是——坐标系这东西，看着简单，但坑最多。你想想看，一个点从真实世界到屏幕像素，中间要经历四次坐标变换。哪一步出问题，重建结果就全歪了。
今天咱们就把这四层坐标系和它们之间的变换关系彻底捋清楚。我保证，看完这一章，你以后再也不会被坐标系搞晕。
3.1 四个坐标系，一个故事
先给个全景图。三维重建里，一个3D点要变成图像上的像素，得走这么一条路：
世界坐标系 → 相机坐标系 → 图像坐标系 → 像素坐标系
每一步都是一次变换。刚体变换负责前两步，投影变换负责后两步。
下面我用SVG画了张图，把整个流程串起来。你盯着看一分钟，后面就好理解了。
嗯，这张图就是整个章节的骨架。下面我们一层层拆开讲。
3.2 世界坐标系 → 相机坐标系：刚体变换
世界坐标系是我们定义真实物体位置的参考系。你可以把它放在任何地方——比如房间的角落、桌面的中心，或者GPS的经纬度原点。我个人习惯把世界坐标系放在场景的中心位置，这样后续计算比较对称。
相机坐标系呢？它的原点在相机的光心（也就是镜头中心），Z轴指向相机正前方，X轴向右，Y轴向下（跟图像的行列方向对齐）。
从一个坐标系到另一个坐标系，只需要旋转加平移——这就是刚体变换。说白了，就是你把世界坐标系"搬"到相机的位置，再"转"到相机的朝向。
刚体变换公式：
Xc = R * Xw + t其中：
- Xc：相机坐标系下的3D点 (3×1)
- Xw：世界坐标系下的3D点 (3×1)
- R：旋转矩阵 (3×3)，正交矩阵，满足 RᵀR = I
- t：平移向量 (3×1)
这里有个细节要注意。旋转矩阵R有9个参数，但自由度只有3个（绕X、Y、Z轴的旋转角）。所以实际存储时，我们常用旋转向量或四元数来节省空间。我在项目中遇到过用欧拉角表示旋转，结果在万向锁附近炸了的情况——嗯，从那以后我基本只用四元数做插值。
刚体变换还有一个更紧凑的写法，用齐次坐标：
| Xc | | R t | | Xw |
| Yc | = | 0 1 | | Yw |
| Zc | | | | Zw |
| 1 | | | | 1 |这个4×4矩阵 [R | t; 0 1] 就是相机的外参矩阵。它描述了相机在世界坐标系中的位置和朝向。
我的经验：外参标定是三维重建里最容易出错的环节。我曾经在一个项目里，因为标定板的坐标系定义跟算法不一致，导致重建出来的模型整体歪了15度。排查了整整两天才发现——就是R矩阵里一个符号的问题。
3.3 相机坐标系 → 图像坐标系：投影变换
这一步是三维到二维的关键一跳。相机坐标系下的3D点 (Xc, Yc, Zc)，通过小孔成像模型，投影到成像平面上。
你想想看，小孔成像的原理其实很简单：光线穿过光心，在成像平面上形成一个倒立的像。数学上就是相似三角形：
x = f * Xc / Zc
y = f * Yc / Zc其中 f 是焦距（物理单位，比如毫米），(x, y) 是图像坐标系下的坐标，原点在光轴与成像平面的交点（也就是主点）。
写成矩阵形式：
| x | | f 0 0 | | Xc |
| y | = | 0 f 0 | | Yc |
| 1 | | 0 0 1 | | Zc |注意，这里得到的 (x, y) 是物理单位（毫米），还不是像素。而且这个变换丢失了深度信息 Zc——这就是为什么单张图像无法直接恢复3D深度，需要多视角或者深度估计。
避坑提醒：我曾经在写代码时，忘了把Zc除到右边，直接用了 x = f * Xc。结果所有投影点都飞到了图像外面。这个错误太低级了，但新手特别容易犯。记住：投影变换一定有除法！
3.4 图像坐标系 → 像素坐标系：离散化
图像坐标系是连续的物理单位（毫米），但像素坐标系是离散的整数坐标。这一步就是把连续量变成离散量。
假设每个像素的物理尺寸是 dx 毫米（宽）和 dy 毫米（高），主点在像素坐标系下的坐标为 (u0, v0)，那么：
u = x / dx + u0
v = y / dy + v0写成矩阵形式：
| u | | 1/dx 0 u0 | | x |
| v | = | 0 1/dy v0 | | y |
| 1 | | 0 0 1 | | 1 |把前面两步合并，就得到了完整的相机内参矩阵 K：
| fx 0 u0 |
K = | 0 fy v0 |
| 0 0 1 |其中 fx = f/dx，fy = f/dy，分别是x和y方向的等效焦距（单位：像素）。
完整投影公式（齐次坐标形式）：
| u | | fx 0 u0 0 | | R t | | Xw |
| v | = | 0 fy v0 0 | | 0 1 | | Yw |
| 1 | | 0 0 1 0 | | | | Zw |
| | | 1 |简写为：s * p = K * [R | t] * Pw
这个公式，就是三维重建里最核心的相机模型。你以后看到的几乎所有算法——SFM、SLAM、立体匹配——都绕不开它。
3.5 四种畸变模型
理想的小孔成像模型是线性的，但真实相机有畸变。我遇到过最头疼的情况，就是畸变参数没标好，导致重建出来的平面变成了曲面。
常见的畸变有两种：
| 畸变类型 | 数学模型 | 典型表现 |
|---|---|---|
| 径向畸变 | x_dist = x * (1 + k1*r² + k2*r⁴ + k3*r⁶) | 图像边缘弯曲（桶形/枕形） |
| 切向畸变 | x_dist = x + [2*p1*x*y + p2*(r²+2x²)] | 镜头与成像平面不平行 |
畸变参数 (k1, k2, k3, p1, p2) 通常通过标定得到。OpenCV的calibrateCamera函数可以直接输出这些参数。
我的建议：如果你用的是手机或消费级相机，径向畸变的前两项 k1, k2 基本就够了。k3 一般用于鱼眼镜头。切向畸变 p1, p2 通常很小，但不要忽略——我曾经在一个高精度重建项目里，忽略了切向畸变，结果边缘区域的误差达到了3个像素。
3.6 实践：从世界坐标到像素坐标的完整代码
下面我给出一段Python代码，把整个流程串起来。你可以直接复制运行，看看一个3D点是怎么变成像素的。
import numpy as np
def world_to_pixel(Pw, R, t, K, dist_coeffs=None):
"""
将世界坐标系下的3D点投影到像素坐标系
参数:
Pw: 世界坐标点 (3,) 或 (N, 3)
R: 旋转矩阵 (3, 3)
t: 平移向量 (3,)
K: 内参矩阵 (3, 3)
dist_coeffs: 畸变系数 [k1, k2, p1, p2, k3] 或 None
返回:
uv: 像素坐标 (2,) 或 (N, 2)
"""
# 1. 刚体变换：世界 → 相机
Pc = R @ Pw + t
# 2. 投影变换：相机 → 图像（归一化平面）
x = Pc[0] / Pc[2]
y = Pc[1] / Pc[2]
# 3. 畸变校正（如果有）
if dist_coeffs is not None:
k1, k2, p1, p2, k3 = dist_coeffs
r2 = x*x + y*y
# 径向畸变
x_dist = x * (1 + k1*r2 + k2*r2*r2 + k3*r2*r2*r2)
y_dist = y * (1 + k1*r2 + k2*r2*r2 + k3*r2*r2*r2)
# 切向畸变
x_dist += 2*p1*x*y + p2*(r2 + 2*x*x)
y_dist += p1*(r2 + 2*y*y) + 2*p2*x*y
x, y = x_dist, y_dist
# 4. 离散化：图像 → 像素
u = K[0, 0] * x + K[0, 2]
v = K[1, 1] * y + K[1, 2]
return np.array([u, v])
# 示例：一个简单的相机
K = np.array([[800, 0, 320],
[0, 800, 240],
[0, 0, 1]])
R = np.eye(3) # 相机朝向与世界坐标系一致
t = np.array([0, 0, 5]) # 相机在Z轴正方向5米处
# 世界坐标系下的一个点
Pw = np.array([1, 2, 10]) # 单位：米
uv = world_to_pixel(Pw, R, t, K)
print(f"世界点 {Pw} 投影到像素坐标: ({uv[0]:.1f}, {uv[1]:.1f})")
# 输出: 世界点 [1 2 10] 投影到像素坐标: (400.0, 400.0)这段代码我用了很多次，基本是三维重建项目的标配工具函数。你可以在它的基础上加批量处理、加畸变、加不同相机模型。
3.7 总结一下
这一章的核心就三句话：
- 刚体变换（外参）解决"相机在哪、朝哪看"的问题
- 投影变换（内参）解决"3D点怎么落到成像平面"的问题
- 畸变模型解决"真实相机不完美"的问题
你只要把这三个东西搞清楚了，整个相机几何模型就通了。后面讲SFM、立体匹配、多视图几何，全都是在这个基础上搭积木。
嗯，坐标系这块就到这里。记住那个公式 s·p = K·[R|t]·Pw，它是三维重建的"hello world"。
公众号:蓝海资料掘金营，微信deep3321