3、刚体变换与李群李代数：SE(3)、SO(3)、指数映射、对数映射、扰动模型

说实话，刚接触SLAM的时候，我最头疼的就是这部分。旋转矩阵、变换矩阵，看着挺规整，但真要优化起来，麻烦就来了——你加个微小增量，它就不再是旋转矩阵了。嗯，这就是为什么我们需要李群李代数。

3.1 为什么需要李群李代数？

先说说痛点。你想想看，一个旋转矩阵R，它必须满足R^T R = I，行列式为+1。这是个约束优化问题。如果你直接用R加上一个微小扰动ΔR，结果大概率不满足约束条件。说白了，旋转矩阵所在的SO(3)空间不是线性空间，你不能在里面随便做加法。

我个人习惯把李群李代数理解成「换个坐标系做优化」。李代数是一个向量空间，你可以自由地做加法、求导。然后通过指数映射，把李代数上的结果映射回李群。这样就把有约束的优化问题，变成了无约束的优化问题。

3.2 SO(3) 与 so(3)

SO(3) 是三维旋转群，它的元素是3x3的旋转矩阵。so(3) 是SO(3)对应的李代数，它的元素是3x3的反对称矩阵，或者等价地，一个三维向量。

为什么是反对称矩阵？因为旋转矩阵的导数在单位元处就是反对称的。这个推导过程我就不展开了，你记住结论就行：

• SO(3) = { R ∈ R^{3×3} | R^T R = I, det(R)=1 }
• so(3) = { φ ∈ R^3, φ^∧ ∈ R^{3×3} }，其中 φ^∧ 是反对称矩阵

这里有个小细节：φ^∧ 这个符号，∧ 表示把向量转换成反对称矩阵。比如 φ = [φ1, φ2, φ3]^T，那么：

φ^∧ = [  0   -φ3   φ2
         φ3    0   -φ1
        -φ2   φ1    0  ]

我在项目中遇到过一个问题：有人把旋转向量和李代数搞混了。其实旋转向量就是李代数的一种表示，它们是一一对应的。旋转向量的方向是旋转轴，模长是旋转角。

3.3 SE(3) 与 se(3)

SE(3) 是三维刚体变换群，包含旋转和平移。它的元素是4x4的变换矩阵T。se(3) 是SE(3)对应的李代数。

SE(3) 的结构是这样的：

T = [ R  t
      0  1 ]

其中R是旋转矩阵，t是平移向量。se(3) 的元素是 ξ = [ρ, φ]^T，其中ρ是平移相关的部分，φ是旋转相关的部分（就是so(3)的向量）。

se(3) 的反对称矩阵形式是：

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

注意这里ρ和φ的维度都是3，所以ξ是6维向量。嗯，这个维度很重要，后面优化的时候自由度就是6。

3.4 指数映射与对数映射

指数映射把李代数映射到李群，对数映射是它的逆操作。说白了，就是向量和矩阵之间的转换。

SO(3) 的指数映射：

对于so(3)中的向量φ，指数映射的结果是：

R = exp(φ^∧) = I + sin(θ)/θ * φ^∧ + (1-cos(θ))/θ^2 * (φ^∧)^2

其中θ = ||φ||。这个公式就是罗德里格斯公式。我记得第一次推导这个公式的时候，花了整整一个下午。后来发现，其实它就是旋转向量到旋转矩阵的转换。

SE(3) 的指数映射：

对于se(3)中的ξ = [ρ, φ]^T，指数映射的结果是：

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

其中J是左雅可比矩阵：

J = sin(θ)/θ * I + (1-sin(θ)/θ) * aa^T + (1-cos(θ))/θ * a^∧

这里a = φ/θ，是单位旋转轴。这个J矩阵在求导的时候特别有用。

3.5 扰动模型

扰动模型是李群李代数最实用的地方。它解决了「如何对旋转/变换矩阵求导」的问题。

有两种扰动模型：

• 左扰动：在当前的R左边乘一个微小旋转
• 右扰动：在当前的R右边乘一个微小旋转

我个人习惯用左扰动模型。为什么？因为左扰动对应的雅可比矩阵形式更简洁，而且和大多数SLAM框架（比如ORB-SLAM）保持一致。

对于SO(3)上的左扰动，导数形式是：

∂(Rp) / ∂φ = -R * p^∧

这个公式在BA优化中反复出现。我曾经在一个项目中，因为搞错了扰动方向，导致优化一直不收敛。排查了两天才发现是左扰动和右扰动搞混了。嗯，从那以后我每次写代码都会在注释里标明「左扰动」。

对于SE(3)上的左扰动，导数形式更复杂一些：

∂(Tp) / ∂ξ = [ I  - (Rp + t)^∧
               0       0      ]

这个矩阵是4x6的，对应了平移和旋转的联合导数。

3.6 知识体系总览

下面这张图展示了本章的核心逻辑：

3.7 实际应用中的选择

在实际项目中，怎么选？我给出一些经验：

场景	推荐方案	原因
纯旋转优化	SO(3) + 左扰动	自由度3，雅可比简单
相机位姿优化	SE(3) + 左扰动	6自由度，平移旋转耦合
IMU预积分	SO(3) + 右扰动	与IMU测量模型更匹配
手眼标定	SE(3) + 双扰动	需要同时优化两个位姿

最后说一句，李群李代数这东西，刚开始会觉得抽象。但一旦你上手写过一次BA优化，亲手推导过雅可比矩阵，你就会发现它其实很自然。说白了，它就是给旋转和变换配了一把合适的「螺丝刀」，让你能在优化的时候拧得动。