2. 坐标系与空间变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系的关系，刚体变换与齐次坐标
各位同学，咱们今天聊点硬核的——坐标系与空间变换。
说实话，我刚开始做机器人导航那会儿，最头疼的就是坐标系。明明一个物体就在那儿，怎么在不同坐标系下算出来的位置完全不一样？后来踩了无数坑才明白：坐标系没对齐，导航就是瞎指挥。
2.1 四个坐标系，一个都不能少
在3D视觉系统里，我们打交道最多的就是这四个坐标系。我习惯把它们想象成「四层翻译官」：
- 世界坐标系：真实世界的绝对坐标。比如你房间的墙角，就是原点。
- 相机坐标系：以相机光心为原点，Z轴指向镜头前方。
- 图像坐标系：在成像平面上，单位是毫米。原点在图像中心。
- 像素坐标系：就是图像里每个像素的行列号，原点在左上角。
你想想看，一个物体从真实世界到你电脑屏幕上的像素点，中间经历了多少次「翻译」？
核心流程：
世界坐标 → 相机坐标 → 图像坐标 → 像素坐标
每一步都是一个变换矩阵在背后默默工作。
2.2 刚体变换：旋转+平移，就这么简单
刚体变换，说白了就是物体在空间里「挪个位置、转个方向」，但形状不变。我做过一个项目，机械臂抓取零件，每次都要把零件从世界坐标系变换到机械臂基座坐标系——那就是典型的刚体变换。
刚体变换由两部分组成：
- 旋转矩阵 R：3×3 的正交矩阵，描述朝向变化
- 平移向量 t：3×1 的向量，描述位置变化
公式很简单：P_cam = R * P_world + t
嗯，这里要注意：旋转矩阵必须是正交矩阵，行列式为+1。我曾经因为矩阵不正交，导致标定出来的结果全是歪的，排查了整整两天。
2.3 齐次坐标：为什么我们需要它？
你可能会问：为什么非要用齐次坐标？直接用三维坐标不行吗？
我刚开始也这么想。直到我写代码时发现：旋转是矩阵乘法，平移是向量加法，两个操作混在一起写起来特别别扭。而且连续变换时，公式会变得又臭又长。
齐次坐标的妙处在于：把旋转和平移统一成一个矩阵乘法。
做法很简单：给三维点加一个维度，变成四维：
// 三维点 (x, y, z) 变成齐次坐标 (x, y, z, 1)
// 变换矩阵变成 4×4 的形式：
| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0 0 0 1 |
// 变换就变成了：P_homo = T * P_homo_world
我的小技巧：写代码时，我习惯把所有变换都写成 4×4 齐次矩阵。这样无论是旋转、平移还是缩放，统一用矩阵乘法搞定，代码简洁又不容易出错。
2.4 坐标系变换的完整链路
咱们把四个坐标系串起来，看看完整的变换流程：
- 世界 → 相机：刚体变换，用外参矩阵（R, t）
- 相机 → 图像：透视投影，用内参矩阵（fx, fy, cx, cy）
- 图像 → 像素：缩放+平移，把毫米变成像素
写成公式就是：
// 像素坐标 = 内参矩阵 × 外参矩阵 × 世界坐标
// 注意：这里用的是齐次坐标
s * [u, v, 1]^T = K * [R | t] * [X, Y, Z, 1]^T
其中：
K = | fx 0 cx |
| 0 fy cy |
| 0 0 1 |
避坑指南：我曾经在项目里把 fx 和 fy 搞反了，结果投影出来的点全跑偏了。记住：fx 对应图像宽度方向，fy 对应高度方向。另外，s 是缩放因子，不是随便取的，它是从投影几何里推导出来的。
2.5 知识体系结构图
下面这张图是我自己画的，把四个坐标系和它们之间的变换关系梳理清楚了。你仔细看看，应该能一目了然：
2.6 实战中的坐标系管理
我在做机器人导航项目时，总结了一套坐标系管理的心得：
- 统一基准：所有传感器数据最终都变换到世界坐标系下处理
- 记录变换链：用树状结构管理坐标系关系，比如「相机→机械臂→基座→世界」
- 验证方法：找一个已知点，手动计算变换结果，和代码输出对比
我的习惯：每次写变换代码前，先在纸上画一遍坐标系关系图。别嫌麻烦，画清楚了再写代码，能省下80%的调试时间。
好了，坐标系与空间变换就讲到这里。记住一句话：坐标系是3D视觉的基石，齐次坐标是变换的利器。下一章咱们聊聊相机标定，到时候你会更深刻地理解今天讲的内容。