3、刚体变换与李群李代数：旋转矩阵、四元数、欧拉角、SE(3)与so(3)
各位同学，欢迎来到第三讲。
说实话，这一章是整个外参标定里最「劝退」的部分。很多工程师一看到旋转矩阵、四元数就头大。但我可以负责任地告诉你——搞不懂李群李代数，你连标定代码都看不懂。
我自己刚入行时，也在这上面栽过跟头。有一次做激光雷达和相机的联合标定，明明算法流程都对，结果跑出来的外参就是不对。折腾了两天，最后发现是欧拉角万向锁导致雅可比矩阵奇异。嗯，从那以后，我再也不敢只用欧拉角做优化了。
核心认知：刚体变换描述的是「一个坐标系下的点，如何映射到另一个坐标系」。而李群李代数，则是我们用来「优雅地优化」这个变换的数学工具。
3.1 旋转的三种表达方式
我们先从最基础的旋转说起。一个刚体在三维空间中的旋转，有三种主流表达方式。我个人的建议是：理解上靠欧拉角，计算上靠旋转矩阵，存储和插值上靠四元数。
3.1.1 旋转矩阵
旋转矩阵是一个 3x3 的正交矩阵，行列式为 +1。它属于特殊正交群 SO(3)。
说白了，旋转矩阵就是一组基向量在另一个坐标系下的投影。你想想看，一个坐标系绕 x 轴转 θ 角，对应的旋转矩阵长这样：
R_x(θ) = [1 0 0 ]
[0 cosθ -sinθ ]
[0 sinθ cosθ ]我在项目中遇到过一个问题：直接用旋转矩阵做优化时，9 个参数有 6 个约束（正交+行列式为1），优化器很容易跑飞。这就是为什么我们需要引入李代数。
3.1.2 欧拉角
欧拉角用三个角度表示旋转，直观但坑多。最常见的坑就是万向锁——当中间轴旋转到 ±90° 时，会丢失一个自由度。
避坑指南：我曾经在无人机姿态估计中，直接用欧拉角做 EKF 更新。结果飞机做大机动时，俯仰角接近 90°，航向角和滚转角突然耦合，姿态直接发散。后来全部换成四元数才解决。
欧拉角的另一个问题是：旋转顺序很重要。ZYX、ZYZ、XYZ 等不同顺序，对应的旋转矩阵完全不同。做标定时，一定要确认你的传感器手册用的是哪种顺序。
3.1.3 四元数
四元数是我个人最喜欢的旋转表达方式。它没有万向锁，插值平滑，而且只需要 4 个参数。
一个单位四元数 q = [w, x, y, z]ᵀ，满足 w² + x² + y² + z² = 1。它对应的旋转矩阵是：
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy) ]
[2(xy+wz) 1-2(x²+z²) 2(yz-wx) ]
[2(xz-wy) 2(yz+wx) 1-2(x²+y²) ]注意：四元数有「双覆盖」特性——q 和 -q 表示同一个旋转。这在优化时会导致歧义，需要小心处理。
| 表达方式 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直接、易组合 | 冗余约束、优化困难 |
| 欧拉角 | 3 | 直观、物理意义明确 | 万向锁、顺序依赖 |
| 四元数 | 4 | 无奇点、易插值 | 单位约束、双覆盖 |
3.2 李群与李代数
好，现在进入正题。为什么我们需要李群李代数？
原因很简单：旋转矩阵和变换矩阵所在的流形不是欧氏空间，你不能直接做加法。
举个例子：两个旋转矩阵 R₁ 和 R₂，它们的乘积 R₁R₂ 仍然是旋转矩阵。但 R₁ + R₂ 呢？大概率不是。这意味着你不能用常规的梯度下降法去优化旋转矩阵——你加一个步长，结果就跑出流形了。
3.2.1 SO(3) 与 so(3)
SO(3) 是特殊正交群，也就是所有旋转矩阵的集合。它的李代数 so(3) 对应的是三维旋转向量（或者说反对称矩阵）。
从 so(3) 到 SO(3) 的映射是指数映射：
R = exp(φ^) 其中 φ 是旋转向量，φ^ 是它的反对称矩阵反过来，从 SO(3) 到 so(3) 是对数映射：
φ = log(R)^∨我在项目中习惯用李代数做优化。为什么呢？因为李代数是一个三维向量，没有约束，可以直接用高斯牛顿法。优化完再映射回 SO(3) 就行。
实用技巧：在 Ceres Solver 或 g2o 中，你不需要手动实现指数映射。它们都内置了 SO(3) 的局部参数化。你只需要告诉优化器「我要优化的是旋转」，剩下的交给库。
3.2.2 SE(3) 与 se(3)
SE(3) 是特殊欧氏群，包含旋转和平移。一个 SE(3) 元素可以写成 4x4 的变换矩阵：
T = [R t]
[0 1]它的李代数 se(3) 是一个六维向量：前三维是平移，后三维是旋转（或者反过来，取决于你的定义）。
这里有个容易混淆的点：se(3) 的指数映射不是简单的「平移部分取指数，旋转部分取指数」。平移部分会受到旋转的影响，公式比较复杂。不过别担心，实际工程中我们很少手写这个映射。
3.3 核心知识体系
下面这张图是我自己总结的，希望能帮你理清思路：
3.4 实践中的选择
讲了这么多理论，最后说说实际工程中怎么选。
场景一：做标定优化
我建议用李代数。把旋转表示成 so(3) 的三维向量，平移就是三维向量，拼起来就是 se(3) 的六维向量。优化时没有任何约束，收敛快且稳定。
场景二：做传感器融合（如 EKF）
用四元数。EKF 的状态向量里放四元数，但误差状态用三维旋转向量。这是目前最主流的做法，IMU 预积分也是基于这个思路。
场景三：做可视化或手动调试
用欧拉角。但记得加一个保护逻辑——当俯仰角接近 ±90° 时，切换到四元数表示。
一句话总结：旋转矩阵是「是什么」，李代数是「怎么优化」。搞懂这两者的关系，你的标定代码就成功了一半。
好了，这一讲的内容就到这里。下一讲我们会把这些数学工具真正用到外参标定中，到时候你会感谢今天认真啃理论的自己。