视觉SLAM十四讲 第4讲李群与李代数

一、李群与李代数基础

上一节给出了三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3)，但并未给出群的概念。

群

群是一种集合加上一种运算的代数结构。把集合记作$A$，运算记为$\cdot$,那么群可以记作$G=(A,\cdot )$。群要求运算满足以下几个性质:

而李群是指具有连续光滑性质的群。刚体可以在三维空间内连续运动，故$SO(3),SE(3)$都是李群。

李代数的引出

对于任意的旋转矩阵$R$，都满足:
$$R{R}^T=I\text{\hspace{1em}(1)}$$
用$R$来表示某个相机的旋转，他是时间的函数，随时间连续变化，即$R(t)$,仍然有:
$$R(t)R(t{)}^T=I\text{\hspace{1em}(2)}$$
在等式两边同时对时间求导，这里不考虑旋转矩阵的实际意义，而只是把他当做普通矩阵来处理，便于李代数的引出。得到
$$\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0\text{\hspace{1em}(3)}$$
进而得：
$$\dot{R}(t)R(t{)}^T=-R(t)\dot{R}(t{)}^T=-(\dot{R}(t)R(t{)}^T{)}^T\text{\hspace{1em}(4)}$$
如果令$W(t)=\dot{R}(t)R(t{)}^T$，则有$W(t)=-W(t{)}^T$,可知$W(t)$为反对称矩阵，进而$\dot{R}(t)R(t{)}^T$为反对称矩阵。

根据上节使用的符号$\wedge$,他将一个向量转换为一个反对称矩阵，同样，我们定义符号$\vee$他将一个反对称矩阵转换为一个向量。
$$a^{\wedge }=A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],A^{\vee }=a\text{\hspace{1em}(5)}$$

这样，对于刚刚的反对称矩阵$W(t)=\dot{R}(t)R(t{)}^T$,就可以找到一个向量$\varphi (t)$，使得$\varphi (t{)}^{\wedge }=W(t)=\dot{R}(t)R(t{)}^T$，即：
$$\dot{R}(t)R(t{)}^T=\varphi (t{)}^{\wedge }\text{\hspace{1em}(6)}$$
两边右乘$R(t)$，有：
$$\dot{R}(t)R(t{)}^{T}R(t)=\varphi (t{)}^{\wedge }R(t)\text{\hspace{1em}(7)}$$
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)\text{\hspace{1em}(8)}$$
可以看出，将旋转矩阵左乘一个$\varphi (t{)}^{\wedge }$矩阵，相当于旋转矩阵$R(t)$对时间求了一次导数。我们在$t_0=0$处，对旋转矩阵$R(t)$进行泰勒展开：
$$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)\text{\hspace{1em}(9)}$$
假设旋转矩阵在初始状态下有$R(t_0)=R(0)=I$，则
$$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\dot{R}(t_0)(t-t_0)\text{\hspace{1em}(10)}$$
进一步由式8
$$R(t)\approx I+\varphi (t_0{)}^{\wedge }It=I+\varphi (t_0{)}^{\wedge }(t)\text{\hspace{1em}(11)}$$
$\varphi$反应了$R$的导数性质，故称它在$S0(3)$原点附近的正切空间上，在$t_0$附近，假设$\varphi$的值保持常数$\varphi (t_0)={\varphi }_0$,由式8：
$$\dot{R}(t)={\varphi }_0^{\wedge }R(t)\text{\hspace{1em}(12)}$$
这是一个常系数一阶微分方程，根据初始条件$R(0)=I$，得到
$$R(t)=exp({\varphi }_0^{\wedge }t)\text{\hspace{1em}(13)}$$
这个式子较为重要

李代数的定义

李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$，和一个二元运算符$[,]$组成，如果满足以下性质，则称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记为$\mathfrak{g}$.

其中的二元运算符称之为李括号。

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

我们在引出一节中提到的$\varphi$就是一种李代数，$SO(3)$对应的李代数就是定义在${\mathbb{R}}^3$上的向量，记作$\varphi$，由上面可知
$$\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\text{\hspace{1em}(14)}$$
同时定义两个向量的李括号运算为:
$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }\text{\hspace{1em}(15)}$$
可以验证该定义下的李括号满足上面提到的几条性质。同时，由于$\Phi$和$\varphi$的紧密关系，这两者都可以称之为$\mathfrak{s}\mathfrak{o}(3)$的元素：
$$\mathfrak{s}\mathfrak{o}(3)=\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\}\text{\hspace{1em}(16)}$$
同时根据式13，$SO(3)$和$\mathfrak{s}\mathfrak{o}(3)$的关系也已经给出:
$$R=exp({\varphi }^{\wedge })\text{\hspace{1em}(17)}$$

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

对于$SE(3)$同样由与之对应的李代数，这里直接给出定义:
$$\mathfrak{s}\mathfrak{e}(3)=\{\xi =\left[\begin{array}{c}\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}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}\text{\hspace{1em}(18)}$$

这里的$\rho$可以理解是三维的平移分量，但与实际的三维平移并不相等，还存在一定的关系，将在后面给出。

二、指数和对数映射

$SO(3)$上的指数映射

接下来的问题是考虑$exp({\varphi }^{\wedge })$是怎么计算的。这在李群和李代数中称为指数映射，将其泰勒展开，可以得到：
$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n\text{\hspace{1em}(19)}$$
将三维向量$\varphi$拆分成模长和向量乘积的形式，即$\varphi =\theta \mathbf{a}$，这里的$\mathbf{a}$为模长为1的方向向量，对于$\mathbf{a}$有：
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-\mathbf{I}\text{\hspace{1em}(20)}$$
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge }\text{\hspace{1em}(21)}$$
通过上述两条性质，可以得到指数映射的表达:

即:
$$exp({\varphi }^{\wedge })=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{a}{\mathbf{a}}^T+sin\theta {\mathbf{a}}^{\wedge }\text{\hspace{1em}(22)}$$
这与上一节中的罗德里格斯公式如出一辙，可见，$SO(3)$和$\mathfrak{s}\mathfrak{o}(3)$的关系，与旋转矩阵和旋转向量的关系相同。
同理，通过相反的对数映射，也可以将$SO(3)$映射到$\mathfrak{s}\mathfrak{o}(3)$上:
$$\varphi =ln(R{)}^{\vee }=(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }\text{\hspace{1em}(23)}$$

$SE(3)$上的指数映射

我们直接给出映射关系:
$$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^T& 1\end{array}\right]\triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\rho \\ 0^T& 1\end{array}\right]\text{\hspace{1em}(24)}$$
左上角的$R$就是$\xi$中的$\varphi$分量的指数映射$R$，之前说过$\rho$与真实的平移分量存在特定的关系：
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }\text{\hspace{1em}(25)}$$

$SO(3),SE(3),\mathfrak{s}\mathfrak{o}(3),\mathfrak{s}\mathfrak{e}(3)$之间的关系如下:

三、李代数求导与扰动模型

BCH公式与近似公式

我们使用李代数的目的是代替李群来进行优化，因为李群本身所具有的约束会给这一问题带来许多困难。那么现在考虑，当在李群$SO(3)$上进行两个矩阵的乘法时，在李代数层面，发生了怎样的变化。我们根据指数函数的性质，可能会有这样的猜测:
$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })\text{\hspace{1em}(26)}$$
但是很遗憾，在标量层面上满足的指数函数性质，在矩阵层面上并不满足，上式并不成立。两个李代数指数映射乘积的完整形式由BCH公式给出，其前几项为:
$$ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\dots \text{\hspace{1em}(27)}$$
但对于${\varphi }_1$或${\varphi }_2$位小量时，有如下的近似关系成立:
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \left\{\begin{array}{rl}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{当ϕ1为小量}\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{当ϕ2为小量}\end{array}\right\}\text{\hspace{1em}(28)}$$

以第一种情况为例，即在旋转矩阵$R_2$左乘一个微小的旋转矩阵，在李代数层面，这约等于原有李代数${\varphi }_2$的基础上加上了一项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$，而这里的$J_l$实际上就是式25:
$$J_l=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }\text{\hspace{1em}(29)}$$
同时也可以得到它的逆:
$$J_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }\text{\hspace{1em}(30)}$$
还有:
$$J_r(\varphi )=J_l(-\varphi )\text{\hspace{1em}(31)}$$
至此，建立了李群乘法和李代数加法之间的关系。假设有一个旋转$R$，对应的李代数为$\varphi$，在此旋转基础上左乘一个微小旋转$\Delta R$,对应的李代数为$\Delta \varphi$，那么在李群上得到的结果为$\Delta RR$，而在李代数上得到的结果为$J_l^{-1}(\varphi )\Delta \varphi +\varphi$，有:
$$exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((J_l^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge })\text{\hspace{1em}(32)}$$
同样，在李代数上进行加法，对应的，在李群上实现了带雅克比的乘法:
$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })\text{\hspace{1em}(33)}$$

$SO(3)$李代数上的求导

在SLAM中我们要估计一个相机的位置和姿态，该位姿是有$SO(3)$上的旋转矩阵或$SE(3)$上的变换矩阵描述的。假设某一时刻机器人的位姿为T，他观察到了一个世界坐标位于$p$的点，产生了一个观测数据$z$，由坐标变换关系:
$$z=Tp+w\text{\hspace{1em}(34)}$$
$w$为噪声，正是由于噪声的存在，理想的观测与实际的数据之间往往会存在误差:
$$e=z-Tp\text{\hspace{1em}(35)}$$
假如有N个这样的路标点和观测，对于机器人的位姿估计，相当是寻找一个最优的T，使得整体误差最小化:
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2\text{\hspace{1em}(36)}$$
这必然需要求解目标函数J对T的导数，而在$SO(3)$和$SE(3)$上并没有封闭的加减法，故无法在$SO(3)$和$SE(3)$上进行直接求导。若将T当做普通矩阵处理，由于欠封闭性，势必需要加以约束。而可以从李代数的层面解决求导问题。

李代数求导

用李代数来表示姿态，然后用李代数的加法来对李代数进行求导:
空间一点坐标为p,旋转变换为R，对应的李代数为$\varphi$，旋转之后点的坐标相对于旋转本身的导数：
$$\frac{\partial (Rp)}{\partial R}=\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }\text{\hspace{1em}(37)}$$
有：

进而得：
$$\frac{\partial (Rp)}{\partial R}=(-Rp{)}^{\wedge }{J}_l\text{\hspace{1em}(38)}$$

这个结果并不是很好，含有J较为复杂。

扰动模型(左乘)

对R进行一次扰动$\Delta R$，对应的李代数为$\Delta \phi$，以左乘为例:
$$\frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\text{\hspace{1em}(39)}$$
对$\exp ({\phi }^{\wedge })$进行泰勒展开，取前两项:
$$\exp ({\phi }^{\wedge })\approx I+{\phi }^{\wedge }\text{\hspace{1em}(40)}$$
带回:
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\exp ({\varphi }^{\wedge })p}{\phi }=\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }\end{array}\text{\hspace{1em}(41)}$$
这次的形式要简洁的多。

$SE(3)$上的李代数求导

设点p经过一次变换T对应李代数为$\xi$得到Tp，现在给T左乘一个扰动$\Delta T=\exp (\delta {\xi }^{\wedge })$,设扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$,有:
$$\frac{\partial (Tp)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (Tp{)}^{\odot }\text{\hspace{1em}(42)}$$
定义运算符$\odot$，把一个齐次坐标的空间点转换成一个$4\times 6$的矩阵

往期链接
视觉SLAM十四讲 第3讲 三维空间刚体运动
视觉SLAM十四讲 第2讲 初识slam