【SLAM十四讲--第四讲之李群与李代数】理论知识部分

SLAM十四讲 - - 第四讲之李群与李代数1

《SLAM十四讲》系列文章链接
SLAM十四讲 - - 第三讲之可视化演示程序逐行讲解

---

纯纯新手一个，记录自己学习后的理解，欢迎大家交流讨论。

---

一、为什么要学李群与李代数？

  在SLAM中相机的位姿是未知的，当我们采集到了观测数据后，我们要解决“什么样的相机位姿最符合当前的观测数据”这样一个问题。说人话就是：有了一堆相机观测的数据，如何准确地（最小化误差）计算出相机的位姿。一种典型的方法是：把这个问题转换成优化问题，求解最优的旋转矩阵$R$和平移矩阵$t$，使得观测误差最小化。
  求解旋转矩阵$R$和平移矩阵$t$和我们李群与李代数有什么关系呢？是这样的，在处理优化问题时，我们尝尝会构建一个损失函数（也就是一个误差函数），在最小化这个损失函数的时候，由于旋转矩阵自身是带约束的（正交矩阵：矩阵转置=矩阵的逆），如果直接使用旋转矩阵$R$作为优化变量的话，会引入额外的约束，使优化变的困难。我们可以通过李群与李代数之间的转换关系，把位姿估计问题转化为无约束的优化问题，简化求解的方式。

二、李群与李代数

1.李群

  群（group）是一种集合和一种运算。我们把集合记作A，运算记作·，那么群可以记作G=(A,·)。群需要满足以下条件：

  $1.封闭性:\forall a_1,a_2\in A,a_1\cdot a_2\in A.$
  $2.结合律:\forall a_1,a_2,a_3\in A,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right).$
  $3.么元:\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a.$
  $4.逆:\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0.$

  在第三讲中我们学习到三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3)，他们写起来就像这样：
$$\begin{array}{l}\mathrm{S}\mathrm{O}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\},\\ \\ \mathrm{S}\mathrm{E}(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{S}\mathrm{O}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}.\end{array}$$  不难观察到SO(3)和SE(3)对矩阵加法是不封闭的，而对矩阵乘法是封闭的，也即：
$$\begin{gathered} {\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{S}\mathrm{E}(3) \\ {\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{S}\mathrm{E}(3) \end{gathered}$$  所以严格的说，旋转矩阵和矩阵乘法构成特殊正交群SO(n)，变换矩阵和矩阵乘法构成特殊欧式群SE(3)。
  群结构保证了在群上的运算具有良好的运算。其中，李群指的是具有连续(光滑)性质的群。直观的来说，刚体能够在空间上连续的运动，所以我们说SO(n)和SE(3)是李群。

2.李代数

2.1引出李代数

  这部分公式敲起来麻烦，这里就直接贴上手写的推导。

  简单的来说，就是经过简单推导可以发现旋转矩阵$R$有一个性质：$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}$是一个反对称矩阵，记作：
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}=\mathbf{\varphi }(t{)}^{\wedge }$$  其中，$\varphi (t)$是一个向量，符号^是第三讲中学习的将向量转换为反对称矩阵的符号。
  再经过一阶泰勒展开和简单的化简我们得到：
$$\dot{\mathbf{R}}(t)={\varphi }_0^{\wedge }\mathbf{R}(t)$$  可以发现，在$t$时刻附近，每次对矩阵进行求导只需要左乘一个$\varphi$^。这说明向量$\varphi$反映了$R$的局部导数性质，所以也称它在SO(3)的正切空间上。
  根据初始值和微分方程求解可以得到：$$\mathbf{R}(t)=\exp \left({\varphi }_0^{\wedge }t\right)$$  到这里，我们不得不思考两个问题：

   1. 给定某时刻的矩阵$R$我们就能计算对应的向量$\varphi$，那么$\varphi$向量有什么含义呢？$\varphi$正是对应对应到SO(3)上的李代数$\mathfrak{s}\mathfrak{o}(3)$。
   2. 给定某时刻的向量$\varphi$，矩阵指数$\exp \left({\varphi }^{\wedge }\right)$如何计算呢(也就是计算矩阵$R$)？反过来，给定$R$时是否有相反的运算来计算向量$\varphi$？这正是李群与李代数之间的指数和对数映射。

2.2李代数的定义

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

  $1.封闭性\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}.$
  $2.双线性\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F},有[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}].$
  $3.自反性\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0.$
  $4.雅可比等价\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0.$

  其中二元运算符称为李括号。举个例子，三维向量${\mathbb{R}}^3$上定义的叉乘$\times$是一种李括号(证明过程)，因此$\mathfrak{g}=({\mathbb{R}}^{3\times 3},\mathbb{R},\times )$构成了一个李代数。

2.2.1 李代数$\mathfrak{s}\mathfrak{o}(3)$

  之前提到的$\varphi$，事实上是一种李代数。SO(3)对应的李代数是定义在${\mathbb{R}}^3$上的向量，记作$\varphi$。 根据前面的推导， 每个$\varphi$都可以生成一个反对称矩阵：$$\mathbf{\Phi }={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$  在此定义下，两个向量${\varphi }_1$和${\varphi }_2$的李括号为：(证明过程)
$$\left[{\varphi }_1,{\varphi }_2\right]={\left({\mathbf{\Phi }}_1{\mathbf{\Phi }}_2-{\mathbf{\Phi }}_2{\mathbf{\Phi }}_1\right)}^{\vee }$$  由于向量$\varphi$与反对称矩阵是一一对应的 ，在不引起歧义的清况下，就说$\mathfrak{s}\mathfrak{o}(3)$的元素是三维向量或者三维反对称矩阵，不加区别：
$$\mathfrak{s}\mathfrak{o}=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$
  总结来说，$\mathfrak{s}\mathfrak{o}(3)$的内容是一个由三维向量组成的集合，每个向量对应一个反对称矩阵， 可以用于表达旋转矩阵的导数。它与SO(3)的关系由指数映射给定(具体计算第三节中说)：$$\mathbf{R}=\exp \left({\varphi }_{}^{\wedge }\right)$$

2.2.2 李代数$\mathfrak{s}\mathfrak{e}(3)$

  与之前类似，SE(3)也有对应的李代数$\mathfrak{s}\mathfrak{e}(3)$，位于${\mathbb{R}}^6$空间，
$$\mathfrak{s}\mathfrak{e}(3)=\left\{\xi =\left[\begin{array}{l}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{s}\mathfrak{o}(3),{\xi }^{\wedge }=\left[\begin{array}{ll}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$  $\mathfrak{s}\mathfrak{e}(3)$中元素记作$\xi$，是一个六维向量。前三维为平移(但含义与变换矩阵中的平移不同，分析见后)，记作$\rho$；后三维为旋转，记作$\varphi$，实质上是$\mathfrak{s}\mathfrak{o}(3)$元素。＾符号进行简单的拓展，表示将一个六维向最转换成四维矩阵：
$${\xi }^{\wedge }=\left[\begin{array}{ll}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$  类似的，两个向量${\xi }_1$和${\xi }_2$的李括号为：$$\left[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2\right]={\left({\mathbf{\xi }}_1^{\wedge }{\mathbf{\xi }}_2^{\wedge }-{\mathbf{\xi }}_2^{\wedge }{\mathbf{\xi }}_1^{\wedge }\right)}^{\vee }$$

3.指数与对数映射

  矩阵指数问题，在李群和李代数中称为指数映射。同样的，这里面的公式比较琐碎，这里只放简单的框架性推导，其中涉及的结论性公式的证明过程见此链接。

3.1 SO(3)与$\mathfrak{s}\mathfrak{o}(3)$

3.2 SE(3)与$\mathfrak{s}\mathfrak{e}(3)$

  如果不想看推导过程，只要能够看懂带图就行。这个图总结的非常全面！高博真是太厉害了！

4.李代数求导与扰动模型

4.1 基础

  前面我们说：我们可以通过李群与李代数之间的转换关系，把位姿估计问题转化为无约束的优化问题，简化求解的方式。而在优化过程中，对损失函数进行求导是非常必要的(第6讲会介绍)。求导过程中的复合求导不可避免的要处理矩阵乘法，那么当在 SO(3) 中完成两个矩阵乘法时，李代数$\mathfrak{s}\mathfrak{o}(3)$发生了什么改变呢？
  这里直接给出结论，两个李代数指数映射的由Baker-Campbell­Hausdorff公式(BCH公式）给出。老规矩，直接上手写的：

  推导中，左乘BCH近似雅可比${\mathbf{J}}_l$形式：
$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$  它的逆为：$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\frac{\mathbf{\theta }}{2}{\mathbf{a}}^{\wedge }$$  右乘雅可比仅需要对自变量取负号即可：${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$。
  对于李代数$\mathfrak{s}\mathfrak{e}(3)$，也有近似的BCH公式，由于其雅克比矩阵形式复杂，实际中未使用，所以跳过。

4.2 李代数导数与扰动模型

  下面来讨论一个带有李代数的函数，以及关于该李代数求导的问题。直接上手写笔记。

  按照思路一：李代数求导，推导过程见此链接。

  按照思路二：扰动模型(左乘)，推导过程见此链接。

  SE(3)上的李代数求导直接给结论，推导过程见此链接。