3、空间同步基础：刚体变换与欧拉角、四元数基础、李群与李代数（SO3, SE3）简介
好，我们进入空间同步这一章。说实话，这是整个传感器融合里最绕不开的一块硬骨头。你想想看，激光雷达和摄像头装在不同位置，它们看到的同一个物体，坐标却不一样。怎么对齐？靠的就是空间同步。
我个人习惯把空间同步拆成两个问题：怎么描述旋转，和怎么描述位姿。前者是姿态，后者是位置加姿态。今天我们就从最基础的刚体变换讲起，一路聊到李群李代数。别怕，我会用我踩过的坑帮你铺路。
3.1 刚体变换：旋转 + 平移
刚体变换，说白了就是物体在空间里“挪了个地方”，但形状大小不变。一个刚体变换由两部分组成：旋转矩阵 R 和 平移向量 t。
数学上写起来很简单：
p' = R * p + t
其中 p 是原始坐标，p' 是变换后的坐标。R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。
嗯，这里要注意：旋转矩阵 R 必须是正交矩阵，且行列式为 +1。为什么？因为旋转不能拉伸、不能镜像。我在项目中遇到过有人直接用随机数填 R，结果算出来的点云全飞了——那就是没满足正交性。
核心要点：刚体变换有 6 个自由度——3 个旋转自由度 + 3 个平移自由度。旋转矩阵虽然有 9 个元素，但受正交约束，实际自由度为 3。
3.2 欧拉角：直观但危险
欧拉角是最直观的旋转表示法。用三个角度描述绕坐标轴的旋转顺序。常见的如 ZYX 顺序：先绕 Z 轴转 yaw（偏航），再绕 Y 轴转 pitch（俯仰），最后绕 X 轴转 roll（横滚）。
举个例子，无人机姿态通常就用欧拉角表示：
yaw = 30°, pitch = 10°, roll = 5°。很直观对吧？
但是——欧拉角有个致命问题：万向锁（Gimbal Lock）。当 pitch 接近 ±90° 时，yaw 和 roll 的旋转轴会重合，丢失一个自由度。我曾经在调试一个云台时，pitch 打到 90°，结果 yaw 怎么转都没反应。当时我还以为是电机坏了，查了半天才发现是万向锁。
避坑指南：千万不要在插值或滤波时直接对欧拉角做线性运算。比如两个姿态的欧拉角取平均，结果很可能不对。我曾经这么干过，结果云台直接抽风。后来老老实实转成四元数再算。
3.3 四元数：旋转的瑞士军刀
四元数，说白了就是“带约束的复数在三维空间的推广”。一个四元数 q 写成：
q = w + xi + yj + zk
其中 w 是实部，x、y、z 是虚部。约束条件：w² + x² + y² + z² = 1。这就是单位四元数，用来表示旋转。
为什么用四元数？三个理由：
- 无万向锁——这是最大的优势
- 插值平滑——可以用球面线性插值（SLERP）
- 计算高效——比旋转矩阵少乘几次
四元数旋转一个向量 v 的公式：
v' = q * v * q⁻¹
注意这里的乘法是四元数乘法，v 要写成纯四元数 (0, vx, vy, vz)。
我的习惯：在代码里统一用四元数做内部运算，只在显示或人工调试时转成欧拉角。这样既避免了万向锁，又保留了直观性。推荐用 Eigen 或 ROS 的 tf2 库，它们都封装好了四元数运算。
3.4 李群与李代数：SO(3) 和 SE(3)
好，接下来是重头戏。李群和李代数，听起来很高大上，其实就是为了解决一个问题：怎么在旋转和位姿空间里做优化。
你想想看，如果我们想优化一个旋转矩阵 R，直接对 R 做加法行吗？不行。因为 R + ΔR 很可能不再是旋转矩阵（不满足正交性）。那怎么办？用李代数。
3.4.1 SO(3)：旋转矩阵的李群
SO(3) 是 Special Orthogonal Group 的缩写，就是所有 3x3 旋转矩阵的集合。它是一个李群，意味着它既是群又是光滑流形。
对应的李代数 so(3) 是三维向量空间，元素是 反对称矩阵。每个三维向量 φ 对应一个反对称矩阵 φ^：
φ = [φ1, φ2, φ3]ᵀ
φ^ = [[0, -φ3, φ2],
[φ3, 0, -φ1],
[-φ2, φ1, 0]]从李代数到李群的映射是指数映射：
R = exp(φ^)
反过来，从李群到李代数是对数映射：
φ^ = log(R)
这有什么用？优化时我们在李代数上做加法，再映射回李群。这样永远保证结果是合法的旋转矩阵。
3.4.2 SE(3)：位姿变换的李群
SE(3) 是 Special Euclidean Group，包含所有刚体变换 (R, t)。对应的李代数 se(3) 是六维向量：
ξ = [ρ, φ]ᵀ
其中 ρ 是平移部分，φ 是旋转部分。se(3) 的元素是 4x4 的矩阵：
ξ^ = [[φ^, ρ],
[0ᵀ, 0]]指数映射得到 SE(3) 的变换矩阵 T：
T = exp(ξ^)
实际应用：在激光雷达-摄像头标定中，我们通常用 SE(3) 表示外参。优化时在 se(3) 上迭代，每次迭代加一个小量 δξ，再映射回 SE(3)。这样既保证了约束，又方便求导。
3.5 代码示例：用 Eigen 实现基本变换
下面给一段我常用的代码片段，演示四元数、旋转矩阵、欧拉角之间的转换：
#include <Eigen/Dense>
#include <iostream>
int main() {
// 定义四元数：绕 Z 轴转 30 度
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitZ());
// 四元数转旋转矩阵
Eigen::Matrix3d R = q.toRotationMatrix();
// 旋转矩阵转欧拉角 (ZYX 顺序)
Eigen::Vector3d euler = R.eulerAngles(2, 1, 0);
std::cout << "yaw: " << euler[0] << ", pitch: " << euler[1] << ", roll: " << euler[2] << std::endl;
// 用四元数旋转向量
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rot = q * v; // Eigen 重载了乘法
std::cout << "rotated vector: " << v_rot.transpose() << std::endl;
// 构建 SE(3) 变换矩阵
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = Eigen::Vector3d(1, 2, 3);
return 0;
}这段代码我用了好几年，基本没出过问题。唯一要注意的是欧拉角的顺序——不同库的默认顺序可能不一样，一定要确认。
3.6 总结与避坑清单
好，我们快速过一遍今天的内容：
| 表示方法 | 自由度 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 (约束后3) | 直接、易组合 | 冗余、需正交约束 |
| 欧拉角 | 3 | 直观 | 万向锁、插值困难 |
| 四元数 | 4 (约束后3) | 无万向锁、易插值 | 不够直观 |
| 李代数 | 3 (so3) / 6 (se3) | 适合优化、无约束 | 需要指数/对数映射 |
最后，给你几个我亲身踩过的坑：
- 我曾经在标定代码里直接用欧拉角做优化变量，结果迭代到某个角度直接发散。后来换成李代数，稳如老狗。
- 我曾经忘记对四元数做归一化，结果旋转矩阵不满足正交性，点云配准全错。现在我在每次更新四元数后都加一句
q.normalize()。 - 我曾经混淆了旋转矩阵的左右乘规则——左乘是绕固定轴，右乘是绕自身轴。这个搞反了，外参标定结果会完全错误。
嗯，空间同步的基础就这些。下一章我们会聊时间同步，那个坑更多。准备好了吗？