数学基础回顾（一）：三维空间刚体运动、旋转矩阵、旋转向量、欧拉角、四元数。李群与李代数基础
做VIO，说白了就是跟“位姿”打交道。你想想看，相机在动，IMU在转，我们得知道每一时刻它“在哪儿”、“脸朝哪”。这背后全是三维空间刚体运动的数学。
我刚开始接触SLAM时，觉得旋转矩阵、四元数这些东西就是一堆公式，背下来就行。后来踩了坑才发现——不理解背后的几何意义，代码写出来全是bug。今天咱们就把这块地基夯实了。
核心思想：三维空间中的刚体运动 = 旋转 + 平移。旋转是难点，也是VIO里最容易出问题的地方。
1. 旋转矩阵：最直观，但最“贵”
旋转矩阵是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。
举个例子：相机坐标系下的点Pc，想转到世界坐标系Pw，就是Pw = R * Pc + t。这里的R就是旋转矩阵。
优点：直观，组合方便（矩阵乘法）。
缺点：9个参数，冗余。而且你想想，每次更新都要保证它是正交矩阵，数值误差一积累，矩阵就“歪”了。
我的经验：在VIO的优化过程中，我从来不用旋转矩阵作为优化变量。为什么？因为它有约束条件（正交且行列式为1），优化起来太麻烦。我更喜欢用李代数。
2. 旋转向量：紧凑，但有奇点
旋转向量用3个参数表示旋转：方向是旋转轴，模长是旋转角度。也就是罗德里格斯公式干的事。
公式很简单：R = cosθ * I + (1 - cosθ) * n * n^T + sinθ * n^∧
其中n是单位旋转轴，θ是旋转角，n^∧是n的反对称矩阵。
优点：紧凑，3个参数。
缺点：当θ接近0或π时，数值不稳定。尤其是θ=0时，旋转轴方向不确定——这就是奇点问题。
避坑指南：我曾经在IMU预积分中用旋转向量做插值，结果在静止状态下（旋转角接近0）数值直接炸了。后来改用四元数球面插值才解决。
3. 欧拉角：最符合直觉，但小心“万向锁”
欧拉角用三个角度表示旋转：偏航(Yaw)、俯仰(Pitch)、横滚(Roll)。说白了就是“先绕Z轴转多少，再绕Y轴转多少，最后绕X轴转多少”。
但有个大坑——万向锁。当俯仰角为±90°时，偏航和横滚的旋转轴重合，丢失一个自由度。这时候你无论怎么调，都转不到某些姿态。
我的建议：欧拉角只用来给人看（比如显示无人机姿态），绝对不要用在VIO的优化或插值里。我见过有人用欧拉角做EKF的状态量，结果万向锁一出，滤波器直接发散。
4. 四元数：VIO里的“真香”选择
四元数是一个超复数：q = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部。它用4个参数表示旋转，没有奇点。
单位四元数（模长为1）表示旋转。旋转一个点p：p' = q * p * q^(-1)。
优点：
- 无奇点
- 插值平滑（球面线性插值）
- 组合方便（四元数乘法）
缺点：需要归一化，而且对新手来说不够直观。
核心公式：四元数乘法 q1 * q2 对应旋转矩阵乘法 R1 * R2。但注意顺序：q1 * q2 表示先转q2再转q1。
嗯，这里要注意：四元数有两种表示方式——Hamilton（右手系）和JPL（左手系）。VIO里常用Hamilton，但有些代码库用JPL。搞混了，旋转方向就反了。我当年就因为这个，调试了整整两天。
5. 李群与李代数：VIO优化的“杀手锏”
李群SO(3)是旋转矩阵的集合，李代数so(3)是反对称矩阵的集合。它们之间通过指数映射和对数映射联系。
为什么要引入李代数？因为旋转矩阵的优化是有约束的，而李代数是无约束的。你想想看，在优化中，我们要求导数、求增量，如果变量是旋转矩阵，你还得保证它正交——多麻烦。但李代数就是个三维向量，随便加，加完再映射回旋转矩阵就行。
核心公式：
- 指数映射：R = exp(φ^∧)，其中φ是李代数向量
- 对数映射：φ = log(R)^∨
- BCH近似：ln(exp(φ1) * exp(φ2)) ≈ φ1 + φ2 + 1/2 * [φ1, φ2] + ...
我的习惯：在VIO的BA优化中，我全部用李代数表示旋转。这样求导简单，而且不用考虑约束。代码里用Sophus库，直接调用SO3::exp()和SO3::log()，非常方便。
6. 各表示方法的对比
| 表示方法 | 参数数量 | 有无奇点 | 是否适合优化 | 是否适合插值 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 否（有约束） | 否 |
| 旋转向量 | 3 | 有（θ=0或π） | 是（但需处理奇点） | 否 |
| 欧拉角 | 3 | 有（万向锁） | 否 | 否 |
| 四元数 | 4 | 无 | 是（需归一化） | 是 |
| 李代数 | 3 | 无 | 是（无约束） | 否 |
7. 知识体系结构图
下面这张图展示了本章的核心逻辑：从三维空间刚体运动出发，引出旋转的多种表示方法，以及它们在VIO中的角色。
8. 代码示例：四元数与旋转矩阵的转换
下面是用Eigen库实现四元数和旋转矩阵互相转换的代码。这是VIO里最常用的操作。
#include <Eigen/Core>
#include <Eigen/Geometry>
// 定义旋转矩阵
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());
// 旋转矩阵 → 四元数
Eigen::Quaterniond q(R);
std::cout << "四元数: " << q.coeffs().transpose() << std::endl;
// 四元数 → 旋转矩阵
Eigen::Matrix3d R_from_q = q.toRotationMatrix();
std::cout << "旋转矩阵:\n" << R_from_q << std::endl;
// 四元数乘法（组合旋转）
Eigen::Quaterniond q1(0.707, 0.0, 0.707, 0.0); // 绕Y轴90度
Eigen::Quaterniond q2(0.866, 0.0, 0.0, 0.5); // 绕Z轴60度
Eigen::Quaterniond q_combined = q1 * q2; // 先转q2再转q1
std::cout << "组合四元数: " << q_combined.coeffs().transpose() << std::endl;
// 注意：四元数需要归一化
q_combined.normalize();重要提醒：Eigen中Quaterniond的构造函数是(w, x, y, z)，但coeffs()返回的是(x, y, z, w)。这个顺序问题我至少搞错过三次。每次调试时发现旋转方向不对，第一件事就是检查四元数的存储顺序。
9. 李代数求导：VIO优化的核心
在VIO的BA优化中，我们需要对位姿求导。如果用旋转矩阵，导数公式复杂且要考虑约束。用李代数就简单多了。
扰动模型：给李代数一个微小扰动δφ，然后看误差怎么变。
// 李代数求导示例（扰动模型）
// 假设有一个3D点p，经过旋转R（对应李代数φ）后得到p'
// 我们想求 p' 对 φ 的导数
Eigen::Vector3d p(1.0, 2.0, 3.0);
Eigen::Vector3d phi(0.1, 0.2, 0.3); // 李代数
// 指数映射得到旋转矩阵
Eigen::Matrix3d R = Sophus::SO3d::exp(phi).matrix();
// p' = R * p
Eigen::Vector3d p_prime = R * p;
// 导数：∂p'/∂φ = -R * p^∧ （右扰动模型）
Eigen::Matrix3d p_hat;
p_hat << 0, -p(2), p(1),
p(2), 0, -p(0),
-p(1), p(0), 0;
Eigen::Matrix<double, 3, 3> J = -R * p_hat;
std::cout << "雅可比矩阵:\n" << J << std::endl;我的建议：刚开始学李代数时，别纠结于数学推导。先会用Sophus库，把指数映射、对数映射、扰动模型这几个API跑通。等代码跑通了，再回头理解数学，会轻松很多。
好了，这一章的内容就到这里。旋转的表示方法很多，但在VIO里，我推荐你记住一个原则：优化用李代数，插值用四元数，给人看用欧拉角。这样能避开绝大多数坑。