SLAM基础——李代数基础概念及公式

• ⭐ Zeal's Blog
• 🛠 Zeal's 知乎专栏
• 🌀 Zeal's Github

李群李代数基础概念

群（Groups） && 李群（LieGroups）

群的定义

群是一种 集合 加上 一种运算 的代数结构，这种运算需要满足几个条件（可以不用记住）

例 ：旋转矩阵（集合）和矩阵乘法（运算），矩阵乘法对于旋转矩阵具有很良好的性质（符合群运算的定义），于是旋转矩阵是一种群。

特殊正交群

特殊正交群（Special Orthogonal Group）记为$SO(n)$，又叫旋转矩阵群。 例：三维 旋转矩阵构成特殊正交群$SO(3)$

$$SO(3)=\{R\in {\mathbb{R}}^{3\times 3}:R^{T}R=\mathbf{I},det(R)=1\}$$

特殊欧式群

特殊欧式群记为$SE(n)$，对应$n$维的欧式变换（旋转加平移）

李群

李群是具有连续（光滑）性质的群

例：旋转可以连续地进行，平移也可以连续地进行，所以$SO(3)$、$SE(3)$都是李群

李代数

反对称矩阵符号

对于一个三维向量$\mathit{\varphi }=[{\varphi }_1,{\varphi }_2,{\varphi }_3{]}^T$，记其对应的反对称矩阵${\mathit{\varphi }}^{\wedge }\in {\mathbb{R}}^{3\times 3}$，具有如下形式。符号$\wedge$称作hat

$$\begin{array}{}\text{(hat)}& {\mathit{\varphi }}^{\wedge }={\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]}^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\end{array}$$

同样的，任意一个反对称矩阵通过vee操作得到对应的三维向量

$$\begin{array}{}\text{(vee)}& ({\mathit{\varphi }}^{\wedge }{)}^{\vee }={\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]}^{\vee }=\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]\end{array}$$

李代数与李群的关系

• 每个李群都对应一个李代数。$SO(3)$群的李代数是$\mathfrak{so}(3)$

• 为了不造成理解困难，这里只用$SO(3)$群作为讲解。（笔者发现，用$SO(3)$群作为例子，不看$SE(3)$群理解起来会更容易）

• 为什么要引入李代数？ 一句话说明：在$SO(3)$群上求导不太容易，因为旋转矩阵上的加法没有良好性质（两个旋转矩阵的加法不一定是一个旋转矩阵;回忆导数的定义，输入的微小变化造成的输出的改变量），而旋转矩阵的乘积在其中一者为微小量的时候，可以对应到李代数空间的线性关系。利用这一性质可以对旋转矩阵进行求导。旋转矩阵的更性也可以对应到李代数空间上的线性更新。后文会详细解说。

• 李代数对应李群单位元处的切空间 旋转矩阵群的单位元就是单位矩阵。李代数对应李群单位元处的切空间这一点只是两者的性质，与后文的指数/对数映射没有太大关系。（笔者一开始总是误会指数/对数映射只能在李群单位元处进行）

对数映射（log）和指数映射（exp）

《视觉SLAM十四讲》中这部分内容（第四讲）是通过旋转矩阵求导引出的，但其实这部分的推导只是建立一个感性的理解。李群李代数的指数/对数映射应该具有更严格的定义。所以这部分内容其实不要太过纠结。

再次强调：这部分推导只是为了建立感性认识

由于旋转矩阵的性质：

$$R{R}^T=\mathbf{I}$$

上式对旋转矩阵求导（$\dot{R}$表示$R$的导数）：

$$\dot{R}{R}^T+R{\dot{R}}^T=\mathbf{0}$$

（这里对旋转矩阵求导，应该将其看作是一个变量，在时刻变化）

即：

$$\dot{R}{R}^T=-R{\dot{R}}^T=-(\dot{R}{R}^T{)}^T$$

也就是$\dot{R}{R}^T$是一个反对称矩阵，我们将其记为${\mathit{\varphi }}^{\wedge }$，则我们可以得到：

$$\begin{array}{rl}\dot{R}{R}^T& ={\mathit{\varphi }}^{\wedge }\\ \dot{R}& ={\mathit{\varphi }}^{\wedge }R\end{array}$$

上式是关于$R$的一个微分方程，我们可以看出，当

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

的时候，刚好满足上面的微分方程。同时，当$R=\mathbf{I}$时，上面的微分方程刚好是$\dot{R}={\mathit{\varphi }}^{\wedge }$，所以说反对称矩阵空间是旋转矩阵单位元处的切平面（导数）。

于是，我们直接地引入定义：指数映射将李代数空间映射到李群空间，指数映射将李群映射到李代数空间。

$$\begin{array}{r}\exp (\cdot ):\mathfrak{so}(3)\to SO(3)\\ \log (\cdot ):SO(3)\to \mathfrak{so}(3)\end{array}$$

SO(3)上的对数映射/$\mathfrak{so}(3)$的指数映射

$\mathfrak{so}(3)$的指数映射

令三维向量$\mathit{\varphi }$的模为$\theta$，方向向量为$\mathit{a}$，即

$$\begin{gathered} \theta =||\mathit{\varphi }|| \\ \mathit{a}=\frac{\mathit{\varphi }}{||\mathit{\varphi }||} \end{gathered}$$

经过一系列的推导，可以得到指数映射的公式如下：

$$\begin{array}{}\text{(exp-1)}& \begin{array}{rl}\exp ({\mathit{\varphi }}^{\wedge })& =\exp (\theta \mathit{a})\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathit{a}{\mathit{a}}^T+\sin \theta {\mathit{a}}^{\wedge }\end{array}\end{array}$$

上面公式的一种等价写法（不使用模和方向代替）为：

$$\begin{array}{}\text{(exp-2)}& \begin{array}{r}\exp ({\mathit{\varphi }}^{\wedge })=\mathbf{I}+\frac{\sin ||\mathit{\varphi }||}{||\mathit{\varphi }||}+\frac{1-\cos ||\mathit{\varphi }||}{||\mathit{\varphi }|{|}^2}({\mathit{\varphi }}^{\wedge }{)}^2\end{array}\end{array}$$

上面这两个公式叫做罗德里格斯（Rodrigues' formula），也是将 旋转向量 变换为旋转矩阵的变换公式。从上面两个公式我们也可以看出，$\mathfrak{so}(3)$李代数空间实际上也是 旋转向量 组成的空间。又因为旋转角的周期为$2\pi$，所以一个旋转向量可以对应到一个旋转矩阵，但是一个旋转矩阵会对应到多个旋转向量（满射）。但是如果将旋转角限制在$[-\pi ,\pi )$，则是单射。

SO(3)上的对数映射

从上面的关系，我们可以知道要把李群$SO(3)$映射到李代数，只需要用旋转矩阵计算旋转矢量即可。从旋转矩阵$R$计算旋转角公式为：

$$\begin{array}{}\text{(ln-01)}& \theta =\arccos \frac{tr(R)-1}{2}\end{array}$$

又由于旋转轴使用旋转矩阵旋转后方向不变，即：

$$\begin{array}{}\text{(ln-02)}& R\mathit{a}=\mathit{a}\end{array}$$

所以旋转轴对应$R$特征值为1的特征向量

轴向量也可以使用下面公式计算

$$\begin{array}{}\text{(ln-03)}& \log (R)=\frac{\theta \cdot (R-R^T)}{2sin(\theta )}\text{with}R\ne \mathbf{I}\end{array}$$

指数映射的一阶近似

指数映射的严格展开形式为:

$$\exp ({\mathit{\varphi }}^{\wedge })=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\mathit{\varphi }}^{\wedge }{)}^n$$

当$\mathit{\varphi }$是微小量时，忽略二次项及以上，得到指数映射的一阶近似：

$$\begin{array}{}\text{(exp-1st)}& \exp ({\mathit{\varphi }}^{\wedge })\approx \mathbf{I}+{\mathit{\varphi }}^{\wedge }\end{array}$$

上面这个公式经常被使用到，要牢记

李群李代数总结

• 引入李代数的原因是在SLAM中常用的群：$SO(3),SE(3),Sim(3)$没有良好定义的加法，因此在优化的时候没法直接往上更新一个微小增量
• 另一个原因是$SO(3),SE(3),Sim(3)$这些群都有额外的限制，直接优化就变成了带约束的优化，而转到李代数空间则是一个无约束的优化问题
• 在后面我们可以看到当旋转矩阵乘以一个微小扰动可以对应到李代数空间的近似线性相加，这为优化带来很大的好处
• 李代数通过指数映射到李群，李群通过对数映射到李代数
• 李代数是李群单位元处的切平面
• 不要纠结上面的这句话！！！！对数映射/指数映射可以对整个李群/李代数空间进行映射！！！

BCH近似

BCH(Baker-Campbell-Hausdorff)公式表达两个李代数指数映射乘积的完整形式（很复杂，不用记），但是但忽略高阶项后则会出现优美的线性表达。

当旋转矩阵$R$左乘或者右乘上一个微小量$\mathrm{\Delta }R$时，会近似（BCH近似）对应李代数空间线性相加，记$R$的李代数为$\mathit{\varphi }$，$\mathrm{\Delta }R$的李代数为$\mathrm{\Delta }\mathit{\varphi }$

BCH近似有左乘右乘两种形式，笔者发现学习的时候以一种形式去推导即可，更容易理解，下面以右乘的形式进行讲解（十四讲中是以左乘作为讲解）

BCH近似右乘形式

$$\begin{array}{}\text{(BCH)}& \exp ({\mathit{\varphi }}^{\wedge })\exp (\mathbf{\Delta }{\mathit{\varphi }}^{\wedge })=\exp (J_r^{-1}(\mathit{\varphi })\mathrm{\Delta }\mathit{\varphi }+\mathit{\varphi })\end{array}$$

其中$J_r(\mathit{\varphi })$乘为右乘雅各比：

$$\begin{array}{}\text{(BCH-Jr)}& J_r(\mathit{\varphi })=\mathbf{I}-\frac{1-\cos ||\mathit{\varphi }||}{||\mathit{\varphi }|{|}^2}+\frac{||\mathit{\varphi }||-\sin ||\mathit{\varphi }||}{||\mathit{\varphi }|{|}^3}({\mathit{\varphi }}^{\wedge }{)}^2\end{array}$$

也可以写成轴-角的形式

$$J_r(\mathit{\varphi })=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathit{a}{\mathit{a}}^T-\frac{1-\cos \theta }{\theta }{\mathit{a}}^{\wedge }$$

上面两种等价关系的变换中需要用到一个性质

$${\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }=\mathit{a}{\mathit{a}}^T-\mathbf{I}$$

另外

$$J_r^{-1}(\mathit{\varphi })=\mathbf{I}+\frac{1}{2}{\mathit{\varphi }}^{\wedge }+(\frac{1}{||\mathit{\varphi }|{|}^2}-\frac{1+\cos ||\mathit{\varphi }||}{2||\mathit{\varphi }||\sin ||\mathit{\varphi }||})({\mathit{\varphi }}^{\wedge }{)}^2$$

BCH近似的其他表达形式

BCH近似是为了能够将$SO(3)$上的矩阵乘积变成李代数上的加法，下面几条公式只是公式（BCH）的其他表达形式，后面也会频繁用到

$$\begin{array}{}\text{(BCH-2)}& \exp ((\mathit{\varphi }+\mathrm{\Delta }\mathit{\varphi }{)}^{\wedge })=\exp ({\mathit{\varphi }}^{\wedge })\exp (J_r\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })\end{array}$$

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

加入我们对一个空间点$\mathit{p}$进行旋转，得到$R\mathit{p}$，要求相对于$R$的导数。由于$SO(3)$上没有加法，于是我们没法直接使用导数的定义进行计算。但是我们可以将$R$映射到李代数空间$\mathit{\varphi }\wedge$，并在李代数空间进行求导（SLAM十四讲公式4.41使用BCH左乘模型推导，这里使用右乘模型推导）

李代数空间求导

$$\begin{array}{rl}\frac{\partial (R\mathit{p})}{\partial R}& =\frac{\partial (\exp ({\mathit{\varphi }}^{\wedge })\mathit{p})}{\partial \mathit{\varphi }}\\ & \stackrel{1}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ((\mathit{\varphi }+\mathrm{\Delta }\mathit{\varphi }{)}^{\wedge })\mathit{p}-\exp ({\mathit{\varphi }}^{\wedge })\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{2}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })\exp (J_r\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })\mathit{p}-\exp ({\mathit{\varphi }}^{\wedge })\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{3}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })(\mathbf{I}+J_r\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })\mathit{p}-\exp ({\mathit{\varphi }}^{\wedge })\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{4}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })J_r\mathrm{\Delta }{\mathit{\varphi }}^{\wedge }\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{5}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{-\exp ({\mathit{\varphi }}^{\wedge })J_r{\mathit{p}}^{\wedge }\mathrm{\Delta }\mathit{\varphi }}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & =-R{J}_r{\mathit{p}}^{\wedge }\end{array}$$

上式的推导中，等号1是导数的定义;等号2使用BCH右乘近似公式，也就是上文公式（BCH-2）;等号3是指数映射的一阶近似，也就是上文公式（exp-1st）;等号5利用了下面的性质

$$\begin{array}{}\text{(skew-prop)}& {\mathit{a}}^{\wedge }\mathit{b}=-{\mathit{b}}^{\wedge }\mathit{a}\end{array}$$

将上面的结果记为 $$\begin{array}{}\text{(dR-alg-R)}& \frac{\partial (R\mathit{p})}{\partial R}=-R{J}_r{\mathit{p}}^{\wedge }\end{array}$$

如果在上面的推导中使用左乘模型进行推导，结果会稍微有点不同：

$$\begin{array}{}\text{(dR-alg-L)}& \frac{\partial (R\mathit{p})}{\partial R}=(-R\mathit{p}{)}^{\wedge }{J}_l\end{array}$$

李群空间求导

另一种更常见的做法是在李群上对$R$进行一个扰动$\exp (\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })$，计算结果相对于扰动的变化率。SLAM十四讲中使用左扰动推导，这里使用右扰动推导一下

$$\begin{array}{rl}\frac{\partial (R\mathit{p})}{\partial R}& =\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })\exp (\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })\mathit{p}-\exp ({\mathit{\varphi }}^{\wedge })\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{1}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })(\mathbf{I}+\mathrm{\Delta }{\mathit{\varphi }}^{\wedge })\mathit{p}-\exp ({\mathit{\varphi }}^{\wedge })\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{2}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{\exp ({\mathit{\varphi }}^{\wedge })\mathrm{\Delta }{\mathit{\varphi }}^{\wedge }\mathit{p}}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & \stackrel{3}{=}\underset{\mathrm{\Delta }\mathit{\varphi }\to 0}{lim}\frac{-\exp ({\mathit{\varphi }}^{\wedge }){\mathit{p}}^{\wedge }\mathrm{\Delta }\mathit{\varphi }}{\mathrm{\Delta }\mathit{\varphi }}\\ \\ & =-R{\mathit{p}}^{\wedge }\end{array}$$

其中等号1使用指数映射的一阶近似，也就是公式（exp-1st）;等号3使用反对称矩阵的性质，也就是公式（skew-prop）。

如果使用左扰动推导，结果会稍有不同，我们将两者记为如下

右扰动模型：

$$\begin{array}{}\text{(dR-group-R)}& \frac{\partial (R\mathit{p})}{\partial R}=-R{\mathit{p}}^{\wedge }\end{array}$$

左扰动模型：

$$\begin{array}{}\text{(dR-group-L)}& \frac{\partial (R\mathit{p})}{\partial R}=-(R\mathit{p}{)}^{\wedge }\end{array}$$