【视觉SLAM十四讲学习笔记】第四讲——李群与李代数基础

专栏系列文章如下：
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数
【视觉SLAM十四讲学习笔记】第三讲——Eigen库

引入

之前我们了解了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角四元数等若干种方式。我们重点介绍了旋转的表示，但是在 SLAM 中，除了表示，我们还要对它们进行估计和优化。因为在SLAM 中位姿是未知的，而我们需要解决形如“什么样的相机位姿最符合当前观测数据”这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的R，t，使得误差最小化。
如前所言，旋转矩阵自身是带有约束的(正交行列式为1)。它们作为优化变量时，会引人额外的约束，使优化变得困难。通过李群一李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

上面的引入摘自原书。学习李群李代数必须要明确一件事（其实我感觉学啥都是一样），运用李群李代数的目的到底是什么，李群李代数用于表达什么东西。

举个例子：比如你拿着相机一边移动一边拍，假设某个时刻相机的位姿是T，它观察到一个在世界坐标系中的一个空间点p，并在相机上产生了一个观测数据z，那么有

z = Tp + noise

noise是观测噪声。那么观测误差就是

e = z - Tp

所以，为了使我们获得更准确的结果，我们的目的就是使得观测误差最小。

假设我们总共有N个这样的三维点p和观测值z，那么我们的目标就是寻找一个最佳的位姿T，使得整体误差最小化，也就是：

求解此问题，就是求目标函数J对于变换矩阵T的导数。这个是我们现在的目的。在继续往下走之前，我们还是得先搞明白群是个什么东西。不过，我可以先把结论抛出来，读者可以带着这个概念往下看：

可以通过对李代数求导来间接的对变换矩阵求导

李群与李代数基础

我们说三维旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧式群SE(3)。

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

可以说两种矩阵并没有良好定义的加法，或者通常矩阵加法对这两个集合不封闭。相对地，它们只有一种较好的运算：乘法。SO(3)和SE(3)关于乘法是封闭的：

同时，我们也可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好的）运算的集合，我们称之为群。

群

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

我们知道旋转矩阵R本身有一定的约束：

两个旋转矩阵R1+R2的结果就不能满足上述约束了，但是R1·R2满足。此外，旋转矩阵还满足结合律：R1·R2=R2·R1，还有幺元是单位矩阵I，也有逆矩阵满足R乘以R的逆等于幺元（单位阵）。容易验证，旋转矩阵集合和矩阵乘法构成群，同样，变换矩阵和矩阵乘法也构成群（因此才能称它们为旋转矩阵群和变换矩阵群）。其他常见的群包括整数的加法（Z,+），去掉0后的有理数的乘法（幺元为1）（Q\0,·）等等。矩阵中常见的群有：

• 一般线性群GL(n) 指n×n的可逆矩阵，它们对矩阵乘法成群。
• 特殊正交群SO(n) 旋转矩阵群，其中SO(2)和SO(3)最常见。
• 特殊欧式群SE(n) n维欧氏变换，如SE(2)和SE(3)。

李群是指具有连续（光滑） 性质的群。我们能够直观地想象一个刚体能够连续地在空间中运动，所以SO(n)和SE(n)在实数空间上是连续的，因此它们都是李群。即旋转矩阵和乘法就构成了旋转矩阵群，变换矩阵和乘法也构成了变换矩阵群。反观整数群Z，那样离散的群没有连续性质，所以不是李群。

李代数的引出

前文提到：相机在三维空间中是连续的旋转或者变换的。而我们SLAM目的就是优化求解相机的这个最佳的位姿T（变换矩阵），优化方法一般都采用迭代优化的方法，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分（导数）。先以SO(3)空间中的旋转矩阵R为例：

考虑任意旋转矩阵R，R为正交矩阵，满足：

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

等式两边对时间求导（左边为分部积分，右边为对常数求导），得到

移项整理得

可以看出，等号左边是一个反对称矩阵。我们在介绍叉积时，引入了^符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，我们也能找到唯一与之对应的向量。把这个运算用符号∨（反对称符号）表示：

反对称矩阵定义如下：
如果一个3X3的矩阵满足如下式子

那么A就是反对称矩阵。
转置之后，对角线元素都为0。对于非对角线元素，其值等于转置后元素取负号。所以反对称矩阵中非对角线元素只用3个元素就可以表示。也就是说反对称矩阵只有三个自由度。这样我们就可以把一个三维向量和一个三维矩阵建立对应关系。所以，我们可以用^符号来表示一个三维向量α与三维矩阵A的对应关系。

通过这个符号，我们把向量和矩阵建立了对应关系。

由于等号左边是一个反对称矩阵，我们可以找到一个三维向量φ(t)∈R³ 与之对应：

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

可以看到，每对旋转矩阵求一次导数，只需左乘一个φ^(t)矩阵即可。换句话说，旋转矩阵的微分是一个反对称矩阵左乘它本身。

考虑t_0=0时，设此时旋转矩阵为R(0)=I。按照导数定义，可以把R(t)在t=0附近进行一阶泰勒展开：

我们看到φ反映了R的导数性质，故称它在SO(3)原点附近的正切空间上。同时在t_0附近，设φ保持为常数φ(t_0)=φ_0。那么有

上式是一个关于R的常微分方程，而且有初始值R(0)=I，解得

这说明在t=0附近，旋转矩阵可以由exp(φ^ _0 t)计算出来。我们看到，旋转矩阵R与另一个反对称矩阵φ^_0t通过指数关系发生了联系。但是矩阵的指数是什么呢？我们由两个问题需要澄清：

1. 给定某时刻的R，我们就能求得一个φ，它描述了R在局部的导数关系。与R对应的φ有什么含义呢？我们说，φ正是对应到SO(3)上的李代数so(3)；
2. 其次，给定某个向量φ时，矩阵指数exp(φ^)如何计算？反之，给定R时，能否有相反的运算来计算φ？事实上，这正是李群与李代数间的指数/对数映射。

下面，我们来解决这两个问题。

李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间，它描述了李群局部的导数。可以理解为：李代数对应了李群的导数。一般的李代数定义如下：
李代数由一个集合V、一个数域F和一个二元运算[,]组成。如果它们满足以下几条性质，则称(V,F,[,])为一个李代数，记作g。

其中二元运算被称为李括号。相对于群中的较为简单的二元运算，李括号表达了两个元素的差异。他不要求结合律，而要求元素和自己做李括号之后为零的性质。作为例子，三维向量R³上定义的叉积×是一种李括号，因此g=(R³, R , × )构成了一个李代数。

结论：对于某个时刻的R(t)（李群空间），存在一个三维向量φ=（φ1，φ2，φ3）（李代数空间），用来描述R在t时刻的局部的导数。

李代数so(3)

之前提到的φ事实上是一种李代数。SO(3)对应的李代数是定义在R³的向量，我们记作φ。根据前面的推导，每个φ都可以生成一个反对称矩阵：

由于向量φ与反对称矩阵是一一对应的，在不引起歧义的情况下，就说so(3)的元素是三维向量或者三位反对称矩阵，不加区别：

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

李代数se(3)

对于SE(3)，它也有对应的李代数se(3)，与so(3)相似，se(3)位于R⁶ 空间中：

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

我们仍使用^和∨符号指代“从向量到矩阵”和“从矩阵到向量”的关系，以保持和so(3)上的一致性。他们依旧是一一对应的。