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

289
0
2020年11月29日 09时18分

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

 

面临的问题:

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

 

解决途径(李群李代数的引入)

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

 

我们先来了解一下封闭的概念:

1属于整数,2也属于整数,1+2的结果还是属于整数。对于所有整数都有此属性,于是就称:整数对于加法是封闭的。

群的概念:

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

 

视觉SLAM十四讲:李群李代数插图

 

此处有一些常见的群:

 

视觉SLAM十四讲:李群李代数插图(1)

 

主角登场:李群李代数

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

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

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

 

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

 

视觉SLAM十四讲:李群李代数插图(2)

 

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

 

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

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

 

视觉SLAM十四讲:李群李代数插图(3)

 

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

 

视觉SLAM十四讲:李群李代数插图(4)

 

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

 

视觉SLAM十四讲:李群李代数插图(5)

 

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

 

视觉SLAM十四讲:李群李代数插图(6)

 

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

 

视觉SLAM十四讲:李群李代数插图(7)

 

指数与对数映射

在前面的文章中,我们有过这样的话语:

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

 

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

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

 

视觉SLAM十四讲:李群李代数插图(8)

 

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

 

视觉SLAM十四讲:李群李代数插图(9)

 

将这个式子展开,得到了:

 

视觉SLAM十四讲:李群李代数插图(10)

 

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

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

 

视觉SLAM十四讲:李群李代数插图(11)

 

注意:

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

 

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

 

视觉SLAM十四讲:李群李代数插图(12)

 

李代数求导及其扰动模型

BCH 公式与近似形式

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

 

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

 

视觉SLAM十四讲:李群李代数插图(13)

 

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

 

视觉SLAM十四讲:李群李代数插图(14)

 

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

 

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

 

视觉SLAM十四讲:李群李代数插图(15)

 

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

视觉SLAM十四讲:李群李代数插图(16)

 

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

 

李代数上的求导

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

 

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

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

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

视觉SLAM十四讲:李群李代数插图(17)

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

 

  1. 用李代数表示姿态,然后对根据李代数加法来对李代数求导。
  2. 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。第一种方式对应到李代数的求导模型,而第二种则对应到扰动模型。下面来一一分析:

 

李代数求导

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

视觉SLAM十四讲:李群李代数插图(18)

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

视觉SLAM十四讲:李群李代数插图(19)

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

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

视觉SLAM十四讲:李群李代数插图(20)

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

 

扰动模型

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

视觉SLAM十四讲:李群李代数插图(21)

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

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

 

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

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

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

发表评论

后才能评论