2. 旋转向量与欧拉角：从理论到代码

聊完旋转矩阵，咱们来看看更直观的两种表达方式——旋转向量和欧拉角。说实话，我刚入行那会儿，觉得旋转矩阵挺完美的，干嘛还要搞别的？直到我在实际项目中调试机械臂，才发现旋转矩阵那9个参数，真不是给人看的。

2.1 旋转向量：最紧凑的表达

旋转向量，也叫轴角（Axis-Angle）。说白了就是：绕一个单位轴，转一个角度。

数学上，旋转向量 θ = θ * n，其中 n 是单位向量，θ 是旋转角度（弧度）。

你想想看，一个三维向量，既有方向（轴），又有长度（角度），多省事。我习惯用旋转向量做优化，因为它在流形上不冗余，没有约束条件。

罗德里格斯公式

旋转向量和旋转矩阵之间，有个经典的桥梁——罗德里格斯公式：

R = I + sin(θ) * [n]× + (1 - cos(θ)) * [n]ײ

其中 [n]× 是 n 的反对称矩阵。嗯，这里要注意，当 θ 接近 0 时，sin(θ)/θ 会出现数值不稳定。我在做视觉SLAM初始化时踩过这个坑，后来改用泰勒展开处理小角度。

2.2 欧拉角：最直观但最坑

欧拉角大家应该不陌生，就是绕三个轴依次旋转。常见的 ZYX 顺序：先绕 Z 轴转 yaw（偏航），再绕 Y 轴转 pitch（俯仰），最后绕 X 轴转 roll（滚转）。

但这里有个大坑——万向锁。当 pitch 接近 ±90° 时，yaw 和 roll 的旋转轴会重合，丢失一个自由度。我曾经在无人机飞控里遇到过，飞机做特技动作时姿态解算突然炸了，查了半天才发现是万向锁问题。

2.3 代码实战：Eigen 实现

咱们用 Eigen 库来演示。Eigen 是 C++ 里最常用的线性代数库，SLAM 项目基本都离不开它。

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

int main() {
    // 1. 旋转向量 → 旋转矩阵
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
    Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
    std::cout << "旋转矩阵:\n" << R << std::endl;

    // 2. 旋转矩阵 → 欧拉角 (ZYX顺序)
    Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0); // ZYX
    std::cout << "欧拉角 (yaw, pitch, roll): "
              << euler_angles.transpose() << std::endl;

    // 3. 欧拉角 → 旋转矩阵
    Eigen::AngleAxisd roll_angle(euler_angles[2], Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitch_angle(euler_angles[1], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yaw_angle(euler_angles[0], Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d R_from_euler = (yaw_angle * pitch_angle * roll_angle).toRotationMatrix();
    
    std::cout << "从欧拉角恢复的旋转矩阵:\n" << R_from_euler << std::endl;

    return 0;
}

2.4 知识体系结构图

下面这张图，是我自己总结的旋转表示法之间的关系。你看一眼就能明白它们怎么互相转换：

2.5 实际项目中的选择建议

我做了这么多年SLAM，总结下来就一句话：看场景选工具。

• 做优化：用旋转向量或四元数。我在ORB-SLAM里就是用旋转向量做图优化，收敛快、不卡壳。
• 做显示：用欧拉角。给客户看姿态，说"偏航30度、俯仰10度"比给个矩阵强多了。
• 做插值：用四元数（下一章会讲）。欧拉角插值会出现奇怪的抖动，我吃过这个亏。