坐标系与刚体运动：世界坐标系、相机坐标系、图像坐标系、欧拉角与四元数、旋转矩阵与变换矩阵

说实话，刚接触SLAM那会儿，我被这些坐标系绕得晕头转向。世界坐标、相机坐标、图像坐标……每个点在不同坐标系下跑来跑去，稍不留神就搞反了方向。后来我慢慢悟出一个道理：搞懂坐标系，就是搞懂“谁在看谁”。今天咱们就把这件事彻底理清楚。

1. 四个坐标系，一个故事

先讲个我自己的经历。有一次做AR项目，虚拟物体怎么都对不准真实场景。折腾了两天，最后发现是图像坐标系的原点搞错了——我默认左上角是(0,0)，但算法里用的是中心点。嗯，这种坑踩过一次就记住了。

咱们先看这四个坐标系的关系：

说白了，就是从“真实世界”到“照片上的像素点”的映射过程。

2. 世界坐标系与相机坐标系

世界坐标系就是咱们生活的三维空间。你站在哪里，原点就在哪里。我习惯把原点设在场景中心，Z轴朝上——这样计算起来比较直观。

相机坐标系则不同。它的原点在相机光心，Z轴指向镜头前方。你想想看，相机看到的“前方”和世界坐标系的“前方”往往不是一回事。

从世界坐标到相机坐标，需要做一次刚体变换：

P_cam = R * P_world + t

这里的R是旋转矩阵，t是平移向量。我建议你把这个公式刻在脑子里——后面所有坐标变换都离不开它。

3. 图像坐标系与像素坐标系

相机坐标系里的点，通过投影落到成像平面上，就变成了图像坐标系。这是个二维坐标系，单位是毫米，原点在光轴与成像平面的交点。

但咱们最终看到的是像素，所以还得转成像素坐标系。这里有个细节：

• 图像坐标系：原点在中心，单位mm
• 像素坐标系：原点在左上角，单位pixel

转换公式很简单：

u = x / dx + u0
v = y / dy + v0

dx和dy是每个像素的物理尺寸，u0、v0是主点坐标。我曾经在标定时把u0和v0搞反了，结果重建出来的模型歪了45度……

4. 旋转矩阵与变换矩阵

旋转矩阵R是个3x3的正交矩阵，行列式为1。它描述了坐标系之间的旋转关系。我个人的经验是：别试图手算旋转矩阵，用库函数或者四元数转换更靠谱。

变换矩阵T把旋转和平移合在一起：

T = [R  t]
    [0  1]

这个4x4矩阵在SLAM里随处可见。每次看到它，我就想起一句话：“一个矩阵，搞定刚体运动”。

5. 欧拉角与四元数

欧拉角直观，但有个致命问题——万向锁。我记得第一次遇到万向锁是在做无人机姿态控制时，飞机突然“卡住”了，怎么转都转不过去。后来查资料才知道，欧拉角在某些角度下会丢失一个自由度。

四元数就没有这个问题。它用四个数表示旋转：

q = w + xi + yj + zk

其中w是实部，x、y、z是虚部。四元数做插值也很平滑，适合做相机轨迹的平滑过渡。

我建议你记住这个转换关系：

表示方式	优点	缺点
旋转矩阵	计算稳定，适合组合变换	占用内存大，有冗余
欧拉角	直观，容易理解	万向锁，插值困难
四元数	无奇点，适合插值	不够直观

6. 知识体系总览

下面这张图帮你理清本章的核心逻辑：

7. 避坑指南

最后分享几个我踩过的坑：

• 坐标系方向：不同库的坐标系方向可能不同。OpenCV是右手系，但有些渲染引擎是左手系。转换前一定要确认。
• 四元数归一化：四元数必须归一化，否则旋转会变形。我见过有人忘了归一化，结果物体越转越大。
• 欧拉角顺序：前面说过，顺序很重要。我建议统一用ZYX，并在代码注释里写清楚。

好了，坐标系和刚体运动就聊到这儿。这些东西看着琐碎，但它们是SLAM的基石。你想想看，如果连“点在哪里”都搞不清楚，后面的场景重建根本无从谈起。把这些基础打牢，后面的路就好走了。