坐标系与刚体变换：从世界到像素的“翻译官”
做位姿估计，说白了就是搞清楚“相机在哪儿”和“相机看到了什么”。这两个问题，都绕不开坐标系和刚体变换。我个人习惯把坐标系理解成“参考系”——你站在不同参考系里看同一个点，坐标值是不一样的。而刚体变换，就是把这些不同参考系下的坐标，互相“翻译”过去。
嗯，咱们先把这个“翻译”链条理清楚。
四个坐标系，一条流水线
一个三维空间点，最终变成图像上的一个像素，中间经历了四次坐标变换。我画了张图，帮你直观理解这条流水线：
你看，从世界到像素，每一步都有明确的数学关系。咱们一个一个拆开讲。
世界坐标系 → 相机坐标系：刚体变换
世界坐标系是“上帝视角”，相机坐标系是“相机视角”。从一个视角切换到另一个视角，需要旋转和平移。这就是刚体变换的核心。
核心公式：
Pc = R * Pw + t其中：Pw 是世界坐标，Pc 是相机坐标，R 是旋转矩阵，t 是平移向量。
旋转矩阵 R
旋转矩阵是个 3×3 的正交矩阵，行列式为 +1。它描述了三个坐标轴的旋转关系。我刚开始学的时候，总觉得这东西很抽象。后来在项目里做手眼标定，天天跟 R 打交道，才真正理解——R 的每一列，其实就是相机坐标系三个轴在世界坐标系下的方向向量。
你想想看，知道了三个轴的方向，再加上原点位置（平移向量），相机坐标系不就完全确定了吗？
我的小技巧： 验证旋转矩阵是否正确，可以检查 R * R^T 是否等于单位阵。如果偏差超过 1e-6，说明你的 R 有问题。我曾经在标定程序里遇到过 R 不满足正交性的情况，结果后续的三角化全乱了。
平移向量 t
平移向量 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。而坐标系和刚体变换，就是描述这个“已知”和“反求”之间关系的数学语言。
嗯，这一章的内容就到这里。坐标系和刚体变换是位姿估计的基石，理解透了，后面的内容才能走得稳。我当年就是在这上面吃了不少亏，希望你一次就搞明白。