3. 旋转表示法：旋转矩阵、欧拉角、旋转向量（轴角）、四元数的原理与互转
旋转，是3D视觉里最基础也最容易翻车的地方。我刚开始做SLAM的时候，觉得旋转不就是转个角度嘛，结果被万向锁坑了好几次，还被四元数的共轭搞晕过。今天咱们就把这四种旋转表示法彻底捋清楚。
3.1 为什么需要四种表示法？
你想想看，一个物体在三维空间里旋转，我们怎么描述它？其实每种表示法都有自己的脾气：
- 旋转矩阵：最直观，但9个参数有冗余，还必须是正交阵
- 欧拉角：人类友好，但会碰到万向锁
- 旋转向量（轴角）：紧凑，但插值不方便
- 四元数：SLAM和图形学的宠儿，但理解起来有点反直觉
我在项目里一般这么用：人机交互用欧拉角，内部计算用四元数，数学推导用旋转矩阵。下面咱们逐个拆解。
3.2 旋转矩阵
旋转矩阵是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。
核心性质：RTR = I，det(R) = 1
假设有一个点p在世界坐标系下的坐标为pw，在相机坐标系下的坐标为pc，那么：
p_c = R * p_w旋转矩阵的优点是能直接做向量旋转，而且组合旋转就是矩阵乘法。但缺点也很明显——9个参数，冗余度太高。你想想看，每次优化都要保证它是正交阵，多麻烦。
我的习惯：在写或做数学推导时，我倾向于用旋转矩阵，因为它公式清晰。但在代码里，我几乎不用它做优化变量。
3.3 欧拉角
欧拉角把旋转分解成绕三个轴的依次旋转。常用的有ZYX顺序（偏航-俯仰-滚转）和XYZ顺序。
举个例子，ZYX顺序的欧拉角：
- 绕Z轴转ψ（偏航角）
- 绕Y轴转θ（俯仰角）
- 绕X轴转φ（滚转角）
对应的旋转矩阵为：
R = R_z(ψ) * R_y(θ) * R_x(φ)我曾经踩过的坑：万向锁！当俯仰角θ = ±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。我在做无人机姿态估计时，就因为没处理好这个问题，导致飞机在俯仰90°时姿态解算直接崩了。
欧拉角的好处是直观，三个角度就能描述旋转。但除了万向锁，它还不适合做插值——两个欧拉角之间线性插值得到的轨迹往往不是最短路径。
3.4 旋转向量（轴角）
旋转向量用一个单位轴u和一个角度θ来描述旋转。形式上就是一个三维向量：
v = θ * u其中v的方向是旋转轴，模长是旋转角度。这个表示法非常紧凑，而且没有冗余。
从旋转向量到旋转矩阵，可以用罗德里格斯公式：
R = I + sinθ * [u]× + (1 - cosθ) * [u]ײ其中[u]×是u的反对称矩阵。
实际应用：在SLAM的后端优化中，旋转向量经常作为旋转的最小表示（3个参数，无约束），配合李代数使用。我做的ORB-SLAM改进版本里，位姿优化用的就是旋转向量。
3.5 四元数
四元数是个让人又爱又恨的东西。它由一个实部和三个虚部组成：
q = w + xi + yj + zk其中w是实部，(x, y, z)是虚部，且满足i² = j² = k² = ijk = -1。
单位四元数（模长为1）可以用来表示旋转。给定旋转轴u和角度θ：
q = [cos(θ/2), u * sin(θ/2)]用四元数旋转一个向量p：
p' = q * p * q⁻¹其中p被表示为纯虚四元数[0, px, py, pz]。
我建议：在代码里统一用四元数做旋转计算。它没有万向锁，插值平滑（球面线性插值SLERP），而且比旋转矩阵省内存。Eigen库的Quaterniond用起来很顺手。
3.6 四种表示法的互转
下面这张表总结了四种表示法的转换关系，我建议你收藏起来：
| 从→到 | 旋转矩阵 | 欧拉角 | 旋转向量 | 四元数 |
|---|---|---|---|---|
| 旋转矩阵 | — | 从R提取各轴角度 | θ = arccos((tr(R)-1)/2) u = (R-RT)∨/(2sinθ) |
从R的9个元素构造 |
| 欧拉角 | 连乘三个轴旋转矩阵 | — | 先转矩阵再转轴角 | 先转矩阵再转四元数 |
| 旋转向量 | 罗德里格斯公式 | 先转矩阵再提取 | — | q = [cos(θ/2), u*sin(θ/2)] |
| 四元数 | 从四元数4个元素构造 | 先转矩阵再提取 | θ = 2arccos(w) u = (x,y,z)/sin(θ/2) |
— |
下面这张SVG图展示了四种表示法之间的转换路径和核心公式：
3.7 代码实战：Eigen库中的旋转互转
下面我用C++和Eigen库演示一下四种表示法的互转。这段代码我在好几个项目里都直接复用：
#include <Eigen/Core>
#include <Eigen/Geometry>
// 1. 定义旋转矩阵
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(M_PI/3, Eigen::Vector3d::UnitX());
// 2. 旋转矩阵 → 欧拉角 (ZYX顺序)
Eigen::Vector3d euler = R.eulerAngles(2, 1, 0); // ZYX
std::cout << "Euler: " << euler.transpose() << std::endl;
// 3. 旋转矩阵 → 旋转向量
Eigen::AngleAxisd angle_axis(R);
std::cout << "Axis: " << angle_axis.axis().transpose() << std::endl;
std::cout << "Angle: " << angle_axis.angle() << std::endl;
// 4. 旋转矩阵 → 四元数
Eigen::Quaterniond q(R);
std::cout << "Quaternion: " << q.coeffs().transpose() << std::endl;
// 5. 四元数 → 旋转矩阵
Eigen::Matrix3d R_from_q = q.toRotationMatrix();
// 6. 四元数旋转向量
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rotated = q * v; // 等价于 R * v
注意：Eigen中四元数的存储顺序是(x, y, z, w)，不是(w, x, y, z)。我第一次用的时候没注意，结果旋转方向全反了，排查了半天才发现是顺序问题。
3.8 工程中的选择建议
说了这么多，到底该用哪种？我根据实际项目经验总结一下：
- SLAM后端优化：用旋转向量（李代数so(3)），因为它是无约束的最小表示
- 姿态滤波（如卡尔曼滤波）：用四元数，避免万向锁，且能平滑插值
- 人机交互界面：用欧拉角，操作员看得懂
- 刚体动力学仿真：用旋转矩阵，公式推导方便
嗯，旋转表示法这块内容确实有点绕，但它是3D视觉的基石。你只要把互转关系练熟了，后面学李群李代数、手眼标定都会轻松很多。