SLAM LECTURE 4：李群与李代数

§4.1 李群与李代数基础

§4.1.1 群

1. 定义： “封结幺逆”：封闭性（乘法）、结合律、幺元、逆元

   • 一般线性群 GL(n)：n×n可逆矩阵，对乘法构成群
   • 特殊正交群 SO(n)：旋转矩阵群
   • 特殊欧式群 SE(n)：欧式变换群

2. 李群：具有连续（光滑）性质的群，如SO(n)，SE(n)

§4.1.2 李代数的定义

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

1. 封闭性 $\mathrm{\forall }X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}$ .
2. 双线性 $\mathrm{\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. 自反性 $\mathrm{\forall }X\in \mathbb{V},[X,X]=0$ .

4. 雅可比等价 $\mathrm{\forall }X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$ .

二元运算称作李括号，表达的是元素的差异。

§4.1.3 李代数 $\mathfrak{so}(3)$

$SO(3)$ 对应的李代数 $\mathfrak{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]={({\mathbf{\Phi }}_1{\mathbf{\Phi }}_2-{\mathbf{\Phi }}_2{\mathbf{\Phi }}_1)}^{\vee }.$$

由于三维向量与三位反对称矩阵一一对应， $\mathfrak{so}(3)$ 的元素视作三维向量或三维反对称矩阵：

$$\mathfrak{so}(3)=\{\varphi \in {\mathbb{R}}^3,\mathbf{\Phi }={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\}.$$

由此，它与 $SO(3)$ 的关系由指数映射给定：

$$R=\exp ({\varphi }^{\wedge }).$$

§4.1.4 李代数 $\mathfrak{se}(3)$

类似地， $SE(3)$ 的李代数定义如下：

$$\mathfrak{sc}(3)=\{\mathit{\xi }=\left[\begin{array}{c}\mathit{\rho }\\ \\ \mathit{\varphi }\end{array}\right]\in {\mathbb{R}}^6,\mathit{\rho }\in {\mathbb{R}}^3,\mathit{\varphi }\in \mathfrak{so}(3),{\mathit{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathit{\varphi }}^{\wedge }& \mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}.$$

其中，前三位为平移，记作 $\rho$ ，但含义与变换矩阵不同。此外，对于 $\xi$ 使用的 ${}^{\wedge }$ 符号是讲六位向量转换成四维向量，与先前不同。

此处李括号定义为：

$$[{\mathit{\xi }}_1,{\mathit{\xi }}_2]=({\mathit{\xi }}_1^{\wedge }{\mathit{\xi }}_2^{\wedge }-{\mathit{\xi }}_2^{\wedge }{\mathit{\xi }}_1^{\wedge }{)}^{\vee }.$$

§4.2 对数与指数映射

§4.3 李代数求导与扰动模型

§4.3.1 BCH公式及其近似形式

现考察李群上两个矩阵乘法时李代数发生的变化，得到BCH公式。当其中一个 $\varphi$ 为小量时，高次可忽略，此时，BCH拥有线性近似表达：

$$\ln {(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right))}^{\vee }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{当}{\varphi }_1\text{为小量},\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{当}{\varphi }_2\text{为小量}.& \end{array}$$

其中，左乘BCH近似雅可比 ${\mathit{J}}_l$ 形式为：

$${\mathit{J}}_l=\mathit{J}=\frac{\sin \theta }{\theta }\mathit{I}+(1-\frac{\sin \theta }{\theta })\mathit{a}{\mathit{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathit{a}}^{\wedge }.$$

其逆为：

$${\mathit{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathit{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathit{a}{\mathit{a}}^{\mathrm{T}}-\frac{\theta }{2}{\mathit{a}}^{\wedge }.$$

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

$${\mathit{J}}_r(\varphi )={\mathit{J}}_l(-\mathit{\varphi }).$$

由以上可得李群乘法与李代数加法之间的关系。

对于旋转 $\mathit{R}$ 及其李代数 $\varphi$ ，给它左乘一个微小旋转 $\mathrm{\Delta }\mathit{R}$ ，其对应的李代数为 $\mathrm{\Delta }\varphi$ 。那么，在李群上得到的结果就是 $\mathrm{\Delta }\mathit{R}\cdot \mathit{R}$ ，而在李代数上，根据BCH近似，为 $J_l^{-1}(\varphi )\mathrm{\Delta }\varphi +\varphi$ 。合并起来可以写成：

$$\exp \left(\mathrm{\Delta }{\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({(\varphi +J_l^{-1}\left(\varphi \right)\mathrm{\Delta }\varphi )}^{\wedge }\right).$$

反之，在李代数上做加法，可以写成李群上的乘法：

$$\exp \left({(\varphi +\mathrm{\Delta }\varphi )}^{\wedge }\right)=\exp \left({\left({\mathit{J}}_l\mathrm{\Delta }\varphi \right)}^{\wedge }\right)\exp \left({\mathit{\varphi }}^{\wedge }\right)=\exp \left({\mathit{\varphi }}^{\wedge }\right)\exp \left({\left({\mathit{J}}_r\mathrm{\Delta }\varphi \right)}^{\wedge }\right).$$

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

$$\begin{array}{r}\exp \left(\mathrm{\Delta }{\mathit{\xi }}^{\wedge }\right)\exp \left({\mathit{\xi }}^{\wedge }\right)\approx \exp \left({({\mathcal{J}}_l^{-1}\mathrm{\Delta }\mathit{\xi }+\mathit{\xi })}^{\wedge }\right),\\ \exp \left({\mathit{\xi }}^{\wedge }\right)\exp \left(\mathrm{\Delta }{\mathit{\xi }}^{\wedge }\right)\approx \exp \left({({\mathcal{J}}_r^{-1}\mathrm{\Delta }\mathit{\xi }+\mathit{\xi })}^{\wedge }\right).\end{array}$$

§4.3.2 两种求导方法

1. 利用李代数表示姿态，然后根据李代数加法对李代数求导：

$$\frac{\partial \left(\mathit{R}\mathit{p}\right)}{\partial \mathit{\varphi }}={(-\mathit{R}\mathit{p})}^{\wedge }{\mathit{J}}_l.$$

2. 对李群左乘或右乘微小扰动，然后对该扰动求导

左扰动模型求导：

$$\begin{array}{rl}\frac{\partial \left(Rp\right)}{\partial \phi }& =\underset{\phi \to 0}{lim}\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\phi }\\ & =\underset{\phi \to 0}{lim}\frac{(\mathit{I}+{\mathit{\phi }}^{\wedge })\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{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}$$

相比于直接对李代数求导，扰动模型更简洁实用。

§4.5 相似变换群和李代数

由于单目相机存在尺度不确定性与尺度漂移，需要尺度因子来表示，因此相机坐标系需要经过一个相似变换，而非欧式变换：

$${\mathit{p}}^{\prime }=\left[\begin{array}{cc}s\mathit{R}& \mathit{t}\\ \\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right]\mathit{p}=s\mathit{R}\mathit{p}+\mathit{t}.$$

类似地，相似变换也对矩阵乘法构成群，称为相似变换群：

$$\mathrm{Sim}(3)=\{\mathit{S}=\left[\begin{array}{cc}s\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}.$$

对应李代数为：

$$\mathfrak{sim}(3)=\{\mathit{\zeta }|\mathit{\zeta }=\left[\begin{array}{c}\mathit{\rho }\\ \\ \mathit{\varphi }\\ \\ \mathit{\sigma }\end{array}\right]\in {\mathbb{R}}^7,{\mathit{\zeta }}^{\wedge }=\left[\begin{array}{cc}\sigma \mathit{I}+{\mathit{\varphi }}^{\wedge }& \mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}.$$

指数映射为：

$$\exp \left({\mathit{\zeta }}^{\wedge }\right)=\left[\begin{array}{cc}{\mathrm{e}}^{\sigma }\exp \left({\mathit{\varphi }}^{\wedge }\right)& {\mathit{J}}_s\mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right].$$

其中，

$$\begin{array}{rl}{\mathit{J}}_s& =\frac{{\mathrm{e}}^{\sigma }-1}{\sigma }\mathit{I}+\frac{\sigma {\mathrm{e}}^{\sigma }\sin \theta +(1-{\mathrm{e}}^{\sigma }\cos \theta )\theta }{{\sigma }^2+{\theta }^2}{\mathit{a}}^{\wedge }\\ & +(\frac{{\mathrm{e}}^{\sigma }-1}{\sigma }-\frac{({\mathrm{e}}^{\sigma }\cos \theta -1)\sigma +({\mathrm{e}}^{\sigma }\sin \theta )\theta }{{\sigma }^2+{\theta }^2}){\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }.\end{array}$$