第4章：刚体变换与李群李代数
各位同学，欢迎来到第四讲。今天我们要啃一块硬骨头——刚体变换与李群李代数。说实话，我第一次接触这东西的时候，也觉得头大。什么旋转矩阵、四元数、SE(3)……一堆数学符号看得人眼花。但后来在实际项目中，我发现这些概念就是自动驾驶的“地基”。地基不牢，后面标定、融合、SLAM全得翻车。
咱们今天的目标很明确：搞懂旋转和平移怎么表示，以及李群李代数到底在解决什么问题。我会尽量用大白话讲，但该有的公式和代码，一个都不会少。
4.1 旋转矩阵：最直观的旋转表示
先说说旋转矩阵。说白了，它就是描述一个坐标系相对于另一个坐标系怎么转的。一个3x3的矩阵，满足两个性质：正交（R^T R = I）和行列式为+1。
我刚开始做标定时，总喜欢用旋转矩阵。为什么？因为它直观啊。你想想看，一个点p在A坐标系下的坐标，乘以R，就得到了在B坐标系下的坐标。多直接。
// 旋转矩阵示例（C++伪代码）
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()) // 绕Z轴转45度
* Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitY()); // 再绕Y轴转30度
Eigen::Vector3d p_A(1.0, 0.0, 0.0);
Eigen::Vector3d p_B = R * p_A; // 旋转后的坐标但旋转矩阵有个问题：9个参数，只有3个自由度。冗余参数太多，优化起来容易出问题。我在做手眼标定时就吃过这个亏——用旋转矩阵做优化，结果矩阵不满足正交性了，标定结果一塌糊涂。
注意：旋转矩阵必须保持正交且行列式为1。如果你在代码里直接对旋转矩阵做加减法，大概率会破坏这个约束。我建议用四元数或李代数做优化，最后再转回旋转矩阵。
4.2 欧拉角：直观但容易踩坑
欧拉角大家应该不陌生。三个角度：横滚（roll）、俯仰（pitch）、偏航（yaw）。直观，好理解。但我要说，欧拉角是“万向锁”的重灾区。
什么是万向锁？当俯仰角接近±90度时，横滚和偏航会变得无法区分。说白了，你丢失了一个自由度。我在做车载激光雷达标定时，遇到过一辆车停在陡坡上，俯仰角刚好接近90度。结果欧拉角表示直接崩了，旋转矩阵算出来全是NaN。
所以我的建议是：欧拉角只适合给人看，不适合给机器算。做可视化、调参时用欧拉角方便，但内部计算一定要用四元数或旋转矩阵。
| 表示方式 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 直观，容易组合 | 冗余参数，优化困难 |
| 欧拉角 | 直观，参数少 | 万向锁，插值困难 |
| 四元数 | 无奇异性，适合插值 | 不够直观 |
4.3 四元数：自动驾驶的“瑞士军刀”
四元数，我愿称之为旋转表示的“最优解”。一个四元数q = [w, x, y, z]，满足w^2 + x^2 + y^2 + z^2 = 1。它没有万向锁，插值平滑，计算效率高。
我记得有一次做多传感器融合，需要把IMU的角速度积分成姿态。用欧拉角积分，几步就漂移了。换成四元数，精度直接提升一个量级。为什么？因为四元数微分方程是线性的，没有奇点。
// 四元数旋转示例
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(M_PI/3, Eigen::Vector3d::UnitX()) // 绕X轴转60度
* Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()); // 再绕Y轴转45度
Eigen::Vector3d p(1.0, 0.0, 0.0);
Eigen::Vector3d p_rotated = q * p; // 四元数旋转
// 四元数转旋转矩阵
Eigen::Matrix3d R = q.toRotationMatrix();小技巧：四元数乘法不满足交换律。q1 * q2 表示先转q2再转q1。我刚开始总搞反，标定结果一直不对。后来养成了习惯：每次写四元数乘法前，先画个坐标系示意图。
4.4 平移向量与刚体变换 SE(3)
旋转讲完了，加上平移就是完整的刚体变换。一个刚体变换由[R|t]组成，其中R是旋转矩阵，t是平移向量。在自动驾驶里，这表示“激光雷达坐标系下的点，怎么变换到车体坐标系”。
SE(3)是特殊欧氏群，它包含了所有刚体变换。说白了，SE(3)里的每个元素就是一个4x4的齐次变换矩阵：
T = [R t]
[0 1]用齐次坐标，一个点p = [x, y, z, 1]^T，左乘T就完成了变换。我在做激光雷达和相机标定时，每天跟这个T打交道。外参标定的目标，就是找到这个T。
4.5 李群与李代数：为什么需要它？
好，问题来了。SE(3)上的优化怎么做？你想想看，SE(3)是流形，不是欧氏空间。你不能直接在SE(3)上做加减法，因为加法会破坏约束（旋转矩阵不再正交）。
李代数so(3)和se(3)就是来解决这个问题的。它们是对应的“切空间”，是欧氏空间。在so(3)上做优化，然后映射回SE(3)，既保证了约束，又方便计算。
我刚开始做BA（光束法平差）时，直接在SE(3)上做梯度下降，结果迭代几步就发散了。后来换成李代数表示，用指数映射和对数映射在流形和切空间之间来回切换，收敛就稳定了。
核心思想：李代数就是SE(3)的“线性化版本”。在局部范围内，用李代数近似SE(3)，做完优化再映射回去。这就像在地球表面走路——地球是球面（流形），但你脚下的局部区域可以近似为平面（切空间）。
4.6 指数映射与对数映射
指数映射：从李代数到李群。说白了，就是给定一个旋转向量（so(3)中的元素），怎么得到对应的旋转矩阵。公式是罗德里格斯公式：
R = I + sin(θ) * [ω]× + (1 - cos(θ)) * [ω]×^2其中θ是旋转角度，ω是单位旋转轴，[ω]×是ω的反对称矩阵。
对数映射反过来：从旋转矩阵得到旋转向量。我在项目中经常用这个——把优化得到的旋转矩阵转回李代数，方便做误差分析。
// Sophus库示例（C++）
Sophus::SO3d R_so3 = Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3)); // 指数映射
Eigen::Vector3d omega = R_so3.log(); // 对数映射
Sophus::SE3d T_se3 = Sophus::SE3d::exp(Eigen::Matrix<double,6,1>(0.1,0.2,0.3,0.4,0.5,0.6));
Eigen::Matrix<double,6,1> xi = T_se3.log();避坑指南：我曾经在标定代码里直接用旋转矩阵做差分，结果矩阵不再正交，导致后续计算全错。后来我改用李代数表示增量，每次更新都用指数映射，问题就解决了。记住：在流形上做优化，永远用李代数表示增量。
4.7 知识体系总览
下面这张图是我自己总结的，把今天讲的内容串起来。你仔细看一遍，应该能理解各个概念之间的关系。
从这张图你可以看到，刚体变换SE(3)由旋转SO(3)和平移t组成。旋转可以用旋转矩阵、欧拉角、四元数表示。而李群李代数提供了在流形上做优化的数学工具。最终这些知识都服务于外参标定、SLAM等实际应用。
4.8 总结与个人建议
今天的内容信息量不小。我最后给你三个建议：
- 代码里用四元数，可视化用欧拉角。这是最稳妥的做法。
- 优化时用李代数。别在SE(3)上直接做梯度下降，你会后悔的。
- 多画图。每次做变换前，在纸上画个坐标系，标出旋转轴和平移方向。我到现在还保持这个习惯。
嗯，今天就到这里。这些概念需要反复琢磨，建议你动手写几段代码，把旋转矩阵、四元数、李代数之间的转换跑一遍。纸上得来终觉浅，绝知此事要躬行。
可可精品资料库