视觉SLAM十四讲：李群李代数

在上一讲中我们介绍了视觉SLAM十四讲的前3讲：基本框架和刚体运动的描述方式（R矩阵，T矩阵）。

面临的问题：

当我们对刚体的运动进行表示了之后，接下来要解决的问题是对得到的表示进行估计和优化。因为在 SLAM 中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的 R; t，使得误差最小化。

解决途径（李群李代数的引入）

但是鉴于旋转矩阵自身是带有约束的（正交且行列式为 1），所以它们作为优化变量时，会引入额外的约束，使优化变得困难。于是聪明的研究人员通过李群——李代数间的转换关系，把位姿估计问题变成了无约束的优化问题，以此来简化求解方式。

我们先来了解一下封闭的概念：
1属于整数，2也属于整数，1+2的结果还是属于整数。对于所有整数都有此属性，于是就称：整数对于加法是封闭的。

群的概念：
一种集合加上一种运算的代数结构，这个集合对于这个个运算是封闭的。群这种结构保证了在群上的运算具有良好的性质。我们把集合记作 A，运算记作 ·，那么群可以记作 G = (A; ·)。群要求这个运算满足以下几个条件：

此处有一些常见的群：

主角登场：李群李代数

李群是指具有连续（光滑）性质的群。像整数群 Z 那样离散的群没有连续性质，所以不是李群。而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。由于 SO(3) 和 SE(3) 对于相机姿态估计尤其重要，我们主要讨论这两个李群。

每个李群都有对应的李代数，李代数描述了李群的局部性质。

李代数的引出在《视觉SLAM十四讲》中有较好的讲述，此处因为懒就不在记录了。

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

举个例子，三维向量 R3 上定义的叉积 × 是一种李括号，g = (R3; R; ×) 构成了一个李代数。

so(3)

特殊正交群 SO(n) 也就是所谓的旋转矩阵群，其中 SO(2) 和 SO(3) 最为常见。

SO(3) 对应的李代数是定义在 R^3上的向量，我们记作 ϕ。ϕ 生成的反对称矩阵：

so(3) 是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定：

se(3)

特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换，如 SE(2) 和 SE(3)。对于 SE(3)，它也有对应的李代数 se(3)，se(3) 位于 R6 空间。

我们把每个 se(3) 元素记作 ξ，它是一个六维向量。前三维为平移，记作 ρ；后三维为旋转，记作 ϕ，实质上是 so(3) 元素‹。同时，在 se(3) 中，使用 ^ 符号表示将一个六维向量转换成四维矩阵。

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

指数与对数映射

在前面的文章中，我们有过这样的话语：

李代数so(3) 是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定。

本小节我们便来探讨指数和对数的运算：

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

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

将这个式子展开，得到了：

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

反之，如果定义对数映射，我们也能把 SO(3) 中的元素对应到 so(3) 中：

注意：
**指数映射只是一个满射。**这意味着每个SO(3) 中的元素，都可以找到一个 so(3) 元素与之对应；但是可能存在多个 so(3) 中的元素，对应到同一个 SO(3)。至少对于旋转角 θ，我们知道多转 360 度和没有转是一样的——它具有周期性。但是，如果我们把旋转角度固定在 ±π 之间，那么李群和李代数元素是一一对应的。

se(3)中的映射形式为：

李代数求导及其扰动模型

####BCH 公式与近似形式
使用李代数的一大动机是为了进行优化，而在优化过程中导数是非常必要的信息。清楚了 SO(3) 和 SE(3)上的李群与李代数关系，但是，当我们在 SO(3) 中完成两个矩阵乘法时，李代数中 so(3)上发生了什么改变呢？即了解李群和李代数之间的关系。

首先是Baker-Campbell-Hausdorff 公式（BCH 公式）给出了两个李代数指数映射乘积的完整形式。如下式：

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

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

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

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

于是，我们便在李群的加法和李代数的乘法之间建立了一个联系。

####李代数上的求导

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

那么，由坐标变换关系知： z = T p + w

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

假设一共有 N 个这样的路标点和观测，于是就有 N 个上式。相当于要寻找一个最优的 T，使得整体误差最小化：

求解此问题，需要计算目标函数 J 关于变换矩阵 T 的导数。这里重点要说的是， 我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：

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

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。下面来一一分析：

李代数求导

我们以so(3)来讨论，假设我们对一个空间点 p 进行了旋转，得到了 Rp。现在，要计算旋转之后点的坐标相对于旋转的导数，记为：

设 R 对应的李代数为ϕ，我们转而计算：

计算该式，可得 −(Rp)^Jl。

旋转后的点相对于李代数的导数：

不过，由于这里仍然含有形式比较复杂的 Jl，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

扰动模型

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

计算后结果为：−(Rp)^ 。

可见，扰动模型相比于直接对李代数求导，省去了一个雅可比 Jl 的计算。这使得扰动模型更为实用。

到了此处，有可能很多同学已经被绕晕了，我也是，，，所以，咱们来理理：

本讲引入了李群 SO(3) 和 SE(3)，以及它们对应的李代数 so(3) 和 se(3)。我们介绍了位姿在它们上面的表达和转换，然后通过 BCH 的线性近似，我们可以对位姿进行求导和扰动了。这给之后讲解位姿的优化打下了理论基础，因为我们需要经常的对某一个位姿的估计值进行调整，使它对应的误差减小。

使用李代数的一大动机是为了对位姿进行优化。