视觉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 的线性近似，我们可以对位姿进行求导和扰动了。这给之后讲解位姿的优化打下了理论基础，因为我们需要经常的对某一个位姿的估计值进行调整，使它对应的误差减小。 使用李代数的一大动机是为了对位姿进行优化。