4. 李群与李代数
说到李群和李代数，很多刚接触SLAM的朋友会觉得头大。说实话，我当年第一次看到这个名词时，也是一脸懵。但后来在实际项目中用多了，发现这东西其实就是个工具——帮我们在旋转矩阵和旋转向量之间来回切换。
为什么要做这种切换？因为旋转矩阵虽然直观，但优化起来很麻烦。你想想看，一个3x3的矩阵有9个参数，但真正的自由度只有3个。直接优化这9个参数，很容易跑出非法的旋转矩阵。而李代数只有3个参数，优化起来就舒服多了。
4.1 从旋转矩阵到旋转向量
我们先从最直观的角度来理解。任何一个旋转，都可以用一个旋转轴和一个旋转角度来描述。这就是旋转向量的思想——向量的方向表示旋转轴，向量的模长表示旋转角度。
那么问题来了：给定一个旋转矩阵R，怎么求出对应的旋转向量？
这里有个公式：
θ = arccos((tr(R) - 1) / 2)
ω = (1 / (2 sin θ)) * [R32 - R23, R13 - R31, R21 - R12]^T其中θ是旋转角度，ω是单位旋转轴。我个人习惯把这个过程叫做「从SO(3)到so(3)的映射」。说白了，就是把9个参数的矩阵压缩成3个参数的向量。
核心要点：SO(3)是旋转矩阵的集合，so(3)是旋转向量的集合。两者通过指数映射和对数映射相互转换。
4.2 指数映射：从so(3)到SO(3)
反过来，给定一个旋转向量，怎么得到旋转矩阵？这就是指数映射要做的事。
公式长这样：
R = exp(φ^) = I + (sin θ / θ) * φ^ + ((1 - cos θ) / θ²) * (φ^)²这里φ是旋转向量，φ^是它的反对称矩阵。这个公式其实就是罗德里格斯公式。我在项目中经常用这个公式，比如在标定相机外参时，需要把优化得到的旋转向量转成旋转矩阵。
小技巧：当θ很小时，sinθ/θ ≈ 1，(1-cosθ)/θ² ≈ 1/2。这时候指数映射可以近似为R ≈ I + φ^。这个近似在视觉SLAM的帧间匹配中很常用。
4.3 SE(3)的指数映射
说完了旋转，我们再来看刚体变换。SE(3)是4x4的变换矩阵，对应的李代数se(3)是6维向量——前3维是平移，后3维是旋转。
SE(3)的指数映射公式稍微复杂一点：
T = exp(ξ^) = [R, t; 0, 1]
其中：
R = exp(φ^)
t = (I - R) * ρ + (θ - sin θ) / θ³ * (φ^) * ρ + ...嗯，这里要注意，平移部分的映射比旋转复杂。我记得有一次在调试手眼标定程序时，就是因为平移部分的映射公式写错了，结果标定出来的外参总是差那么一点点。后来查了半天才发现是这里的问题。
避坑指南：我曾经在SE(3)的指数映射中踩过一个坑——当旋转角度接近0时，平移部分的公式会出现数值不稳定的问题。解决办法是用泰勒展开近似，或者直接用一阶近似。
4.4 知识体系总览
下面这张图总结了本章的核心逻辑：
4.5 实际应用中的选择
在实际的SLAM系统中，我们通常这样使用李群和李代数：
| 场景 | 使用李群 | 使用李代数 |
|---|---|---|
| 状态表示 | 旋转矩阵R、变换矩阵T | 旋转向量φ、6维向量ξ |
| 状态更新 | 乘法更新：R ← R * exp(δφ^) | 加法更新：φ ← φ + δφ |
| 优化求解 | 约束优化（麻烦） | 无约束优化（简单） |
| 插值 | 直接插值会破坏结构 | 线性插值后指数映射 |
我个人更倾向于在优化过程中使用李代数表示，在最终输出时再转成李群。这样做的好处是：优化过程简单，结果又符合物理意义。
总结一下：李群和李代数的指数映射，说白了就是「旋转矩阵 ↔ 旋转向量」和「变换矩阵 ↔ 6维向量」之间的桥梁。理解了这个桥梁，你就能在SLAM的优化问题中游刃有余。
嗯，这一章的内容就到这里。记住，指数映射不是魔法，它只是一个工具。用熟了，你会发现它其实很自然。