视觉slam十四讲学习笔记（三）李群与李代数

1. 理解李群与李代数的概念，掌握 SO(3), SE(3) 与对应李代数的表示方式。

2. 理解 BCH 近似的意义。

3. 学会在李代数上的扰动模型。

4. 使用 Sophus 对李代数进行运算。

前言

一、李群李代数基础

1 群

2 李代数的引出

3 李代数的定义

4 李代数 so(3)

5 李代数 se(3)

二、指数与对数映射

1 SO(3) 上的指数映射

2 SE(3) 上的指数映射

三、李代数求导与扰动模型

1 BCH 公式与近似形式

2 SO(3) 李代数上的求导

3 李代数求导

4 扰动模型（左乘）

5 SE(3) 上的李代数求导

四、实践：Sophus

总结

前言

三维世界中刚体运动的描述:旋转矩阵、旋转向量、欧拉角、四元数等。
除了表示之外，还要对它们进行估计和优化。
旋转矩阵自身带有约束（正交且行列式为1)。作为优化变量时，会引入额外的约束，使优化变得困难。
李代数上可以变成无约束优化。

哔哩哔哩课程链接：视觉SLAM十四讲ch4_哔哩哔哩_bilibili

一、李群李代数基础

三维旋转矩阵构成了特殊正交群 SO(3)，而变换矩阵构成了特殊欧氏群 SE(3)：

旋转矩阵也好，变换矩阵也好，它们对加法是不封闭的。换句话说，对于任意两个旋转矩阵 R1, R2，它们按照矩阵加法的定义，和不再是一个旋转矩阵：

对于变换矩阵亦是如此。我们发现，这两种矩阵并没有良好定义的加法，相对的，它们只有一种较好的运算：乘法。SO(3) 和 SE(3) 关于乘法是封闭的:

道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合，把它叫做群。

1 群

旋转矩阵集合和矩阵乘法构成群。
同样变换矩阵和矩阵乘法也构成群。
因此称它们为旋转矩阵群和变换矩阵群。

群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 A，运算记作 ·，那么群可以记作 G = (A, ·)。群要求这个运算满足以下几个条件：

常见的群有：

一般线性群 GL(n) 指 n × n 的可逆矩阵，它们对矩阵乘法成群。
特殊正交群 SO(n) 也就是所谓的旋转矩阵群，其中 SO(2) 和 SO(3) 最为常见。
特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换，如 SE(2) 和 SE(3)。

李群是指具有连续（光滑）性质的群。像整数群 Z 那样离散的群没有连续性质，所以不是李群。而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。

2 李代数的引出

任意旋转矩阵 R，满足：

如果，R 是某个相机的旋转，它会随时间连续地变化，即为时间的函数：R(t)。由于它仍是旋转矩阵，有：

对于任意反对称矩阵，我们亦能找到一个与之对应的向量。把这个运算用符号 ∨ 表示：

等式两边右乘 R(t)，由于 R 为正交阵，有：

可以看到，每对旋转矩阵求一次导数，只需左乘一个 ϕ ∧(t) 矩阵即可。设 t0 = 0，并设此时旋转矩阵为 R(0) = I。按照导数定义，可以把 R(t) 在 0 附近进行一阶泰勒展开：

我们看到 ϕ 反映了 R 的导数性质，故称它在 SO(3) 原点附近的正切空间 (Tangent Space) 上。同时在 t0 附近，设 ϕ 保持为常数 ϕ(t0) = ϕ0。

由于做了一定的假设，所以它只在 t = 0 附近有效。旋转矩阵 R 与另一个反对称矩阵 ϕ0 通过指数关系发生了联系。也就是说，当知道某个时刻的 R 时，存在一个向量 ϕ，它们满足这个矩阵指数关系。

如果上式成立，那么给定某时刻的 R，就能求得一个 ϕ，它描述了 R 在局部的导数关系。与 R 对应的 ϕ 有什么含义呢？后面会看到，ϕ 正是对应到 SO(3) 上的李代数 so(3)；
其次，矩阵指数 exp(ϕ ∧) 如何计算？——事实上，这正是李群与李代数间的指数/对数映射。

3 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定义如下：

李代数由一个集合 V，一个数域 F 和一个二元运算 [, ] 组成。如果它们满足以下几条性质，称 (V, F, [, ]) 为一个李代数，记作 g。

其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

4 李代数 so(3)

SO(3) 对应的李代数是定义在 R 3上的向量，记作 ϕ。每个 ϕ 都可以生成一个反对称矩阵：

由于 ϕ 与反对称矩阵关系很紧密，在不引起歧义的情况下，就说 so(3) 的元素是 3 维向量或者 3 维反对称矩阵，不加区别：

与 SO(3) 的关系由指数映射给定：

5 李代数 se(3)

对于 SE(3)，它也有对应的李代数 se(3)。为省略篇幅，我们就不描述如何引出 se(3)了。与 so(3) 相似，se(3) 位于 R 6 空间中：

把每个se(3)元素记作ξ，它是一个六维向量。前三维为平移，记作 ρ；后三维为旋转，记作 ϕ，实质上是 so(3) 元素①。同时，拓展了 ∧ 符号的含义。在 se(3) 中，同样使用 ∧ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称：

仍使用 ∧ 和 ∨ 符号来指代“从向量到矩阵”和“从矩阵到向量”的关系，以保持和 so(3) 上的一致性。读者可以简单地把 se(3) 理解成“由一个平移加上一个 so(3) 元素构成的向量”（尽管这里的 ρ 还不直接是平移）。同样，李代数 se(3) 亦有类似于 so(3) 的李括号：

二、指数与对数映射

1 SO(3) 上的指数映射

exp(ϕ ∧) 是如何计算的？它是一个矩阵的指数，在李群和李代数中，称为指数映射（Exponential Map）。同样，会先讨论 so(3) 的指数映射，再讨论 se(3) 的情形。

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。

同样地，对 so(3) 中任意一元素 ϕ，亦可按此方式定义它的指数映射：

由于 ϕ 是三维向量，可以定义它的模长和它的方向，分别记作 θ 和 a，于是有 ϕ = θa。这里 a 是一个长度为 1 的方向向量。首先，对于a ∧，有以下两条性质：

利用这两个性质，可以把指数映射写成：

so(3) 实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它们，把 so(3) 中任意一个向量对应到了一个位于 SO(3) 中的旋转矩阵。反之，如果定义对数映射，也能把 SO(3) 中的元素对应到 so(3) 中：

2 SE(3) 上的指数映射

se(3) 上的指数映射形式如下：

ξ 的指数映射左上角的 R 是我们熟知的 SO(3) 中的元素，与 se(3) 当中的旋转部分 ϕ 对应。而右上角的 J 则可整理为（设 ϕ = θa）：

从左上的 R 计算旋转向量，而右上的 t 满足：t = Jρ。由于 J 可以由 ϕ 得到，所以这里的 ρ 亦可由此线性方程解得。

SO(3), SE(3), so(3), se(3) 的对应关系如上图。

三、李代数求导与扰动模型

1 BCH 公式与近似形式

在 SO(3) 中完成两个矩阵乘法时，李代数中 so(3)上发生了什么改变呢？反过来说，当 so(3) 上做两个李代数的加法时，SO(3) 上是否对应着两个矩阵的乘积？如果成立的话，相当于：

exp (ϕ ∧ 1 ) exp (ϕ ∧ 2 ) = exp ( (ϕ1 + ϕ2) ∧ ) .

如果 ϕ1, ϕ2 为标量，那显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。

在研究下式是否成立：ln (exp (A) exp (B)) = A + B ?

很遗憾，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式（BCH 公式）给出。由于它完整的形式较复杂，我们给出它展开式的前几项：

其中 [] 为李括号。当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑 SO(3) 上的李代数 ln (exp (ϕ ∧ 1 ) exp (ϕ ∧ 2 ))∨，当 ϕ1 或ϕ2 为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达：

当对一个旋转矩阵 R2（李代数为 ϕ2）左乘一个微小旋转矩阵 R1（李代数为 ϕ1）时，可以近似地看作，在原有的李代数 ϕ2 上，加上了一项 Jl(ϕ2) −1ϕ1。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在 BCH近似下，分成了左乘近似和右乘近似两种，在使用时须加注意，使用的是左乘模型还是右乘模型。

书以左乘为例。左乘 BCH 近似雅可比 Jl 事实上就是:

假定对某个旋转 R，对应的李代数为 ϕ。我们给它左乘一个微小旋转，记作 ∆R，对应的李代数为 ∆ϕ。那么，在李群上，得到的结果就是 ∆R · R，而在李代数上，根据 BCH近似，为：Jl −1 (ϕ)∆ϕ + ϕ。合并起来，可以简单地写成：

反之，如果在李代数上进行加法，让一个 ϕ 加上 ∆ϕ，那么可以近似为李群上带左右雅可比的乘法：

这将为之后李代数上的做微积分提供了理论基础。同样的，对于 SE(3)，亦有类似的BCH 近似公式：

这里 J l 形式比较复杂，它是一个 6 × 6 的矩阵。由于在计算中不用到该雅可比，故这里略去它的实际形式。

2 SO(3) 李代数上的求导

在 SLAM 中要估计一个相机的位置和姿态，该位姿是由 SO(3) 上的旋转矩阵或 SE(3) 上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为 T。它观察到了一个世界坐标位于 p 的点，产生了一个观测数据 z。那么，由坐标变换关系知：

然而，由于观测噪声 w 的存在，z 往往不可能精确地满足 z = T p 的关系。所以，通常会计算理想的观测与实际数据的误差：

寻找一个最优的 T，使得整体误差最小化：

李代数解决求导问题的思路分为两种：

用李代数表示姿态，然后对根据李代数加法来对李代数求导。
对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。下面我们来讨论这两种思路的异同。

3 李代数求导

首先，考虑 SO(3) 上的情况。假设对一个空间点 p 进行了旋转，得到了 Rp。现在，要计算旋转之后点的坐标相对于旋转的导数，不严谨地记为：

按照导数的定义，有：

第二行的近似为 BCH 线性近似，第三行为泰勒展开舍去高阶项后近似，第四行至第五行将反对称符号看作叉积，交换之后变号。于是，推导了旋转后的点相对于李代数的导数：

4 扰动模型（左乘）

另一种求导方式，是对 R 进行一次扰动 ∆R。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 ∆R 对应的李代数为φ。然后，对 φ 求导，即：

5 SE(3) 上的李代数求导

给出 SE(3) 上的扰动模型，而直接李代数上的求导就不再介绍了。假设某空间点 p 经过一次变换 T（对应李代数为 ξ），得到 T p。现在，给 T 左乘一个扰动∆T = exp (δξ ∧)，设扰动项的李代数为 δξ = [δρ, δϕ] T，那么：

把最后的结果定义成一个算符 ⊙，它把一个齐次坐标的空间点变换成一个 4 × 6的矩阵。

四、实践：Sophus

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff

Sophus 本身亦是一个 cmake 工程。想必你已经了解如何编译 cmake 工程了。Sophus 库只须编译即可，无须安装。

mkdir build
cd build
cmake ..
make

sudo make install

如果遇到了关于fmt的问题：

git clone  https://github.com/fmtlib/fmt.git
cd fmt
mkdir build&&cd build
cmake ..
make
make install

在build文件里面生成useSophus文件运行结果：

2.22045e-16          -1           01 2.22045e-16           00           0           1
SO(3) from quaternion:
2.22045e-16          -1           01 2.22045e-16           00           0           1
they are equal
so3 =      0      0 1.5708
so3 hat=0 -1.5708       01.5708       0      -0-0       0       0
so3 hat vee=      0      0 1.5708
SO3 updated = 0          -1           01           0     -0.00010.0001 2.03288e-20           1
*******************************
SE3 from R,t= 
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
SE3 from q,t= 
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 0   -1.5708         0  0.7853981.5708         0        -0 -0.785398-0         0         0         00         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
SE3 updated = 
2.22045e-16          -1           0      1.00011 2.22045e-16           0           00           0           1           00           0           0           1