3. 刚体变换与李群李代数：旋转矩阵、平移向量、欧拉角、四元数、SO(3)与SE(3)、李代数基础
各位同学，欢迎来到第三章。
这一章，我们聊聊自动驾驶感知里最绕不开的数学工具——刚体变换与李群李代数。说实话，很多工程师一听到“李群”、“李代数”就头大，觉得这是纯数学家的玩具。但我在实际项目中吃过亏，才明白这东西有多重要。
你想想看，一辆车在路上跑，激光雷达装在车顶，IMU藏在底盘，摄像头嵌在后视镜里。这些传感器各自有各自的坐标系。怎么把它们统一起来？怎么描述车在三维空间里的运动？答案就在这一章里。
3.1 旋转矩阵：最直观的旋转表达
旋转矩阵，说白了就是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。
我记得刚入行时，总觉得旋转矩阵太“重”了——9个元素，但真正自由的只有3个。不过它的好处是直观，而且可以连续左乘，实现多次旋转的叠加。
核心性质：
- 正交性：RTR = I
- 行列式为+1
- 逆矩阵等于转置：R-1 = RT
在点云处理中，我们经常用旋转矩阵来对齐两帧点云。比如，你有一帧当前时刻的点云，想把它变换到上一时刻的坐标系下，那就左乘一个旋转矩阵。
// C++示例：点云旋转
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(0.25 * M_PI, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(0.1 * M_PI, Eigen::Vector3d::UnitY());
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*input_cloud, *transformed_cloud, R, Eigen::Vector3d::Zero());
3.2 平移向量：简单但容易踩坑
平移向量就是一个3维向量，表示从原坐标系原点到新坐标系原点的位移。听起来很简单对吧？
但我在项目中遇到过一个问题：平移向量是在哪个坐标系下表达的？是原坐标系，还是目标坐标系？
嗯，这里要注意。通常，平移向量是在原坐标系下表达的。也就是说，如果你想把一个点从坐标系A变换到坐标系B，平移向量表示的是“B的原点在A中的位置”。
我曾经踩过的坑：在做多传感器标定时，把平移向量的方向搞反了。结果标定出来的外参，车明明是直行，点云却往左偏。查了两天才发现是平移向量符号错了。所以，每次写完变换代码，先拿一个已知点验证一下。
3.3 欧拉角：直观但小心万向锁
欧拉角用三个角度来描述旋转：偏航角(yaw)、俯仰角(pitch)、滚转角(roll)。在自动驾驶里，我们通常用ZYX顺序——先绕Z轴转yaw，再绕Y轴转pitch，最后绕X轴转roll。
欧拉角的好处是直观，人脑容易理解。但它的致命问题是万向锁(Gimbal Lock)。当pitch接近±90°时，yaw和roll的旋转轴会重合，丢失一个自由度。
为什么会这样？因为欧拉角本质上是三个依次执行的旋转，当中间那个旋转达到90°时，第一个和第三个旋转轴就对齐了。
我的建议：在代码内部，尽量用旋转矩阵或四元数。欧拉角只用来做可视化展示，或者给用户输入输出。别在算法核心逻辑里用欧拉角做插值或优化，你会后悔的。
3.4 四元数：高效且无奇点
四元数，我个人的最爱。它是一个四维向量q = [w, x, y, z]，其中w是实部，x,y,z是虚部。单位四元数可以表示任意三维旋转。
四元数的好处太多了：
- 没有万向锁问题
- 插值平滑（球面线性插值SLERP）
- 计算效率高，比旋转矩阵少乘几次
- 存储只需要4个浮点数
我记得在做IMU和LiDAR的融合时，IMU输出的就是四元数。直接用四元数做姿态预测，比转成欧拉角再转回来要稳定得多。
// 四元数转旋转矩阵
Eigen::Quaterniond q(0.707, 0.0, 0.707, 0.0); // w, x, y, z
Eigen::Matrix3d R = q.toRotationMatrix();
// 旋转一个点
Eigen::Vector3d p(1.0, 0.0, 0.0);
Eigen::Vector3d p_rotated = q * p; // 注意：四元数乘法
3.5 SO(3)与SE(3)：旋转与刚体变换的数学结构
SO(3)是特殊正交群，所有3x3旋转矩阵的集合。SE(3)是特殊欧几里得群，所有4x4刚体变换矩阵的集合。
说白了，SO(3)管旋转，SE(3)管旋转加平移。一个SE(3)元素长这样：
| R t |
| 0 1 |
其中R是3x3旋转矩阵，t是3x1平移向量。
为什么要把它们叫做“群”？因为满足群的四条公理：封闭性、结合律、单位元、逆元。你想想看，两个旋转矩阵相乘还是旋转矩阵，对吧？这就是封闭性。
实际应用：在SLAM和点云配准中，我们经常用SE(3)来表示相机的位姿。比如，ICP算法的每一步迭代，都是在SE(3)上寻找一个增量，使得点云对齐误差最小。
3.6 李代数基础：为什么要学它？
好，终于到了最“劝退”的部分。李代数，到底是什么？
简单说，李代数是李群在单位元处的切空间。SO(3)对应的李代数是so(3)，SE(3)对应的李代数是se(3)。
你可能会问：我直接用旋转矩阵做优化不行吗？
行，但不好。因为旋转矩阵有正交约束，直接优化的话，每次迭代完还得投影回SO(3)上，麻烦得很。而李代数是无约束的，可以在欧几里得空间里随便优化，最后通过指数映射回到李群上。
我在做图优化时，就深刻体会到了李代数的威力。用李代数表示位姿，优化变量只有6个（3个旋转+3个平移），而且不用考虑约束，优化器跑得又快又稳。
核心公式：
- 指数映射：exp(so(3)) → SO(3)
- 对数映射：log(SO(3)) → so(3)
- 对于小量δ，有：exp(δ) ≈ I + δ（一阶近似）
// 使用Sophus库（一个常用的李代数C++库）
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
// 构造SO(3)
Sophus::SO3d R = Sophus::SO3d::rotZ(0.5);
// 李代数so(3)是一个3维向量
Eigen::Vector3d omega(0.1, 0.0, 0.0);
Sophus::SO3d R_new = Sophus::SO3d::exp(omega) * R;
// SE(3)同理
Sophus::SE3d T(R, Eigen::Vector3d(1.0, 2.0, 3.0));
Eigen::Vector3d t = T.translation();
Sophus::SO3d R_part = T.so3();
3.7 知识体系总览
下面这张图，是我自己整理的本章知识结构。你可以把它当作一个思维导图来用。
3.8 实战中的选择建议
讲了这么多，你可能会问：在实际项目中，到底该用哪种表示？
我的经验是这样的：
| 场景 | 推荐表示 | 原因 |
|---|---|---|
| 传感器标定 | SE(3) + 李代数优化 | 无约束优化，收敛快 |
| 点云配准(ICP) | 四元数 + 平移向量 | 插值平滑，无奇点 |
| IMU姿态估计 | 四元数 | IMU原生输出，避免转换误差 |
| 可视化/用户交互 | 欧拉角 | 人脑友好，容易理解 |
| SLAM后端优化 | SE(3) + 李代数 | 图优化标准做法 |
好了，这一章的内容就到这里。记住，数学工具是拿来用的，不是拿来背的。多写代码，多跑实验，慢慢就熟了。
可可精品资料库