《视觉SLAM十四讲》学习笔记：第4讲 李群与李代数

《视觉SLAM十四讲》学习笔记：第4讲 李群与李代数

前言：本学习笔记将记录《视觉SLAM十四将》中一些重要的知识点，并对书中一些比较难的知识点添加上一些笔者个人的理解，以供笔者本人复习并与各位同学一起交流学习。本笔记结构将与原书结构一致，如果某一目录下面没有任何笔记，则代表笔者认为该小节内容相对来说没有过多重点知识。

本讲主要解决问题
理论：
1.理解李群与李代数的概念，掌握 $SO(3)，SE(3)$ 与对应李代数的表示方式
2. 理解 BCH 近似的意义。
3. 学会在李代数上的扰动模型。
实践：
1.使用 Sophus 对李代数进行运算。

本章意义：由于在SLAM中位姿是未知的，因此需要解决什么样的相机位姿最符合当前观测数据这个问题，其中一个典型的方式就是把它构建成一个优化问题，求解最优的$R,t$，使得误差最小化。但是，如果直接将旋转矩阵/变换作为优化变量时，会引入额外的约束（旋转矩阵/变换本身带有约束，即旋转部分正交且行列式为1)，因此会给优化带来许多麻烦。而通过李群——李代数间的转换关系，把位姿估计变成无约束的优化问题，简化求解方式。

4.1 李群李代数基础

4.1.1 群
1.群的定义，群是一种集合加上一种运算的代数结构。将集合记作$A$，运算记作$\cdot$，那么群可以记作$G=(A,\cdot )$。群要求这个运算满足以下几个条件：

可以验证，旋转矩阵集合和矩阵乘法构成群，同样变换矩阵和矩阵乘法也构成群。
2.李群：李群是指具有连续（光滑）性质的群。本章重点关注李群$SO(3)$和$SE(3)$。
4.1.2 重新排版说明
本章第一节剩余部分与本章第二节排版较原书略有不同，因此，可能会出现一些奇奇怪怪的节名
但是此处，我们首先需要明确，本章最终想要获得的，是典型的李群$SO(3)$，$SE(3)$分别和李代数$so(3)$，$se(3)$相互之间的转换关系
4.1.3 李代数的定义
每个李群都有与之对应的李代数。李代数描述了李群的局部性质（至于）。通用的李代数的定义如下：
李代数有一个集合$V$，一个数域$F$和一个二元运算[,]组成。如果它们满足以下几条性质，称$(V,F,[,])$为一个李代数，记作$g$。

4.1.3 李代数so(3)
定义李代数$so(3)$:

其对应的李括号为：

可以推导，李代数$so(3)$与$SO(3)$的关系由指数映射给定：

其中，$\varphi$满足：

推导过程如下：
对于任意旋转矩阵$R\in SO(3)$，有：

若认为$R$会随时间连续地变化，比如相机的旋转，那么有：

等式两边对时间求导，得到:

整理得到：

因此，可以看出$\dot{R}(t)R(t{)}^T$是一个反对称矩阵。
这里给出一个符号$\vee$该符号表示将反对称矩阵变为向量：

由于$\dot{R}(t)R(t{)}^T$是一个反对称矩阵，可以找到一个三维向量$\varphi (t)\in R^3$与之对应，于是有：

等式两边右乘$R(t)$，有：

可以看到，每对旋转矩阵求一次导数，只需左乘一个$\varphi (t{)}^{\wedge }$矩阵即可。（$\varphi (t{)}^{\wedge }$是和旋转矩阵有关的，而不是说上式的二阶导就是再加一个一模一样的$\varphi (t{)}^{\wedge }$）。
假设$t_0$等于0，并设此时旋转矩阵为$R(0)=I$，将$R(t)$在0附近进行一阶泰勒展开：

根据上式可以发现，在$t_0$附近，设$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$。那么有：

知道初始值$R(0)=I$,求解上面的微分方程，得到：

（上面的推导由作者给出，个人感觉是有些奇怪的，其一，根据后面的结果，${\varphi }_0^{\wedge }$应该会随t变化，其二，罗德里格斯公式并没有$R(0)=I$这样的限制，笔者数学并没有学到这个部分，目前还在查询相关资料，欢迎各位明白这个公式推导的大佬解释）。
4.1.4 李代数se(3)
$se(3)$位于$R^6$空间中：

我们把每个$se(3)$元素记作$\xi$，它是一个六维向量。前三维为平移，记作$\rho$；后三维为旋转，记作$\varphi$，实质上是$so(3)元素$。同时，此处符号$\wedge$将一个六维向量转换成思维矩阵，但这里不再表示反对称。
李括号定义如下：

4.2 指数与对数映射

4.2.1 SO(3)上的指数映射
指数映射定义如图所示（在收敛的情况下有结果）：

同样地，对$so(3)$中任意一元素$\varphi$，可以定义它的指数映射：

下面进一步进行推导，由于$\varphi$是三维向量，可以定义它的模长和方向，分别记作$\theta$和$a$，于是有$\varphi =\theta a$。这里$a$是一个长度为1的方向向量。首先，对于$a^{\wedge }$，有以下两条性质：

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

最终推导得到的公式即为罗德里格斯公式：

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

但是，通常不用泰勒展开去计算对数映射，而是用第3讲中的方法利用迹的方法分别求解转角和转轴。
指数映射只是一个满射，可能存在多个$so(3)$中的元素，对应到同一个$SO(3)$。但是，如果把旋转角度固定在正负$\pi$之间，那么李群和李代数元素是一一对应的。
由此，可以发现旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。
4.2.2 SE(3)上的指数映射
$se(3)上的指数映射形式如下:$

推导如同$so(3)$。从结果上看，$\xi$的指数映射左上角的$R$是$SO(3)$中的元素，与$se(3)$当中的旋转部分$\varphi$，右上角的$J$则整理为（设$\varphi =\theta a$）:

由此可见，平移部分经过指数映射之后，发生了一次以$J$为系数矩阵的线性变换。
同样的，也可以推得对数映射，但是通过这个方式得到：

由于$J$可以由$\varphi$得到，所以这里的$\rho$亦可由此线性方程解得。
由此，李群、李代数的相互的转换关系已经完全结束。

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式
两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hausdorff 公式（BCH公式）给出。其展开式的前几项如下：

其中$[]$为李括号。
特别地，考虑$SO(3)$上的李代数$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }{)}^{\vee }$，当${\varphi }_1$或${\varphi }_2$为小量时，小量二次以上的项都可以被忽略掉。此时，$BCH$拥有线性近似表达：

其中$J_l({\varphi }_2)$实际上代表了由${\varphi }_2$张成的左乘$BCH$近似雅可比阵，实际上也就是：

它的逆为：

而右乘雅可比仅需要对自变量取负号即可：

下面阐述$BCH$近似的意义：
假设对某个旋转$R$，对应的李代数为$\varphi$。给它左乘一个对应的李代数为$\Delta \varphi$的微小旋转$\Delta R$，那么，根据$BCH$近似，可以得到：

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

同样，对于$SE(3)$，也有类似的$BCH$近似公式：

但是这里的$J_l$形式比较复杂，但基本不会用到其实际形式，这里略去。
4.3.2 SO(3)李代数上的求导
李代数求导问题的引出：
在SLAM中，我们要估计一个相机的位置和姿态，该位姿是由$SO(3)$上的旋转矩阵或$SE(3)$上的变换矩阵描述的。假设在某个时刻，机器人的位姿为$T$，它观察到了一个世界坐标位于$p$的点，产生了一个观测数据$z$。那么，由坐标变换关系得：

这里$w$是观测噪声。
由于观测噪声$w$的存在，$z$往往不可能精确地满足$z=Tp$的关系，因此会计算理想的观测与实际数据的误差:

假设一共有$N$个这样的路标点和观测，于是就有$N$个上式。那么，对该机器人位姿的估计，相当于是寻找一个最优的$T$,使得整体误差最小化：

而求解这个问题，就需要计算目标函数$J$关于变换矩阵$T$的导数。
注意：这里重点要说的是， 我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而， $SO(3)$, $SE(3)$ 上并没有良好定义的加法，它们只是群。如果我们把 $T$ 当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种:
1.用李代数表示姿态，然后对根据李代数加法来对李代数求导。
2.对李群左乘或右乘微小扰动。然后对该扰动求导，称为左扰动和右扰动模型。（注意，扰动模型下的求导定义和数学上的微分定义不太一样。）
4.3.3 李代数求导
首先，考虑$SO(3)$上的情况。假设我们对一个空间点$p$进行了旋转，得到了$Rp$。现在，要计算旋转之后点的坐标相对于旋转的导数，记为（这里只是作为一个记号，其并不符合微分的数学含义）：

由于$SO(3)$没有加法，所以该导数无法按照导数的定义进行计算。设$R$对应的李代数为$\varphi$，转而计算：

根据导数的定义，有：

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

不过，由于这里仍然含有形式较为复杂的$J_l$，因此一般会用扰动模型。
4.3.4 扰动模型（左乘）
另一种求导方式，是对$R$进行一次扰动$\Delta R$，这个扰动可以乘在右边，最后结果会有一点儿微小的差异，以左扰动为例。（一定记住，这里的微分应该按照数学上的概念去理解，就是上面有一个微小变换，下面也有一个微小变换。而别用数学上面的定义来理解）。设左扰动$\Delta R$对应的李代数为$\phi$。然后，对$\phi$求导，即：

由此式得到：

可见，扰动模型相比于直接对李代数求导，省去了一个雅可比$J_l$的计算。因此扰动模型更为实用。
4.3.5 SE(3)上的李代数求导
仍然利用扰动模型，假如某空间点$p$经过一次变换$T$（对应李代数为$\xi$），得到$Tp$。现在，给$T$左乘一个扰动，$\Delta T=exp(\delta {\xi }^{\wedge })$，设扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$，那么：

最后的结果定义成一个算符$\odot$，它把一个$4\times 6$的矩阵。

4.4 实践： Sophus

直接make这个库会出现报错，根据报错信息，解决方法参考博客：
https://blog.csdn.net/shanpenghui/article/details/104297120
同时，$include$的文件需要改成：

否则会出现报错。

4.5 相似变换群与李代数

在单目SLAM中使用$SE(3)$表示位姿，那么由于尺度不确定性与尺度飘逸，整个SLAM过程中的尺度会发生变化，这在$SE(3)$中未能体现出来。因此，在单目情况下一般会显式地把尺度因子表达出来。用数学语言来说，对于位于空间的点$p$，在相机坐标系下要经过一个相似变换，而非欧氏变换：

在相似变换中，我们把尺度$s$表达了出来。它同时作用在$p$的三个坐标之上，对$p$进行了一次缩放。相似变换亦对矩阵乘法构成群，称为相似变换群$Sim(3)$：

同样地，$Sim(3)$也有对应的李代数、指数映射、对数映射等等。李代数$sim(3)$元素是一个其为向量，它的前六维与$se(3)$相同，最后多了一项$\sigma$。

关联$Sim(3)$和$sim(3)$的仍是指数映射和对数映射。指数映射为：

其中$J_S$形式为：

通过指数映射，我们能够找到李代数与李群的关系。对于尺度因子，可以看到李群中的$s$即为李代数中$\sigma$的指数函数。
同样的，$Sim(3)$的$BCH$近似与$SE(3)$是类似的，也存在微分模型和扰动模型两种方式，而扰动模型较为简单。设给予$Sp$左侧一个小扰动，并求$Sp$对于扰动的导数。因为$Sp$四维的齐次坐标，$\zeta$是七维向量，该导数应该是$4\times 7$的雅可比，记$Sp$的前三维组成向量$q$，那么：