视觉SLAM十四讲ch4——李群与李代数

4.1 李群李代数基础

什么是群？

• 群（Group）是一种集合加上一种运算的代数结构
• 记集合为$A$， 运算为$\cdot$，那么运算满足以下性质时，称$(A,\cdot )$成群。
• 封结幺逆的运算 （凤姐咬你）

举例：

当$A$为整数时，运算为$+$，$A,\cdot$成群

• 可以验证：

  • 旋转矩阵集合和矩阵乘法 构成群
  • 同样变换矩阵和矩阵乘法也构成群
  • 因此，称它们为旋转矩阵群 和 变换矩阵群

• 其他常见群

  • 一般线性群$GL(n)$：指$n\times n$的可逆矩阵，它们对矩阵乘法成群
  • 特殊正交群$SO(n)$：也就是所谓的旋转矩阵群，其中$SO(2)$和$SO(3)$最为常见
  • 特殊欧式群$SE(n)$：也就是前面提到的$n$维欧式变换，如$SE(2)$和$SE(3)$。

• 群结构保证了在群上的运算具有良好的性质。

• 群论是研究群的各种结构和性质的理论，具体见各种抽象代数或近世代数教材

• 李群（Lie Group）

  • 具有连续（光滑）性质的群
  • 既是群，也是流形。
  • 直观上看，一个刚体能够连续地在空间中运动，故$SO(3)$ 和 $SE(3)$都是李群
  • 但是，$SO(3)$ 和 $SE(3)$只有定义良好的乘法，没有加法，所以难以进行取极限、求导等操作。但是在位姿估计领域需要对其求导取极限，以寻找最优解，因此这也就是位姿估计的主要研究难点之一。

• 李代数：与李群对应的一种结构，位于向量空间

  • 通常记作小写的$so(3)$ 或 $se(3)$
  • 事实上是李群单位元处的正切空间。

• 李代数的引出：

  • 任意旋转矩阵$\mathbf{R}$，满足
    $${\mathbf{R}\mathbf{R}}^{\text{T}}=\mathbf{I}$$

  • 考虑$\mathbf{R}$ 随时间的变化，有：

  $$\mathbf{R}(t)\mathbf{R}(t{)}^{\text{T}}=\mathbf{I}\text{\hspace{1em}(1)}$$

  • 两侧对时间求导：

  $$\dot{\mathbf{R}(t)}\mathbf{R}(t{)}^{\text{T}}+\mathbf{R}(t)\dot{\mathbf{R}(t{)}^{\text{T}}}=0\text{\hspace{1em}(2)}$$

  • 整理得：

  $$\dot{\mathbf{R}(t)}\mathbf{R}(t{)}^{\text{T}}=-{\left(\dot{\mathbf{R}(t)}\mathbf{R}(t{)}^{\text{T}}\right)}^{\text{T}}\text{\hspace{1em}(3)}$$

  • 可以看出来，上式为一个反对称矩阵，注意$\varphi (t)$为一个向量，记：

  $$\dot{\mathbf{R}(t)}\mathbf{R}(t{)}^{\text{T}}=\varphi (t{)}^{\wedge }\text{\hspace{1em}(4)}$$

  • 这里需要注意 反对称符号的引入：即一个向量$a$的上尖为一个反对称矩阵$A$， 反对称矩阵$A$的下尖为向量$a$

  $$a^{\wedge }=\mathbf{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{A}}^{\vee }=a$$

  • 两侧同时右乘$\mathbf{R}(t)$，得
    $$\dot{\mathbf{R}(t)}={\varphi }^{\wedge }\mathbf{R}(t)\text{\hspace{1em}(5)}$$

  • 可以看到，对$\mathbf{R}$ 求导后，左侧多出一个$\varphi (t{)}^{\wedge }$

  • 考虑简单的情况：$t_0=0,\mathbf{R}(0)=\mathbf{I}$，即上式微分方程从$0$时刻开始，对$t_0$开始，进行一阶泰勒展开，得到：
    $$\begin{array}{rl}\mathbf{R}(t)& \approx \mathbf{R}(t_0)+\dot{\mathbf{R}(t_0)}(t-t_0)\\ & =\mathbf{I}+\varphi (t_0{)}^{\wedge }(t)\end{array}\text{\hspace{1em}(6)}$$

  • 可见$\varphi$ 反映了一阶导数性质，它位于单位圆附近的正切空间（tangent space）上。

    • 在$t_0$附近，假设$\varphi$ 不变，则根据公式（5），有微分方程：
      $$\dot{\mathbf{R}(t)}=\varphi (t_0{)}^{\wedge }\mathbf{R}(t)={\varphi }_0^{\wedge }\mathbf{R}(t)\text{\hspace{1em}(7)}$$

    • 已知初始情况：$\mathbf{R}(0)=\mathbf{I}$，解之，得：
      $$\mathbf{R}(t)=\exp ({\varphi }_0^{\wedge }t)\text{\hspace{1em}(8)}$$

    • 公式（8）说明，对任意$t$，都可以找到一个$\mathbf{R}$和一个$\varphi$的对应关系：

      • 该关系为指数映射（Exponential Map）
      • 这里的$\varphi$称为$SO(3)$ 对应的李代数： $so(3)$

  • 问题：

    • $so(3)$ 的定义和性质？
    • 指数映射如何求？

• 李代数（Lie Algebra）

  • 每个李群都有与之对应的李代数。

  • 李代数描述了李群单位源数的正切空间性质

  • 李代数需要满足的性质：封自双雅（疯子爽鸭）

• 其中二元运算$[,]$被称为李括号（Lie Bracket）

  • 直观上说，李括号表达了两个元素的差异

• 例子：三维空间向量 + 叉积运算 构成 李代数

• 李代数$so(3)$：
  $$so(3)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}\text{\hspace{1em}(9)}$$

  • 其中:
    $$\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]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$

• 同理，对于李代数$se(3)$：
  $$se(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\text{T}}& 0\end{array}\right]\right\}\text{\hspace{1em}(10)}$$

  • $se(3)$ 由三个平移分量 和 三个旋转分量 组成
    • 旋转和$so(3)$ 相同
    • 平移是普通的向量——注意，不是$SE(3)$上的平移分量！！
  • 上尖尖${}^{\wedge }$ 不再是反对称矩阵，但仍保留记法：

  $${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\text{T}}& 0\end{array}\right]\in 4\times 4$$

  • 李括号

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

  • 这里需要注意的是：
    • 不同书籍对$se(3)$ 的平移/旋转分量的先后顺序定义不同。这里使用平移在前的方式，也有些地方是旋转在前。
    • 把李代数理解成向量形式或者矩阵形式都可以的。向量形式更自然一些。

  4.2 指数映射和对数映射

  4.2.1 $so(3)\leftrightarrow SO(3)$

• 指数映射反映了李代数到李群的对应关系：
  $$\mathbf{R}=\exp ({\varphi }^{\wedge })\text{\hspace{1em}(11)}$$

  • 但是 ${\varphi }^{\wedge }$ 是一个矩阵，对于矩阵，如何定义求指数运算呢？——Taylor展开

  $$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n\text{\hspace{1em}(12)}$$

  • 由于$\varphi$ 是向量，定义其角度和模长：

    • 角度乘单位向量： $\varphi =\theta a$
    • 关于$a$，可以验证以下性质：

    $$a^{\wedge }{a}^{\wedge }=a{a}^{\text{T}}-\mathbf{I}\text{\hspace{1em}(13 a)}$$

    $$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }\text{\hspace{1em}(13 b)}$$

    • 这为化解Taylor展开式中的高阶项提供了有效的方法

  • Taylor 展开：

• 最后得到一个似曾相识的结果：

$$\exp (\theta a^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )a{a}^{\text{T}}+\sin \theta a^{\wedge }\text{\hspace{1em}(14)}$$

• 公式（14）即为：罗德里格斯公式，罗德里格斯公式能够将旋转向量转换为旋转矩阵，这说明李代数到李群的转化为罗德里格斯公式
• 这说明$so(3)$ 的物理意义就是旋转向量
• 反之，给定旋转矩阵时，亦能求李代数（对数运算）：

$$\varphi =\ln (\mathbf{R}{)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}\right)}^{\vee }\text{\hspace{1em}(15)}$$

• 但是实际应用当中，没必要这样求，在旋转向量小节中已经介绍过矩阵到向量的转换关系：
  $$\theta =\arccos (\frac{\text{tr}(\mathbf{R})-1}{2})\text{\hspace{1em}(16 a)}$$

  $$\mathbf{R}n=n\text{\hspace{1em}(16 b)}$$

• 至此，说明了$SO(3)$ 和 $so(3)$ 的对应关系。

  • $so(3)\to SO(3)$ ：指数运算，也就是罗德里格斯公式
  • $So(3)\to sO(3)$：对数运算

  4.2.2 $se(3)\leftrightarrow SE(3)$

• 指数映射：
  $$\begin{array}{rl}\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^{\text{T}}& 1\end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathbf{R}& {\mathbf{J}}_{\rho }\\ {\mathbf{o}}^{\text{T}}& 1\end{array}\right]\\ & =\mathbf{T}\end{array}\text{\hspace{1em}(17)}$$

  • 左上角表示李代数的平移部分到矩阵平移部分，相差一个线性变换（雅可比矩阵），由$\mathbf{J}$给出：

  $$\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^{\text{T}}+\frac{1-\cos \theta }{\theta }{a}^{\wedge }\text{\hspace{1em}(18)}$$

  4.2.3 小总结：$so(3)\leftrightarrow SO(3)$、$se(3)\leftrightarrow SE(3)$的转换关系

4.3 李代数求导与扰动模型

4.3.1 引入李代数的原因

• 在实际应用当中，我们经常需要对位姿进行估计
• 但李群元素只有乘法，没有加法，无从定义导数：

$${\mathbf{R}}_1+{\mathbf{R}}_2\notin SO(3)$$

4.3.2 李代数求导方法

• 那么如何对李群进行求导呢？

• 直观的想法：

  • 能否利用李代数上的加法，定义李群元素的导数？
  • 能否使用指数映射和对数映射完成变换关系

• 基本问题：当在李代数中做加法时，是否等价于在李群上做乘法？
  $$\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })?=\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })\text{\hspace{1em}(19)}$$

  • 在使用标量的情况下，显然，公式（19）是成立的

  • 但这里的${\varphi }^{\wedge }$ 为矩阵！显然公式（19）不成立的

  • 完整形式的BCH（Backer-Campbell-Hausdorff）公式给出：

    • 完整形式非常复杂，需要自行百度

    • 部分展开式：（方括号为李括号）
      $$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots \text{\hspace{1em}(20)}$$

    • 特别的是，当$\mathbf{A}$或者$\mathbf{B}$特别小时，可以只保留一阶项

    • 即：当$\mathbf{A}$或者$\mathbf{B}$ 其中一个量为小量时，忽略其高阶项，BCH具有线性近似形式：
      $$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_1\text{ is small,}\\ {\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{ if }{\varphi }_2\text{ is small.}\end{array}\text{\hspace{1em}(21)}$$

    • 其中：

      • 左乘雅可比矩阵：

      $${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^{\text{T}}+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$

      $${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)a{a}^{\text{T}}-\frac{\theta }{2}{a}^{\wedge }$$

      • 右乘雅可比矩阵：

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

    • 后续以左乘为例展开

    • 以左乘为例，直观的写法：

      • 在李群行左乘小量时，李代数上的加法相差左雅克比矩阵的逆
        $$\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp \left((\varphi +{\mathbf{J}}_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge }\right)\text{\hspace{1em}(22)}$$

      • 反之：
        $$\exp \left((\varphi +\Delta {\varphi }^{\wedge }{)}^{\wedge }\right)=\exp \left(({\mathbf{J}}_l\Delta \varphi {)}^{\wedge }\right)\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp \left(({\mathbf{J}}_r\Delta \varphi {)}^{\wedge }\right)\text{\hspace{1em}(23)}$$

        • 李代数上进行小量加法时，相当于李群上左（右）乘一个带左（右）雅可比的量

      • $se(3)$ 上形式更复杂：
        $$\exp (\Delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\approx \exp \left(({\mathcal{J}}_l^{-1}\Delta \xi +\xi {)}^{\wedge }\right)\text{\hspace{1em}(24 a)}$$

        $$\exp ({\xi }^{\wedge })\exp (\Delta {\xi }^{\wedge })\approx \exp \left(({\mathcal{J}}_r^{-1}\Delta \xi +\xi {)}^{\wedge }\right)\text{\hspace{1em}(24 b)}$$

  • 通过BCH线性近似，可以定义李代数上的导数

  • 考虑一个基本问题：旋转后的点$p$关于旋转的导数：
    $$\text{不严谨地记为： }\frac{\partial \mathbf{R}p}{\partial \mathbf{R}}$$

  • 由于$\mathbf{R}$ 没有加法，导数无从定义

  • 存在两种解决方法：

    • 对$\mathbf{R}$ 对应的李代数加上小量，求相对于小量的变化率（导数模型）
    • 对 $\mathbf{R}$ 左乘或者右乘一个小量，求相对于小量的李代数的变化率（扰动模型）

  • 导数模型

    • 按照定义可以推出：

    $$\begin{array}{rl}\frac{\partial (\exp ({\varphi }^{\wedge })p)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left(({\mathbf{J}}_l\delta \varphi {)}^{\wedge }\right)\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & \approx \underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+({\mathbf{J}}_l\delta \varphi {)}^{\wedge }\right)\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{({\mathbf{J}}_l\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-(\exp ({\varphi }^{\wedge })p{)}^{\wedge }{\mathbf{J}}_l\delta \varphi }{\delta \varphi }\\ & =-(\mathbf{R}p{)}^{\wedge }{\mathbf{J}}_l\end{array}\text{\hspace{1em}(25)}$$

    • 结果中含有左乘雅可比矩阵，比较复杂，因此可以尝试消除

  • 扰动模型

    • 左乘小量，令其李代数为零：（$SO(3)$ 上的扰动模型）

    $$\begin{array}{rl}\frac{\partial (\mathbf{R}p)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & \approx \underset{\phi \to 0}{\lim }\frac{(1+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{R}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}p{)}^{\wedge }\phi }{\phi }\\ & =-(\mathbf{R}p{)}^{\wedge }\end{array}\text{\hspace{1em}(26)}$$

    • 可以看出，相对于导数模型，扰动模型的最终结果更简洁，也更为实用

  • $SE(3)$ 上的扰动模型：
    $$\begin{array}{rl}\frac{\partial \mathbf{T}p}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp (\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })p-\exp ({\mathbf{\xi }}^{\wedge })p}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })p-\exp ({\mathbf{\xi }}^{\wedge })p}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })p}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^{\text{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}p+t\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(\mathbf{R}p+t)+\delta \rho \\ 0\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}p+t{)}^{\wedge }\\ 0^{\text{T}}& 0^{\text{T}}\end{array}\right]\\ & \triangleq (\mathbf{T}p{)}^{\odot }\end{array}\text{\hspace{1em}(27)}$$

4.3.3 小结

• 利用BCH线性近似，可以推导$so(3)$ 和 $se(3)$ 上的导数模型和扰动模型
• 通常情况下，扰动模型形式更为简单，所以更常用一些

4.4 演示：SOPHUS库

GitHub链接：https://github.com/gaoxiang12/slambook

本节对应的是 ch4