视觉SLAM第4讲｜李群与李代数：位姿优化的数学基石

前面第三讲我们讲了好几种描述刚体位姿的方式：旋转矩阵、四元数、欧拉角。学到这里你可能会问：能用的表示方式都有了，还折腾李群李代数干嘛？

答案很现实：做优化的时候麻烦了 。

旋转矩阵是带约束的：必须正交、行列式必须等于1。你要是把旋转矩阵直接当优化变量，每次更新完都得想办法把它"掰回"正交的样子，既麻烦又容易发散。

李群与李代数，就是来解决这个痛点的：把带约束的李群元素，映射成无约束的李代数向量。这样一来，位姿优化就变成了普通的无约束向量优化，求导、迭代都顺理成章。

一、李群：带特殊性质的矩阵集合

先从李群说起。群不是什么玄乎的东西，就是一个集合 + 一个运算，满足四条基本规则。

1.1 群的四条基本性质

以我们最熟悉的旋转矩阵SO(3)为例，运算就是矩阵乘法：

封闭性：两个旋转矩阵相乘，结果还是旋转矩阵。转两次，合起来还是一次旋转，符合直觉。
结合律：先算前两个相乘还是后两个相乘，最终结果一样。
幺元：单位矩阵。乘单位矩阵等于没转。
逆元：旋转矩阵的转置（也就是逆），对应反向旋转。转过去再转回来，等于没转。

满足这四条，这个集合就构成一个群。

💡通俗解释：群就是"自己和自己玩，怎么玩都出不去这个圈子"。旋转怎么复合都是旋转，怎么逆都是旋转，所以旋转矩阵全体构成一个群。

1.2 什么是李群

李群就是具有"连续光滑"性质的群。刚体可以连续地旋转、连续地平移，不是跳变的，所以SO(3)、SE(3)都是李群。

我们主要打交道的就是两个矩阵李群：

SO(3)：三维旋转矩阵群，所有3×3、正交、行列式为1的矩阵。
SE(3)：三维欧氏变换群，所有4×4的刚体变换矩阵。

二、李代数：李群单位元处的正切空间

李代数描述的是李群在单位元附近的局部性质，说白了就是：李群的导数空间。

2.1 从旋转矩阵求导引出so(3)

我们从一个很直观的问题切入：一个随时间连续变化的旋转矩阵R(t)，它对时间的导数长什么样？

对"R乘以自身转置等于单位矩阵"这个式子两边求导，整理之后会得到一个结论：

R的导数等于一个反对称矩阵乘以R本身。

反对称矩阵，我们第三讲讲过，和三维向量是一一对应的。这个和导数对应的三维向量，就是李代数so(3)的元素。

💡通俗解释：你可以把李代数理解成"旋转的速度向量"。就像速度是位置的导数，李代数就是旋转矩阵在单位元处的"切向速度"。

so(3)的元素就是三维向量，它对应的反对称矩阵，就是我们第三讲讲的尖号算子形式。

两个李代数元素的李括号，本质就是叉乘运算。

2.2 se(3)：SE(3)对应的李代数

SE(3)对应的李代数se(3)，是一个六维向量：

后三维：旋转部分，对应so(3)
前三维：平移部分，但注意：它不是直接的平移向量t

se(3)对应的4×4矩阵，左上角是旋转对应的反对称矩阵，右侧是平移向量，底部一行全是0。

⚠️易错提醒：se(3)的平移分量，和变换矩阵里的平移向量t不是一回事。中间差了一个雅可比矩阵J的变换，后面指数映射会讲到。

三、指数与对数映射：李群和李代数的双向桥梁

有了李代数，怎么和李群互相转换？靠指数映射和对数映射。

3.1 SO(3)的指数映射

so(3)向量做指数映射，就得到SO(3)的旋转矩阵。

说穿了，指数映射就是罗德里格斯公式 。

旋转向量转旋转矩阵，我们第三讲就会了，本质就是指数映射。

反过来，旋转矩阵做对数映射，就得到对应的so(3)向量。

💡衔接记忆：第三讲的旋转向量 ↔ 旋转矩阵，本质就是李代数 ↔ 李群的指数/对数映射。换了个数学名字而已。

指数映射是满射，不是单射。意思是每个旋转矩阵都能找到对应的李代数，但可能有好几个李代数对应同一个旋转。毕竟旋转角度差2π，转出来结果一样。

通常我们把角度限制在±π之间，这样就一一对应了。

3.2 SE(3)的指数映射

SE(3)的指数映射稍微复杂一点：

左上角：旋转部分，还是so(3)的指数映射，没变化。
右上角：平移部分，不是直接等于se(3)的平移分量，要左乘一个雅可比矩阵J。

这个J矩阵由旋转部分的角度和转轴决定。也就是说：李代数里的平移量，要经过J的线性变换，才最终变成变换矩阵里的平移向量t。

反过来也一样，从变换矩阵求对数映射得到se(3)，平移部分也要经过J的逆变换。

四、BCH近似：李代数加法对应李群乘法

李代数最大的用处是做优化、做增量更新。那问题来了：李群上两个矩阵相乘，对应李代数上是什么操作？

直接相加是不对的。完整的展开式叫BCH公式（贝克-坎贝尔-豪斯多夫公式）。

4.1 BCH线性近似

当其中一个旋转量是很小的增量时，高阶小项可以忽略，就有了BCH近似。

简单说：

给旋转R左乘一个微小旋转，对应的李代数增量 ≈ 左雅可比的逆 × 微小李代数 + 原李代数
右乘同理，对应右雅可比

💡通俗解释：李群上"乘一个小变换"，近似等于李代数上"加一个小向量"。只是这个小向量要乘一个雅可比系数，不是直接加。

这就是为什么我们能对位姿做增量优化：每次算一个李代数增量，加到当前李代数上，再映射回李群，就完成了一次位姿更新。

4.2 扰动模型：求导的标准姿势

做优化必须求导。对位姿求导，标准做法就是扰动模型：给位姿左乘或者右乘一个微小扰动，然后求结果关于扰动的导数。

最常用的左扰动模型，结论非常简洁：

一个空间点p经过旋转R之后得到Rp，对左扰动李代数求导，结果等于 -(Rp)的反对称矩阵。

💡这个结论有多重要？后面视觉里程计、后端BA里，所有重投影误差对位姿的雅可比，全都是从这个式子推出来的。可以说，不会这个导数，后端优化就没法写。

SE(3)的扰动模型同理，最终得到一个4×6的雅可比矩阵，是后面位姿图优化的基础。

五、实践：Sophus库的使用

理论最终落地到代码。Eigen没有原生的李代数支持，Strasdat维护的Sophus库就是专门干这个的，直接基于Eigen开发，纯头文件，用起来很方便。

5.1 基本操作

构造：可以从旋转矩阵、四元数直接构造SO3对象；从R和t构造SE3对象。
对数映射：调用.log()方法，直接得到对应的李代数向量。
指数映射：Sophus::exp(李代数向量)，得到对应的李群矩阵。
增量更新：用exp(增量)乘原李群元素，就是带BCH近似的更新。

💡新手注意：Sophus的se(3)存储顺序是平移在前、旋转在后，和有些书里的顺序不一样，用的时候注意对应。

5.2 工程常用：轨迹误差计算

SLAM里评估算法精度，最常用的指标就是ATE（绝对轨迹误差） 。

做法很简单：把估计轨迹和真实轨迹，每一位姿做差，转成李代数算距离，再算均方根误差RMSE。

用Sophus写起来非常简洁：两个变换矩阵求逆相乘，log转李代数，算范数平方，平均开根号就是RMSE。

这个指标是论文里的标配，做算法对比必算。

六、补充：相似变换群Sim(3)

单目SLAM有尺度不确定性，所以还会用到相似变换群Sim(3)，就是在SE(3)基础上多了一个尺度因子s。

对应的李代数sim(3)是7维向量，比se(3)多了一维尺度σ。

指数映射之后，旋转部分多一个指数缩放，平移部分的雅可比也更复杂。

单目稠密重建、单目回环校正里经常会碰到Sim(3)，知道有这么个东西就行。

小结

这一讲看起来偏数学，但它是整个后端优化的数学基石。

核心价值就是一句话：把带约束的位姿优化，转化成了无约束的李代数向量优化 。

不用再操心正交约束、行列式约束，直接在向量空间里算梯度、做迭代，效率和稳定性都上了一个台阶。

后面讲后端的光束法平差、位姿图优化，全都是建立在李代数扰动模型的基础之上。把这一讲吃透，后面看后端公式就不会觉得是凭空蹦出来的了。