视觉SLAM十四讲 第四讲 李代数和李群

1. 什么是群

群（group）是一种集合加上一种运算的代数结构。集合记作A，运算记作 · ，那么群可以记作$G=(A,\cdot )$。群需要满足四个条件（封结幺逆，凤姐要你）

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,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
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$

2. 什么是李群

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

关键是连续的群。自己可以简单记住特殊正交群SO(3)、旋转矩阵是李群，旋转向量是李代数。
​

3. 什么是李代数

每个李群都有预支对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。一般的李代数定义如下：
​

李代数由一个集合$\mathbb{V}$、一个数域$\mathbb{F}$和一个二元运算$[,]$组成，这个二元运算被称为李括号。 对于李代数$g=(\mathbb{V},\mathbb{F},[,])$，需要满足四条性质（封双自雅，风霜之牙）

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

对于李括号[,]，由如下性质：$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$
可以用这个性质证明雅克比等价。
​

4. 指数映射和对数映射

4.1 SO(3)

李代数的指数映射为李群。比如对于旋转向量A属于SO(3)，对应的李代数$\varphi$，有指数映射：
$$R=exp({\varphi }^{\wedge })\text{\hspace{1em}(4.1)}$$

指数映射怎么求解？可以根据以下思路进行推导（注重思路，帮助手推）：

1. 任意矩阵的指数映射都可以写成一个泰勒展开
2. 将矩阵写成so(3)中的${\varphi }^{\wedge }$形式
3. phi是一个三维向量，可以写成角度乘以单位向量的形式$\theta a,||a||=1$
4. 根据反对称矩阵的性质，可以推断出两条性质 $$a^{\wedge }{a}^{\wedge }=a{a}^{\top }-I\text{\hspace{1em}(4.2)}$$$$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }\text{\hspace{1em}(4.3)}$$
5. 把指数映射泰勒展开，凑cosine和sine的泰勒展开形式，就能得到罗德里格斯公式（罗德里格斯公式就是指数映射）：$$R=exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=cos\theta I+(1-cos\theta )a{a}^{\top }+sin\theta a^{\wedge }\text{\hspace{1em}(4.4)}$$

每一个李代数元素都指数映射到唯一的李群元素，但是一个李群元素可能对应好几个李代数元素，比如旋转角加360度的效果是一样的。
​

4.2 SE(3)

类似的，也能推导处SE(3)上的指数映射：
$$\begin{array}{rl}T=exp({\xi }^{\wedge })& =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^{\top }& 1\end{array}\right]\\ & =\left[\begin{array}{cc}R& J\rho \\ 0^{\top }& 1\end{array}\right]\end{array}\text{\hspace{1em}(4.5)}$$

其中J也是可以通过展开后，凑cosine和sine的泰勒展开形式，得到J的表达式。J的表达式和罗德里格斯公式（推导过程）类似，但不完全一样。
$$\begin{array}{rl}J& \stackrel{def}{=}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\\ & =\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^{\top }+\frac{1-cos\theta }{\theta }{a}^{\wedge }\end{array}\text{\hspace{1em}(4.6)}$$
​

4.3 对数映射

对于对数映射，不太希望使用推导的方式，可以用等式两边求迹，然后求R的特征值为1的特征向量来求。
​

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

使用李代数的一大动机是进行优化，而在优化过程中导数是非常必要的信息。我们现在知道了李代数so(3)和SO(3)之间是指数映射关系。那么李代数的加法是否等于李群的乘积呢，$exp(a+b)=exp(a)exp(b)$
​

答案是否定的，两个李代数指数映射的乘积的完整形式，由Baker-Campbell-Hausdorff公式（BCH公式）给出。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。如果两个李代数其中之一为小量，那么小量二次以上的项都可以被忽略（在连续的运动中，旋转矩阵对应的李代数就是小量），此时，BCH拥有线性近似表达：
$$\begin{array}{r}ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2,当{\varphi }_1为小量\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1,当{\varphi }_2为小量\end{array}\end{array}\text{\hspace{1em}(5.1)}$$
左乘BCH近似雅可比$J_l$事实上就是式(4.6)的内容：
$$\begin{array}{r}{J}_l=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^{\top }+\frac{1-cos\theta }{\theta }{a}^{\wedge }\end{array}\text{\hspace{1em}(5.2)}$$
它的逆为（看书）。而右乘雅可比仅需要对自变量取负号即可：
$$J_r(\varphi )=J_l(-\varphi )\text{\hspace{1em}(5.3)}$$

如果将小量表示为$\Delta \varphi$，则乘法$\to$加法的BCH近似可以表示为：
$$exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })\text{\hspace{1em}(5.4)}$$
反过来，加法$\to$乘法的BCH近似可以表示为（记住这里没有逆的符号）：
$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp({\varphi }^{\wedge })exp((J_r\Delta \varphi {)}^{\wedge })\text{\hspace{1em}(5.5)}$$

5.1 李代数求导

前面提到，使用李代数的一大动机是为了优化，导数就很重要。比如要求解${\sum }_{i=1}^N||z_i-T{p}_i|{|}_2^2$，这个时候就需要计算SE(3)T的导数。重点是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，SO(3)SE(3)没有定义良好的加法，所以可以利用李代数（加法到乘法的指数映射）和扰动模型来求解这个问题：

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

​

李代数求导请看书P84，其中运用到了泰勒展开，反对称矩阵性质。对于李代数求导的结果$\frac{\partial (Rp)}{\partial \varphi }=(-Rp{)}^{\wedge }{J}_l$，仍然有复杂的J，我们不想计算他，所以可以使用扰动模型来求解。扰动模型求解中也运用到了泰勒展开和反对称矩阵性质，结果是：$\frac{\partial (Rp)}{\partial \varphi }=(-Rp{)}^{\wedge }$

6. 什么是Sophus

Sophus是一个李代数库，支持SO(2)SE(2)及Sim(3)的内容（求导等），是直接在Eigen的基础上开发的。
​

7. 未完善

关于相似变换Sim(3)的求导和扰动没有深入理解。
课后习题。