第二讲：三维空间刚体运动（上）
各位同学好，我是你们的VIO课程讲师。今天我们来聊聊三维空间刚体运动，这是整个VIO系统的数学基础。说白了，就是搞清楚相机和IMU在三维空间里到底是怎么动的。
我个人习惯把这一讲拆成两半：上半场讲旋转的数学表达，下半场讲Eigen库怎么用。今天我们先搞定上半场。
1. 旋转矩阵：最直观的旋转表达
旋转矩阵，其实就是3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。
举个例子：假设相机坐标系下有个点pc，世界坐标系下是pw。那么：
p_c = R * p_w + t这里的R就是旋转矩阵。嗯，这里要注意：R的每一列都是单位正交的，而且RT = R-1。
核心性质：
- RTR = I（正交性）
- det(R) = +1（右手系）
- 旋转矩阵构成特殊正交群SO(3)
我在项目中遇到过一个问题：直接用9个参数表示旋转，优化时很容易跑飞。因为旋转矩阵的约束太强了，你随便加个微小扰动，它就不再是旋转矩阵了。所以实际工程中，我们很少直接用旋转矩阵做优化。
3. 旋转向量：最紧凑的表达
旋转向量，也叫轴角。它用一个三维向量表示旋转：方向是旋转轴，模长是旋转角度。
为什么需要它？因为旋转矩阵有9个参数，但自由度只有3个。说白了就是冗余。旋转向量只有3个参数，紧凑多了。
从旋转向量到旋转矩阵，用罗德里格斯公式：
R = cosθ * I + (1-cosθ) * nnT + sinθ * n^其中n是单位旋转轴，θ是旋转角，n^是n的反对称矩阵。
我的经验：旋转向量在SLAM后端优化中非常常用。因为它是无约束的3维向量，可以直接作为优化变量。我曾经在VINS-Mono里就用旋转向量做状态量，效果不错。
4. 欧拉角：最符合直觉的表达
欧拉角把旋转分解成绕三个轴的依次旋转。常见的顺序有ZYX、ZYZ等。在航空领域，常用的是偏航-俯仰-滚转（Yaw-Pitch-Roll）。
但欧拉角有个致命问题：万向锁。当俯仰角接近±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。
避坑指南：我曾经在无人机项目里用欧拉角做姿态控制，结果飞机做大机动时直接失控了。后来查了半天，发现是万向锁导致的。从那以后，我只要涉及大角度旋转，一律用四元数。
所以欧拉角适合做可视化展示，但不适合做数学计算。你想想看，如果两个姿态之间的角度差很大，用欧拉角插值会得到非常诡异的路径。
5. 四元数：最优雅的表达
四元数，说白了就是复数的推广。一个四元数q = w + xi + yj + zk，其中i²=j²=k²=ijk=-1。
单位四元数可以表示旋转：
q = [cos(θ/2), n*sin(θ/2)]其中n是单位旋转轴，θ是旋转角。
四元数的优点：
- 无万向锁
- 插值平滑（球面线性插值SLERP）
- 计算效率高（比旋转矩阵快）
四元数的乘法：
q1 * q2 = [w1*w2 - v1·v2, w1*v2 + w2*v1 + v1×v2]嗯，这里要注意：四元数乘法不满足交换律。顺序很重要！
核心公式：用四元数旋转一个三维点p：
p' = q * p * q-1其中p写成纯四元数[0, px, py, pz]
6. Eigen库基础操作与几何模块
Eigen是C++里最常用的线性代数库。我建议你直接把它当成标配，就像Python里的NumPy一样。
先看基础操作：
#include <Eigen/Core>
// 定义矩阵
Eigen::Matrix3d R; // 3x3 double矩阵
Eigen::Vector3d v; // 3维向量
// 赋值
R << 1, 0, 0,
0, 1, 0,
0, 0, 1;
v << 1, 2, 3;
// 运算
Eigen::Vector3d result = R * v; // 矩阵乘向量再看几何模块：
#include <Eigen/Geometry>
// 旋转向量
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));
// 旋转矩阵
Eigen::Matrix3d R = rotation_vector.matrix();
// 四元数
Eigen::Quaterniond q(rotation_vector);
// 欧拉角
Eigen::Vector3d euler = R.eulerAngles(2,1,0); // ZYX顺序我的习惯：在VIO代码里，我通常用四元数做状态量，用旋转向量做增量更新。因为四元数做乘法方便，旋转向量做加法方便。两者配合，完美。
Eigen的几何模块还提供了变换矩阵：
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Eigen::Vector3d(1,2,3));
Eigen::Vector3d p_w(0,0,0);
Eigen::Vector3d p_c = T * p_w; // 齐次坐标变换这里要注意：Isometry3d本质上是4x4矩阵，但Eigen会优化它，只存储旋转和平移部分，节省计算量。
知识体系总览
下面这张图总结了四种旋转表达方式的关系：
这张图把四种表达方式的关系和推荐用法都画出来了。我个人建议：做VIO时，状态量用四元数，更新量用旋转向量。这样既避免了万向锁，又方便做优化。
重要提醒：Eigen默认是列优先存储。如果你用行优先的矩阵，记得加Eigen::RowMajor。我曾经因为这个bug调了一整天，最后发现是存储顺序搞反了。
好了，今天的内容就到这里。旋转矩阵、旋转向量、欧拉角、四元数，这四种表达方式各有各的用武之地。Eigen库的几何模块把这些都封装好了，用起来很方便。下一讲我们会继续聊三维空间刚体运动的下半场：李群李代数。
公众号:蓝海资料掘金营，微信deep3321