第三章 数学基础（二）：李群与李代数

好，咱们接着聊。上一章我们把三维空间里的旋转和变换矩阵搞清楚了，但有个问题一直卡着我——你想想看，优化一个位姿的时候，总不能直接去加减矩阵里的元素吧？那玩意儿有约束，加完就不是旋转矩阵了。我在做第一个视觉SLAM项目时就踩过这个坑，直接对旋转矩阵做差分，结果迭代出来的矩阵行列式不再是1，整个系统直接崩了。

后来我才明白，得用李群和李代数这套工具。说白了，李群就是那些连续的、光滑的变换群，比如旋转矩阵构成的SO(3)，还有刚体变换矩阵构成的SE(3)。而李代数呢，就是它们对应的“切空间”，也就是在单位元附近的微小扰动。

核心思想：李群描述“状态”，李代数描述“变化”。优化时我们在李代数上做加法，再映射回李群，这样就能保证约束始终满足。

3.1 SO(3) 与 so(3)

SO(3) 是三维旋转矩阵的集合，满足 RTR = I 且 det(R)=1。这个集合构成了一个群，而且是个李群——因为它是连续的，你可以从单位矩阵平滑地变到任意旋转。

对应的李代数 so(3) 呢？其实就是三维向量的集合，通常记作 φ ∈ ℝ3。每个三维向量对应一个反对称矩阵：

φ = [φ1, φ2, φ3]T  →  φ^ = 
[  0   -φ3   φ2 ]
[ φ3    0   -φ1 ]
[-φ2   φ1    0  ]

我个人习惯把 φ 叫做“旋转向量”，因为它其实就是旋转轴乘以旋转角度。嗯，这里要注意：so(3) 里的元素和三维空间中的角速度是同一个东西，物理意义很直观。

3.2 SE(3) 与 se(3)

SE(3) 是刚体变换矩阵的集合，形式是 4×4 矩阵：

T = [ R  t ]
    [ 0  1 ]

对应的李代数 se(3) 是六维向量 ξ = [ρ, φ]T，其中 ρ 是平移部分，φ 是旋转部分。它的反对称矩阵形式是 4×4 的：

ξ^ = [ φ^   ρ ]
     [ 0    0 ]

我在做多传感器融合时，经常需要在 SE(3) 上做插值和滤波。直接在矩阵上做线性插值？不行，插出来的矩阵大概率不是刚体变换。得在 se(3) 上插值，再映射回 SE(3)。

3.3 指数映射与对数映射

这是李群和李代数之间最核心的桥梁。指数映射把李代数中的元素映射到李群，对数映射则反过来。

对于 SO(3)：

给定旋转向量 φ = θ·n（θ 是旋转角，n 是单位旋转轴），指数映射就是罗德里格斯公式：

R = exp(φ^) = I + sinθ·n^ + (1-cosθ)·(n^)2

反过来，给定旋转矩阵 R，对数映射提取旋转向量：

θ = arccos((tr(R)-1)/2)
n^ = (R - RT) / (2·sinθ)

对于 SE(3)：

指数映射稍微复杂一点，涉及一个雅可比矩阵 J：

T = exp(ξ^) = [ exp(φ^)   J·ρ ]
              [   0        1   ]

其中 J = (sinθ/θ)·I + (1-sinθ/θ)·n·nT + ((1-cosθ)/θ)·n^

我的小技巧：实际编程时，别自己手写这些公式，直接用 Sophus 或 Eigen 的库函数。我早期手写过一次，调试了三天才发现是 θ 接近 0 时数值不稳定。

3.4 扰动模型

终于到重点了。在SLAM中，我们经常要对位姿求导。比如，误差函数对位姿的雅可比矩阵怎么算？

有两种方式：

  1. 左扰动模型：在位姿左边乘一个小扰动，然后对扰动求导
  2. 右扰动模型：在位姿右边乘一个小扰动

我个人更常用左扰动模型，因为直观。对于 SO(3)：

∂(R·p) / ∂φ = -R·p^

这个公式的意思是：旋转一个点 p，然后对旋转向量 φ 求导，结果就是 -R 乘以 p 的反对称矩阵。

对于 SE(3)：

∂(T·p) / ∂ξ = [ I   -R·p^ ]
              [ 0     0    ]

我曾经在优化相机位姿时，用错了扰动模型，导致雅可比矩阵符号反了，优化结果一直在震荡。后来换成左扰动模型，一次收敛。

避坑指南：不同文献对扰动模型的定义可能不同。有的用左扰动，有的用右扰动，有的在局部坐标系下扰动。一定要搞清楚你用的库或用的是哪种，否则求导结果差一个负号或一个旋转。

3.5 知识体系总览

下面这张图总结了本章的核心逻辑，我建议你把它记在脑子里：

李群 SO(3): 旋转矩阵 R SE(3): 变换矩阵 T 约束: RTR=I, det(R)=1 李代数 so(3): 旋转向量 φ se(3): 六维向量 ξ 无约束，可自由加减 指数映射 exp(φ^) = R 对数映射 log(R) = φ^ 扰动模型（核心应用） 左扰动: ∂(R·p)/∂φ = -R·p^ 右扰动: ∂(T·p)/∂ξ = [I, -R·p^] 优化时：在李代数上做加法 → 指数映射回李群 → 保证约束始终满足 应用场景：位姿优化、插值、滤波、求导、状态估计

3.6 实际编程中的注意事项

讲完了理论，说点实际的。我在写SLAM系统时，总结了几条经验：

  • 用 Sophus 库：它封装了 SO(3) 和 SE(3) 的常用操作，包括指数映射、对数映射、扰动模型。别自己造轮子。
  • 注意数值稳定性：当旋转角接近 0 时，罗德里格斯公式的分母接近 0，要用泰勒展开近似。
  • 区分左乘和右乘：在优化中，左扰动对应世界坐标系下的雅可比，右扰动对应局部坐标系下的雅可比。搞混了，梯度方向就错了。
  • 链式法则：实际误差函数往往很复杂，记得用链式法则把扰动模型的雅可比和误差函数的雅可比乘起来。

调试技巧：如果你不确定自己的雅可比算对了没，可以用数值微分验证。给位姿加一个小扰动，看误差的变化是否和解析雅可比一致。我每次写新的优化问题都会这么干一遍。

好了，李群和李代数就讲这么多。这套工具是SLAM优化的基石，你后面会反复用到。刚开始可能觉得抽象，但用多了就会发现——说白了，就是一套让你能在流形上做优化的数学工具。

可可精品资料库

← 上一章 📖 返回目录 下一章 →