📚 课程目录第3章坐标系与位姿表示

3. 坐标系与位姿表示

在PnP视觉定位中，我们真正要回答的问题并不是“像素点在哪里”，而是“这个像素点对应的三维空间点，相对于相机或世界处在什么位置、什么朝向”。因此，坐标系与位姿表示是整个位姿估计链路的地基。若坐标系定义混乱，后续的旋转矩阵、欧拉角、四元数乃至优化求解都会失去意义。本节从四个坐标系出发，逐步过渡到旋转的多种表示，并初步引入李群李代数的思想。

一、四个坐标系及其变换链

PnP问题通常涉及四个坐标系：世界坐标系（World）、相机坐标系（Camera）、图像物理坐标系（Image plane）和像素坐标系（Pixel）。世界坐标系是场景的绝对参考，相机坐标系以光心为原点、Z轴指向光轴前方，图像物理坐标系位于成像平面，单位为毫米，像素坐标系则以像素为单位、原点通常在图像左上角。

从世界点到像素点的完整投影链为：世界点先经外参 [R|t] 变换到相机坐标系，再经内参 K 投影到像素坐标系。其数学表达为：

p_pixel = K · (R · P_world + t)
其中：
K = [[fx, 0, cx],
     [0, fy, cy],
     [0,  0,  1]]
R 为 3×3 旋转矩阵，t 为 3×1 平移向量

这里的外参 R, t 正是PnP要求解的相机位姿。理解这条变换链，才能明白为什么位姿表示方式的选择会直接影响求解的稳定性与效率。

二、旋转矩阵、欧拉角与四元数的取舍

旋转矩阵是最基础的表示，它直接作用于三维向量，复合旋转即矩阵相乘，形式统一且无奇异。但旋转矩阵有9个元素，实际自由度只有3，优化时需保持正交约束，直接迭代容易漂移。欧拉角用三个角描述姿态，物理意义直观，适合人机交互与显示，但存在万向锁：当中间角为±90°时，两个旋转轴重合，丢失一个自由度。四元数用四个数表示旋转，无万向锁，单位四元数可高效插值，广泛用于SLAM与视觉里程计，但存在双覆盖（q与-q表示同一旋转）和归一化约束。

表示方式	参数个数	自由度	优点	缺点	典型用途
旋转矩阵 R	9	3	无奇异、复合直接	冗余、需正交约束	变换计算、外参存储
欧拉角	3	3	直观、易读	万向锁、依赖旋转顺序	显示、调试、人机交互
四元数 q	4	3	无万向锁、可插值	双覆盖、需归一化	SLAM、姿态融合
李代数 ξ	3 或 6	3 或 6	无约束、适合优化	需指数/对数映射	BA、PnP 迭代求解

三、李群与李代数初步：为什么优化偏爱李代数

旋转矩阵构成特殊正交群 SO(3)，刚体变换构成特殊欧氏群 SE(3)，它们都是李群——既是群又是光滑流形。李群上直接做加法会破坏约束，而李代数 so(3)、se(3) 是李群在单位元处的切空间，是向量空间，可以自由加减。通过指数映射 exp 和对数映射 log，可以在李代数与李群之间往返：

SO(3):  R = exp(φ^)      φ ∈ so(3) ≅ R³
SE(3):  T = exp(ξ^)      ξ ∈ se(3) ≅ R⁶
其中 φ^ 表示将三维向量转为反对称矩阵
ξ = [ρ; φ]，ρ 为平移部分，φ 为旋转部分

在PnP或光束法平差中，我们通常把位姿增量 Δξ 定义在李代数上，用 T ← exp(Δξ^) · T 更新，这样每次迭代都是无约束的最小二乘，既保持旋转矩阵正交性，又避免参数冗余。这就是李代数在视觉定位中不可替代的原因。

工程提示：在代码中存储外参时，建议用四元数或旋转向量存储旋转，用向量存储平移；在优化时统一转成李代数增量。OpenCV 的 Rodrigues 可在旋转向量与旋转矩阵之间转换，cv::solvePnP 输出的 rvec 正是 so(3) 上的旋转向量。

四、案例：一个世界点如何走到像素

假设世界点 P_world = [0.2, -0.1, 1.5]ᵀ，相机外参旋转向量 rvec 对应 R，平移 t = [0.05, 0.02, 0.3]ᵀ，内参 fx=fy=800, cx=640, cy=360。计算流程为：

P_cam = R · P_world + t
u = fx * (Xc / Zc) + cx
v = fy * (Yc / Zc) + cy

若把 R 误写成欧拉角顺序不一致，或把世界坐标系与相机坐标系搞反，最终像素误差可能达到几十甚至上百像素。实际工程中，建议先用已知标定板验证投影链，再进入PnP求解。

常见误区：

欧拉角旋转顺序未统一：不同库默认顺序不同（ZYX、XYZ 等），混用会导致姿态完全错误。
四元数未归一化：长时间迭代后模长漂移，旋转矩阵不再正交。
把李代数增量直接加到旋转矩阵上：破坏 SO(3) 约束，必须用指数映射更新。
坐标系定义不写清楚：世界系是 Y 向上还是 Z 向上，相机系是 Z 向前还是 X 向前，必须团队统一。

五、复盘与工程建议

本节的核心可以概括为一条链：世界点 → 外参 → 相机点 → 内参 → 像素点，以及一套表示：旋转矩阵、欧拉角、四元数、李代数。旋转矩阵适合计算与存储，欧拉角适合显示与调试，四元数适合插值与姿态融合，李代数适合无约束优化。PnP求解的本质，就是在已知世界点与像素点对应关系的前提下，寻找最优的 R, t，使重投影误差最小。理解这些表示之间的转换关系，是后续学习 solvePnP、RANSAC、BA 优化的前提。

建议在工程中建立统一的位姿工具类，提供 quat2rot、rot2quat、euler2rot、rot2euler、se3_exp、se3_log 等函数，并在单元测试中验证：任意旋转矩阵经四元数往返后误差小于 1e-6，任意李代数经 exp/log 往返后误差小于 1e-6。只有表示层可靠，PnP 的位姿估计才可靠。