【SLAM14讲CH4】李群与李代数 笔记

CH4 李群与李代数 笔记

问题背景

在SLAM中位姿是未知的，我们需要解决什么样的相机位姿最符合当前的观测数据这样的问题。比较典型的解决方式是，把它构建成一个优化问题，求解最优的R，t使误差最小。在优化问题的求解中，常常需要使用导数，但旋转矩阵关于加法的运算不封闭（两个旋转矩阵相加就不是旋转矩阵了，两个矩阵相乘才表示两次旋转的复合）使得导数难以定义，于是需要引入李群和李代数来解决求导问题。

群和李群的概念

群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 A，运算记作 ·， 那么群可以记作 G = (A, ·)。旋转矩阵集合和矩阵乘法构成群，变换矩阵和矩阵乘法也构成群。

旋转矩阵群被称为特殊正交群 SO(n) ,n维欧氏变换被称为特殊欧式群 SE(n) 。

李群是指具有连续（光滑）性质的群。像整数群 Z 那样离散的群没有连续性质，所以不是李群。而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。每个李群都有对应的李代数。

关于李代数 引入

考虑一个相机随时间变化不断旋转，由于旋转矩阵是正交阵，它满足：
$$\mathbf{R}(t){\mathbf{R}}^{\mathbf{T}}(t)=\mathbf{I}$$
两边对时间求导后整理得：
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T{)}^T$$
可见$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T$是一个反对称矩阵

***反对称矩阵：***$A^T=-A$ 每个向量有一个与之对应的反对称矩阵，两个向量的叉积可转化为前一个向量的反对称矩阵与后一个向量的乘法 $a\times b=a^{\wedge }b=Ab$

我们用 $\wedge$ 表示向量到其对应的矩阵的转化，用 $\vee$ 表示矩阵到向量的转化

由于$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T$是一个反对称矩阵，我们可以找到一个三维向量$\varphi (t)$与之对应
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=\varphi (t{)}^{\wedge }$$
等式两边右乘 R(t)，由于 R 为正交阵，有：
$$\dot{\mathbf{R}}(t)=\varphi (t{)}^{\wedge }\mathbf{R}(t)$$
设在$t_0$附近，$\varphi$保持常数$\varphi (t_0)={\varphi }_0$,则有
$$\dot{\mathbf{R}}(t)={\varphi }_0^{\wedge }\mathbf{R}(t)$$
上式是一个关于 R 的微分方程，当初始值$\mathbf{R}(0)=\mathbf{I}$时，解之，得：
$$\mathbf{R}(t)=exp({\varphi }_0^{\wedge }t)$$
我们看到，旋转矩阵 R 与另一个反对称矩阵${\varphi }_0$通过指数关系发生 了联系。也就是说，当我们知道某个时刻的 R 时，存在一个向量$\varphi$，它们满足这个矩阵指数关系。

李代数定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。李代数由一个集合 V，一个数域 F 和一个二元运算 [, ] 组成，其中二元运算被称为李括号。李括号表达了两个元素的差异（自己与自己运算结果为0）。作为例子，三维向量 上定义的叉积 × 是一种李 括号，因此 g = (R3 , R, ×) 构成了一个李代数。

李代数 so(3) se(3)

so(3)

SO(3) 对应的李代数是定义在 R3 上的向量，我们记作 $\varphi$ 。每个 $\varphi$ 都可以生成一个反对称矩阵记做$\Phi$。

两个向量 ${\varphi }_1$, ${\varphi }_2$ 的李括号为:$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$

se(3)

se(3)位于六维空间中，将se(3)元素记为$\xi$，$\xi$是一个六维向量，前三维为平移，记做$\rho$，后三维为旋转记做$\varphi$（so(3)元素）。在这里^符号表示将一个刘伟向量转化为一个四维矩阵。
$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]$$
其李括号与so(3)类似：$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$

指数映射与对数映射

SO(3)上的指数映射

根据矩阵指数泰勒展开，可以将$exp({\varphi }^{\wedge })$写成${\varphi }^{\wedge }$的1到无穷阶项求和的形式，再利用单位向量反对称矩阵的性质对高阶项进行化简，最终得到罗德里格斯公式
$$exp(\theta {\mathbf{a}}^{\wedge })=cos(\theta )(I)+(1-cos(\theta ){\mathbf{a}\mathbf{a}}^T)+sin(\theta ){\mathbf{a}}^{\wedge }$$
其中$\theta$为$\varphi$的模长，$\mathbf{a}$为与$\varphi$方向相同的单位向量，即$\varphi =\theta \mathbf{a}$
由指数映射的公式我们可以将so(3)上的一个向量对应到SO(3)中的一个矩阵。将SO(3)中的旋转矩阵对应到so(3)上的向量通常采用第三讲中由矩阵的迹求解的方式。
注意：可能存在多个旋转向量对应一个旋转矩阵（旋转角有周期性）

SE(3)上的指数映射

其中右上角$\mathbf{J}$雅克比矩阵整理为（$\varphi =\theta \mathbf{a}$）

对数映射：由T左上角的R计算$\varphi$,由$\varphi$计算J，由t和J计算$\rho$。

李代数求导

导数是优化过程中十分必要的信息，使用李代数的一大目的即解决求导问题。在标量中，矩阵通过指数可以将乘法与加法相互转化，但在矩阵中并不成立。由BCH公式知，当矩阵指数相乘时会产生一些由李括号组成的小量。

考虑SO(3)李代数上的$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }$当${\varphi }_1$或${\varphi }_2$为小量时，高次项被忽略，此时BCH公式有近似线性表达

以第一个近似为例。该式告诉我们，当对一个旋转矩阵 $R_2$（李代数为 ${\varphi }_2$）左乘一个 微小旋转矩阵$ R_1$（李代数为 ${\varphi }_1$）时，可以近似地看作，在原有的李代数 $ϕ_2 $上，加上了一 项 $J_l(\varphi 2{)}^{-1}\varphi 1$。
同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在 BCH 近似下，分成了左乘近似和右乘近似两种，在使用时我们须加注意，使用的是左乘模型还是右乘模型。

左乘雅克比：

逆矩阵：

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

BCH 近似的意义

假定对某个旋转$R$，对应的李代数为 $\varphi$。我们给它左乘一个微小旋转，记作 $∆R$，对 应的李代数为$ ∆ϕ$。那么，在李群上，得到的结果就是 $∆R\cdot R$，而在李代数上，根据 BCH 近似，为：$J^{-1}(\varphi )∆\varphi +\varphi$。

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

对于SE(3)同样

SO(3)上李代数求导

思路：

1. 用李代数表示姿态，然后对根据李代数加法来对李代数求导。（求导模型）
2. 对李群左乘或右乘微小扰动，然后对该扰动求导。（扰动模型）

导数模型：

首先，考虑 $SO(3)$ 上的情况。假设我们对一个空间点$ p$ 进行了旋转，得到了$ Rp$.由于$ SO(3) $没有加法，所以该导数无法按照导数的定义进行计算。设 $R$ 对应的李代数为$ ϕ$，我们转而计算：$\frac{\partial(exp(\phi^{\land})p)}{\partial\phi}$,得到

这里仍然含有形式比较复杂的$ J_l$，我们不太希望计算它。扰动模型则提供了更简单的导数计算方式。

扰动模型

另一种求导方式，是对$ R$ 进行一次扰动$ ∆R$。我们以左扰动为例。设左扰动$ ∆R $对应的李代数为$ φ$。然后，对$ φ $求导。该求导更加简单，最终得到
$$\frac{\partial (Rp)}{\partial \phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }$$

SE(3)上李代数求导

最后，我们给出$ SE(3)$ 上的扰动模型。假设 某空间点$ p$ 经过一次变换$ T$（对应李代数为$ ξ$），得到$ T p$。现在，给 $T$ 左乘一个扰动$ ∆T = exp (δξ ^{∧})$，我们设扰动项的李代数为$ δξ = [δρ, δϕ]^{ T}$，那么
$$\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \xi }=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (Tp{)}^{⨀}$$