4、数学基础（一）：三维空间刚体运动：旋转矩阵、旋转向量、欧拉角、四元数
做SLAM这么多年，我越来越觉得一个道理：搞懂三维空间里的旋转，你就拿到了SLAM的半个入场券。
你想想看，相机在动，地图点在变，机器人到处跑。所有这些运动，归根结底就是两件事：它转到哪儿了？它移到哪儿了？ 平移好办，就是三个数（x, y, z）。但旋转？嗯，这里面的门道可就多了。
今天我们就来把这几种旋转表示法彻底捋一遍。我会结合我实际踩过的坑来讲，希望能帮你少走弯路。
核心观点： 旋转矩阵适合做运算，四元数适合做插值和滤波，欧拉角适合给人看。没有银弹，只有最合适的场景。
4.1 旋转矩阵：最直观，但也最笨重
旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的姿态。
它的核心性质就两条：正交（矩阵的逆等于它的转置）和行列式为+1。满足这两个条件的，才叫旋转矩阵，数学上叫 SO(3) 群。
我的小技巧： 判断一个矩阵是不是合法的旋转矩阵，我一般先算一下它的行列式。如果接近1，基本没问题。如果差得远，那肯定是代码写错了。
在实际工程里，旋转矩阵最大的好处是可以连续左乘。比如你把相机转了R1，又转了R2，那总的旋转就是 R = R2 * R1。注意顺序，是右乘先发生的旋转。
// C++ 示例：用 Eigen 构造旋转矩阵
#include <Eigen/Dense>
Eigen::Matrix3d R;
// 绕 Z 轴旋转 45 度
double theta = M_PI / 4;
R << cos(theta), -sin(theta), 0,
sin(theta), cos(theta), 0,
0, 0, 1;
std::cout << "旋转矩阵 R:\n" << R << std::endl;
但旋转矩阵有个致命缺点：9个参数，冗余度太高。你想想看，明明3个自由度就够了，你非得用9个数去描述。这不仅浪费内存，还容易因为数值误差导致矩阵不再是正交的。我早期做VIO的时候就吃过这个亏——迭代几次后矩阵就“漂”了，姿态全乱套。
4.2 旋转向量：紧凑，但运算麻烦
旋转向量，也叫轴角（Axis-Angle）。它用一个三维向量来表示旋转：方向代表旋转轴，模长代表旋转角度。
比如你想绕单位向量 n 转 θ 角，那旋转向量就是 θ·n。
从旋转向量到旋转矩阵，有一个著名的公式——罗德里格斯公式：
R = cosθ * I + (1 - cosθ) * n * n^T + sinθ * n^∧其中 n^∧ 是 n 的反对称矩阵。这个公式在代码里很常用，Eigen 已经封装好了：
Eigen::AngleAxisd rotation_vector(theta, Eigen::Vector3d::UnitZ());
Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
注意： 旋转向量虽然紧凑，但你不能直接对它做加法或插值。比如两个旋转向量相加，结果通常不是合法的旋转。这一点和四元数不同。
4.3 欧拉角：人类友好，但小心万向锁
欧拉角，说白了就是把一次旋转拆成绕三个轴的依次旋转。最常用的是 ZYX 顺序（偏航-俯仰-滚转），也就是 yaw-pitch-roll。
为什么说它人类友好？因为直观啊！你一说“偏航30度，俯仰10度”，我脑子里立刻就有画面了。调试的时候，我经常把四元数转成欧拉角打印出来看。
但欧拉角有个臭名昭著的问题——万向锁。当俯仰角接近 ±90° 时，偏航和滚转的旋转轴会重合，导致丢失一个自由度。这时候你无论怎么转偏航和滚转，效果都一样。
避坑指南： 我曾经在无人机项目里直接用欧拉角做姿态控制，结果飞机做大机动时直接失控。后来换成四元数才解决。记住：欧拉角只适合做显示和人工输入，不适合做核心运算。
// Eigen 中欧拉角的使用
Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0); // ZYX 顺序
std::cout << "yaw pitch roll: " << euler_angles.transpose() << std::endl;
4.4 四元数：SLAM 的工业标准
四元数，说实话我第一次接触时觉得这玩意儿很玄乎。一个标量加三个虚部，怎么就描述旋转了？
但用久了你会发现，它简直是 SLAM 的瑞士军刀：无奇点、可插值、运算快。
一个单位四元数 q = [w, x, y, z]^T，满足 w² + x² + y² + z² = 1。它对应的旋转是：绕单位轴 n 转 θ 角，则 q = [cos(θ/2), sin(θ/2)·n]。
注意那个 θ/2！这是初学者最容易搞错的地方。我刚开始写代码时，直接把角度传进去，结果旋转角度翻倍，找了好久才发现问题。
// 四元数基本操作
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
// 旋转一个点
Eigen::Vector3d p(1, 0, 0);
Eigen::Vector3d p_rot = q * p; // 注意：Eigen 重载了 * 运算符
// 四元数乘法（表示旋转复合）
Eigen::Quaterniond q1, q2;
Eigen::Quaterniond q_total = q2 * q1; // 先转 q1，再转 q2
我的习惯： 在 SLAM 系统里，我统一用四元数做状态变量。只在输出给可视化模块时，才转成欧拉角。这样既避免了万向锁，又方便调试。
4.5 四种表示法的对比
| 表示法 | 参数数量 | 优点 | 缺点 | SLAM 中的典型用途 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 运算方便，可连续左乘 | 冗余，数值易漂移 | 坐标变换、图优化中的约束 |
| 旋转向量 | 3 | 紧凑，无约束 | 不能直接插值 | 优化中的增量表示 |
| 欧拉角 | 3 | 直观，人类友好 | 万向锁，不连续 | 调试输出、遥控指令 |
| 四元数 | 4 | 无奇点，可插值，运算快 | 不够直观，需归一化 | 状态估计、滤波、位姿插值 |
最后说一句，没有最好的表示法，只有最合适的场景。我个人在写 SLAM 系统时，内部状态全部用四元数，优化时用旋转向量做增量，调试时转成欧拉角看。各取所长，才是工程之道。
总结一下： 旋转矩阵是基础，四元数是主力，欧拉角是辅助，旋转向量是工具。把这四种吃透了，三维空间里的刚体运动，你就再也不会迷糊了。
公众号:蓝海资料掘金营，微信deep3321