📚 课程目录第2章坐标系与刚体变换

2. 坐标系与刚体变换

做位姿估计，说白了就是搞清楚“相机在哪儿”和“相机看到了什么”。这两个问题，都绕不开坐标系和刚体变换。我个人习惯把坐标系理解成“参考系”——你站在不同参考系里看同一个点，坐标值是不一样的。而刚体变换，就是把这些不同参考系下的坐标，互相“翻译”过去。

嗯，咱们先把这个“翻译”链条理清楚。

一个三维空间点，最终变成图像上的一个像素，中间经历了四次坐标变换。我画了张图，帮你直观理解这条流水线：

你看，从世界到像素，每一步都有明确的数学关系。咱们一个一个拆开讲。

世界坐标系是“上帝视角”，相机坐标系是“相机视角”。从一个视角切换到另一个视角，需要旋转和平移。这就是刚体变换的核心。

核心公式：

Pc = R * Pw + t

其中：Pw 是世界坐标，Pc 是相机坐标，R 是旋转矩阵，t 是平移向量。

旋转矩阵是个 3×3 的正交矩阵，行列式为 +1。它描述了三个坐标轴的旋转关系。我刚开始学的时候，总觉得这东西很抽象。后来在项目里做手眼标定，天天跟 R 打交道，才真正理解——R 的每一列，其实就是相机坐标系三个轴在世界坐标系下的方向向量。

你想想看，知道了三个轴的方向，再加上原点位置（平移向量），相机坐标系不就完全确定了吗？

我的小技巧： 验证旋转矩阵是否正确，可以检查 R * R^T 是否等于单位阵。如果偏差超过 1e-6，说明你的 R 有问题。我曾经在标定程序里遇到过 R 不满足正交性的情况，结果后续的三角化全乱了。

平移向量 t 表示相机坐标系原点在世界坐标系中的位置。注意方向——是从世界坐标系原点指向相机坐标系原点。

这里有个容易搞混的地方：t 不是相机在世界坐标系下的坐标，而是世界坐标系原点在相机坐标系下的坐标的相反数。嗯，绕了点，但很重要。

避坑指南： 我曾经在写手眼标定代码时，把 R 和 t 的对应关系搞反了。结果标定出来的外参，投影误差直接爆表。后来花了整整一天排查，才发现是平移向量的符号问题。记住：Pc = R * Pw + t，这个 t 是“世界原点在相机坐标系下的坐标”的相反数。

刚体变换 Pc = R * Pw + t 是非线性的（有加法）。如果我们想连续做多次变换，公式会变得很啰嗦。齐次坐标就是来解决这个问题的。

说白了，就是给三维坐标加一维，变成四维：

Pw_homogeneous = [Xw, Yw, Zw, 1]^T
Pc_homogeneous = [Xc, Yc, Zc, 1]^T

然后刚体变换就变成了一个矩阵乘法：

Pc_homogeneous = [R  t;  0  1] * Pw_homogeneous

这个 4×4 矩阵，就是传说中的外参矩阵。它把旋转和平移统一到了一个矩阵里，连续变换只需要不断左乘矩阵就行。

表示方式	维度	变换形式	优点
欧氏坐标	3×1	Pc = R * Pw + t	直观，容易理解
齐次坐标	4×1	Pc = T * Pw	统一为矩阵乘法，便于级联

为什么用齐次坐标？ 因为在实际SLAM系统中，我们经常需要把多个变换串联起来。比如：机器人移动了，相机跟着动，然后还要投影到图像上。用齐次坐标，所有变换都是矩阵乘法，代码写起来干净利落，而且GPU对矩阵乘法有硬件加速。

这一步把三维点投影到二维成像平面。用的是针孔相机模型：

x = f * Xc / Zc
y = f * Yc / Zc

其中 f 是焦距，(x, y) 是成像平面上的物理坐标（单位：毫米）。

注意这里有个尺度不确定性：一个点在相机坐标系下放大缩小，投影到图像上的位置不变。这就是为什么单张图像无法恢复深度——你丢失了 Zc 的信息。

成像平面上的物理坐标 (x, y) 还得转成像素坐标 (u, v)。这一步由相机内参决定：

u = fx * x + cx
v = fy * y + cy

写成矩阵形式：

[u]   [fx  0  cx] [x]
[v] = [ 0  fy  cy] [y]
[1]   [ 0   0   1] [1]

fx, fy 是焦距的像素表示，cx, cy 是主点（光轴与成像平面的交点）的像素坐标。这个 3×3 矩阵就是内参矩阵 K。

经验之谈： 内参矩阵是相机的固有属性，但会随温度、机械振动等因素轻微变化。我建议每半年重新标定一次内参，尤其是用在精度要求高的场景（比如AR/VR）。

把上面几步串起来，从世界坐标到像素坐标的完整公式就是：

s * [u, v, 1]^T = K * [R | t] * [Xw, Yw, Zw, 1]^T

其中 s 是尺度因子（其实就是 Zc）。这个公式，就是位姿估计的核心方程。你做的所有工作——特征匹配、PnP求解、BA优化——本质上都是在解这个方程里的 R 和 t。

一句话总结： 位姿估计，就是已知 K 和匹配点对，反求 R 和 t。而坐标系和刚体变换，就是描述这个“已知”和“反求”之间关系的数学语言。

嗯，这一章的内容就到这里。坐标系和刚体变换是位姿估计的基石，理解透了，后面的内容才能走得稳。我当年就是在这上面吃了不少亏，希望你一次就搞明白。