3. 数学基础（上）

各位同学，欢迎来到数学基础部分。说实话，很多做SLAM的朋友一听到数学就头疼，觉得太抽象。我当年刚入行时也一样，觉得旋转矩阵、四元数这些东西离实际工程很远。直到有一次，我在标定一个激光雷达和IMU时，因为欧拉角万向锁的问题，折腾了整整三天……嗯，从那以后，我再也不敢轻视这些数学工具了。

这一章，我们重点讲清楚两件事：三维空间刚体变换和李群与李代数。它们是外参标定的数学根基，也是你理解后续所有算法的基础。

3.1 旋转矩阵：最直观的旋转表达

旋转矩阵，说白了就是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的姿态。

假设你有一个点 p 在世界坐标系下的坐标为 p_w，在相机坐标系下为 p_c，那么：

p_c = R * p_w + t

这里的 R 就是旋转矩阵，t 是平移向量。

旋转矩阵的性质：

• R^T * R = I（正交性）
• det(R) = 1（右手系）
• 三个自由度（9个元素，6个约束）

3.2 欧拉角：直观但危险

欧拉角用三个角度（roll, pitch, yaw）来描述旋转。你想想看，这多直观啊——飞机怎么转的？先绕X轴滚转，再绕Y轴俯仰，最后绕Z轴偏航。

但问题来了：万向锁。当pitch接近±90°时，roll和yaw的旋转轴会重合，丢失一个自由度。我在做无人机IMU标定时就踩过这个坑——飞机悬停时pitch接近90°，欧拉角表示直接炸了。

我个人建议：在SLAM系统中，尽量别用欧拉角做内部计算。它只适合给人看的，比如显示姿态角。

表达方式	优点	缺点
旋转矩阵	无奇异性，易于组合	冗余，不紧凑
欧拉角	直观，3个参数	万向锁，不连续
四元数	无奇异性，紧凑	不直观，需归一化

3.3 四元数：SLAM中的主力军

四元数，说白了就是一个超复数：q = w + xi + yj + zk，其中 w^2 + x^2 + y^2 + z^2 = 1。

为什么SLAM系统偏爱四元数？因为它既没有万向锁，又比旋转矩阵紧凑（4个参数），而且插值方便。

四元数乘法：

q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
          (w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
          (w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
          (w1*z2 + x1*y2 - y1*x2 + z1*w2)k

嗯，这个公式看着复杂，但实际用的时候，Eigen库一行代码就搞定了：q1 * q2。

3.4 李群与李代数：为什么需要它？

你可能会问：有了旋转矩阵和四元数，为什么还要学李群李代数？

原因很简单：优化问题需要求导。旋转矩阵和四元数都有约束（正交性、单位模长），直接求导很麻烦。而李代数是一个无约束的向量空间，求导方便多了。

SO(3) 与 so(3)：

• SO(3)：三维旋转群，就是所有旋转矩阵的集合
• so(3)：SO(3)的李代数，由三维向量组成，对应旋转轴和角度

它们之间通过指数映射和对数映射关联：

R = exp(φ^)    // 从李代数到李群
φ = log(R)     // 从李群到李代数

其中 φ^ 是 φ 的反对称矩阵。这个映射关系，说白了就是把旋转向量变成旋转矩阵，反过来也一样。

3.5 SE(3) 与 se(3)：带上平移一起玩

SE(3) 是刚体变换群，包含旋转和平移。它的李代数是 se(3)，是一个6维向量（3维旋转 + 3维平移）。

在实际的外参标定中，我们通常用 SE(3) 来表示两个传感器之间的变换关系。比如激光雷达坐标系到IMU坐标系的变换：

T_lidar_imu = [R, t; 0, 1]

这个 T 就是一个 SE(3) 元素。

3.6 知识体系总览

下面这张图，是我自己总结的本章知识结构。你把它理解了，后面的课程就顺了。

3.7 实战中的选择建议

讲了这么多，你可能想问：到底用哪个？

我个人经验是这样的：

• 做优化时：用李代数（so3/se3），方便求导和增量更新
• 做插值时：用四元数（球面线性插值Slerp）
• 给人看时：转成欧拉角显示
• 做变换时：用旋转矩阵或SE(3)变换矩阵