三维空间刚体运动基础（上）：坐标系定义与旋转矩阵

各位同学，欢迎来到第二讲。今天我们要聊的，是整个SLAM和视觉位姿估计的基石——坐标系和旋转矩阵。

说实话，我见过不少新手，代码跑得飞起，但一问到“世界坐标系原点在哪？相机坐标系怎么转到像素坐标？”就卡壳了。嗯，这其实很正常。我自己刚入行那会儿，也在这上面栽过跟头。有一次做AR项目，虚拟物体死活对不准真实场景，折腾了两天，最后发现是坐标系定义反了。从那以后，我养成了一个习惯：写代码前，先把坐标系画清楚。

好，我们正式开始。

1. 四个坐标系，一个都不能少

在三维视觉里，我们打交道的主要有四个坐标系。它们就像一条流水线，把现实世界里的一个点，最终变成你屏幕上的一个像素。

坐标系	英文	维度	单位	一句话描述
世界坐标系	World Coordinate System	3D	米 (m)	你定义的“宇宙原点”，场景中所有物体的位置
相机坐标系	Camera Coordinate System	3D	米 (m)	以相机光心为原点，Z轴指向镜头前方
图像坐标系	Image Coordinate System	2D	毫米 (mm)	物理成像平面上的坐标，原点在光轴与平面交点
像素坐标系	Pixel Coordinate System	2D	像素 (pixel)	你最终看到的图像，原点在左上角

你看，从世界到像素，经历了“刚体变换（外参）→ 投影（内参）→ 离散化（像素）”。每一步都有明确的数学对应。

2. 旋转矩阵：从定义到推导

旋转矩阵，说白了，就是描述一个坐标系怎么转才能和另一个坐标系对齐。它是一个3x3的正交矩阵，行列式为+1。

为什么会是正交矩阵？你想想看，刚体旋转不改变物体的形状和大小，只改变朝向。这意味着旋转前后，向量的长度不变，向量间的夹角也不变。用数学语言说，就是内积保持不变。

假设我们有一个向量 v，旋转后变成 v' = R v。那么：

v'^T v' = (R v)^T (R v) = v^T (R^T R) v = v^T v

要让这个等式对任意 v 都成立，必须有 R^T R = I。这就是正交矩阵的定义。再加上右手系规则，行列式必须为+1。

我个人习惯把旋转矩阵理解成“基向量的变换”。你看，一个坐标系的三根轴，在另一个坐标系里怎么表示，把它们并排一放，就是旋转矩阵。

3. 绕单轴的旋转矩阵

最基础的旋转，是绕X、Y、Z轴转一个角度θ。这些矩阵是构建复杂旋转的积木。

绕Z轴旋转θ：

R_z(θ) = 
[ cosθ  -sinθ   0 ]
[ sinθ   cosθ   0 ]
[   0      0    1 ]

绕X轴旋转θ：

R_x(θ) = 
[   1      0      0 ]
[   0    cosθ  -sinθ ]
[   0    sinθ   cosθ ]

绕Y轴旋转θ：

R_y(θ) = 
[ cosθ    0    sinθ ]
[   0     1      0  ]
[-sinθ    0    cosθ ]

注意绕Y轴旋转时，负号的位置和绕X、Z轴不一样。这是由右手定则决定的。我曾经在写一个无人机姿态解算的代码时，把绕Y轴的负号放反了，结果飞控输出的姿态一直在震荡。排查了半天，才发现是这里的问题。

4. 旋转矩阵的性质

旋转矩阵有一些非常重要的性质，在后续的位姿优化中会反复用到。

• 正交性： R^T R = I，且 R^T = R^{-1}。这意味着求逆就是求转置，计算量小很多。
• 行列式为+1： 保证了是刚体旋转，而不是反射（反射的行列式为-1）。
• 封闭性： 两个旋转矩阵相乘，结果还是旋转矩阵。这对应了旋转的复合。
• 不可交换： R1 * R2 ≠ R2 * R1。旋转的顺序很重要，这一点在后续的欧拉角中会特别强调。

嗯，这里要注意。虽然旋转矩阵有9个元素，但它只有3个自由度（因为正交和行列式约束）。所以直接用9个参数去优化，效率很低。这也是为什么后面我们要引入李群李代数、四元数等更紧凑的表达方式。

5. 知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张图。你可以看到，从世界坐标系到像素坐标系，每一步都有明确的数学工具。

这张图把整个流程串起来了。你从世界坐标系出发，经过外参矩阵（R和t）变换到相机坐标系，再经过内参矩阵投影到图像坐标系，最后通过像素尺寸离散化到像素坐标系。旋转矩阵，就是外参矩阵里那个最核心的部分。