第4讲 李群与李代数

第4讲 李群与李代数

1. 目标

1. 理解李群与李代数的概念，掌握$SO(3)、SE(3)$与对应李代数的表示方式；
2. 理解$BCH$近似的意义；
3. 学会在李代数上的扰动模型；
4. 使用$Sophus$对李代数进行运算；

2. 知识点

1. 理解群、李群、李代数的概念与定义；
2. 理解群与李代数满足的性质；
3. 理解$SO(3)和SE(3)$的定义；
4. 理解$so(3)和se(3)$的定义；
5. 推导$SO(3)与so(3)$指数与对数映射；
6. 推导$SE(3)与se(3)$指数与对数映射；
7. 掌握$BCH$近似公式的表达及其意义；
8. 推导$SO(3)$上的李代数求导与左扰动模型；
9. 推导$SE(3)$上的李代数求导与左扰动模型；
10. 试着推导右扰动模型；

3. 实践

1. 基础：$SO(3)、SE(3)、so(3)、se(3)$的定义及$BCH$近似公式：

   1. 三维旋转矩阵构成特殊正交群$SO(3)$（Sepcial Orthogonal Group）

      $SO\left( 3 \right) =\left{ \boldsymbol{R}\in R^{{3×3}|\boldsymbol{RR}}T=\boldsymbol{I}\text{，}\det \left( \boldsymbol{R} \right) =1 \right} $

   2. 三维变换矩阵构成特殊欧氏群$SE(3)$（Special Euclidean Group）

      $SE\left(3\right)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|\mathbf{R}\in SO\left(3\right),\mathbf{t}\in R^3\right\}$

   3. $SO(3)$对应的李代数$so(3)$，位于$R^3$空间，其元素是三维向量或三维反对称矩阵：

      $so\left( 3 \right) =\left{ \phi \in R^3,\boldsymbol{\Phi }=\phi ^{\land}\in R^{3×3} \right} $

   4. $SE(3)$对应的李代数$se(3)$，位于$R^6$空间，由一个平移加上一个$so(3)$元素构成的向量：

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

   5. $BCH$近似公式：

      $\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2\text{当}{\varphi }_1\text{为小量}\\ {\mathbf{J}}_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1\text{当}{\varphi }_2\text{为小量}\end{array}$

   6. $BCH$近似的意义：

      1. 假定对某个旋转矩阵$R$，对应的李代数为$\varphi$，给它左乘一个微小旋转，记作$△\mathbf{R}$，对应的李代数为$△\varphi$。

         那么，在李群上，得到的结果就是$\boldsymbol{J}_l\left( \phi \right) ^{-1}\bigtriangleup \phi +\phi $。合并起来，可以简单地写成：

         $\bigtriangleup \boldsymbol{R}\cdot \boldsymbol{R}=\exp \left( \bigtriangleup \phi ^{\land} \right) \exp \left( \phi ^{\land} \right) =\exp \left( \left( \boldsymbol{J}_l\left( \phi \right) ^{-1}\bigtriangleup \phi +\phi \right) ^{\land} \right) $

      2. 反之，如果在李代数上进行加法，让一个$\varphi$加上$△\varphi$，那么可以近似为李群上带左右雅可比地乘法：

         $\exp \left( \left( \phi +\bigtriangleup \phi \right) ^{\land} \right) =\exp \left( \left( \boldsymbol{J}_l\bigtriangleup \phi \right) ^{\land} \right) \exp \left( \phi ^{\land} \right) =\exp \left( \phi ^{\land} \right) \exp \left( \left( \boldsymbol{J}_r\bigtriangleup \phi \right) ^{\land} \right) $

   7. 矩阵求导顺序：

      假设$a、b、x、y$都是列向量，那么有如下规则：

      ${\lim }_{x\to 0\text{，}y\to 0}\frac{{\left[\begin{array}{cc}\mathbf{a}& \mathbf{b}\end{array}\right]}^T}{{\left[\begin{array}{cc}\mathbf{x}& \mathbf{y}\end{array}\right]}^T}=\frac{d\left[\begin{array}{c}\mathbf{a}\\ \mathbf{b}\end{array}\right]}{d\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]}={\left[\frac{d{\left[\begin{array}{cc}\mathbf{a}& \mathbf{b}\end{array}\right]}^T}{d\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]}\right]}^T={\left[\begin{array}{cc}\frac{d\mathbf{a}}{d\mathbf{x}}& \frac{d\mathbf{b}}{d\mathbf{x}}\\ \frac{d\mathbf{a}}{d\mathbf{y}}& \frac{d\mathbf{b}}{d\mathbf{x}}\end{array}\right]}^T=\left[\begin{array}{cc}\frac{d\mathbf{a}}{d\mathbf{x}}& \frac{d\mathbf{a}}{d\mathbf{y}}\\ \frac{d\mathbf{b}}{d\mathbf{x}}& \frac{d\mathbf{b}}{d\mathbf{x}}\end{array}\right]$

   8. 以上是后续进行指数、对数映射，李代数求导，扰动模型推导的基础；

2. 推导实践

   1. $SO(3)、so(3)、SE(3)、se(3)$指数与对数映射的对应关系:

   2. $SO(3)$上的李代数求导与左扰动模型:

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

         假设对一个空间点$p$进行了旋转，得到了$Rp$。

         现在，要计算旋转之后点地坐标相对于旋转的导数，记作：$\frac{\partial \left(\mathbf{R}p\right)}{\partial \mathbf{R}}$

         由于，$SO(3)$没有加法，所以该导数没法按照导数的定义进行计算。

         设$R$对应的李代数为$\varphi$，转而计算李代数导数：
         $$\frac{\partial \left(\mathbf{R}p\right)}{\partial \mathbf{R}}=\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left(\varphi +\delta \varphi \right)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \varphi \right)}^{\wedge }\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }{\mathbf{J}}_l\delta \varphi }{\delta \varphi }$$

         $$=-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }{\mathbf{J}}_l=-{\left(\mathbf{R}p\right)}^{\wedge }p$$

         说明：

         第一行：李代数形式求导公式；

         第二行：$BCH$左乘近似公式；

         第三行：$\exp \left({\left({\mathbf{J}}_l\delta \varphi \right)}^{\wedge }\right)$的一阶泰勒展开；

         第四行：消去同类项；

         第五行：运用$a\times b=-b\times a$，令$a={\mathbf{J}}_l\delta \varphi ,b=\exp \left({\varphi }^{\wedge }\right)p$；

      2. 左扰动模型：对李群左乘微小扰动，然后对该扰动求导

         另一种求导方式：对$R$进行一次扰动$△\mathbf{R}$，看结果相对于扰动的变化率；

         以左扰动为例，设左扰动$△\mathbf{R}$对应的李代数为$△\varphi$，然后，对$△\varphi$求导，即：
         $$\frac{\partial \left(\mathbf{R}p\right)}{\partial \left(\delta \varphi \right)}=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left(\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left(\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{{\left(\delta \varphi \right)}^{\wedge }\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

         $$=\underset{\delta \varphi \to 0}{\lim }\frac{-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }\delta \varphi }{\delta \varphi }$$

         $$=-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }=-{\left(\mathbf{R}p\right)}^{\wedge }$$

         说明：

         第一行：对李群左乘小扰动；

         第二行：$\exp \left({\left(\delta \varphi \right)}^{\wedge }\right)$的一阶泰勒展开；

         第三行：消去同类项$\exp \left({\varphi }^{\wedge }\right)p$；

         第四行：运用$a\times b=-b\times a$，令$a=\delta \varphi ,b=\exp \left({\varphi }^{\wedge }\right)p$；

   3. $SE(3)$上的李代数求导与左扰动模型;

      1. 李代数求导：略

      2. 左扰动模型：

         假设某空间点$p$经过一次变换$T$（对应李代数$\xi$），得到$Tp$。

         现在，给$T$左乘一个扰动$△\mathbf{T}=exp({\left(\delta \xi \right)}^{\wedge })$，设扰动项的李代数$\delta \xi ={\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T$，那么：

      $$\frac{\partial \left(\mathbf{T}p\right)}{\partial \left(\delta \xi \right)}=\underset{\delta \xi \to 0}{\lim }\frac{\exp \left({\left(\delta \xi \right)}^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)p-\exp \left({\xi }^{\wedge }\right)p}{\delta \xi }\left(1\right)$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left(\delta \xi \right)}^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)p-\exp \left({\xi }^{\wedge }\right)p}{\delta \xi }\left(2\right)$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{{\left(\delta \xi \right)}^{\wedge }\exp \left({\xi }^{\wedge }\right)p}{\delta \xi }\left(3\right)$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}{\left(\delta \varphi \right)}^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}p+\mathbf{t}\\ 1\end{array}\right]}{\delta \xi }\left(4\right)$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}{\left(\delta \varphi \right)}^{\wedge }\left(\mathbf{R}p+\mathbf{t}\right)+\delta \rho \\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\left(5\right)$$

      $$=\left[\begin{array}{cc}\frac{\partial \left({\left(\delta \varphi \right)}^{\wedge }\left(\mathbf{R}p+\mathbf{t}\right)+\delta \rho \right)}{\delta \rho }& \frac{\partial \left({\left(\delta \varphi \right)}^{\wedge }\left(\mathbf{R}p+\mathbf{t}\right)+\delta \rho \right)}{\delta \varphi }\\ 0^T& 0\end{array}\right]\left(6\right)$$

      $$=\left[\begin{array}{cc}\mathbf{I}& -\left(\mathbf{R}p+\mathbf{t}\right)\\ 0^T& 0\end{array}\right]\stackrel{def}{=}{\left(\mathbf{T}p\right)}^{\odot }\left(7\right)$$

      说明：

      （5）、（6）涉及矩阵求导顺序；

3. 编程实践

   1. useSophus

      1. 安装Sophus
      2. 构造$SO(3)、SE(3)$对象，并进行指数、对数映射；
      3. 当知道更新量后，如何对李群元素进行更新；

   2. trajectoryError

      1. 读取文件
      2. SO(3)和Rotation_matrix是一样的，Sophus::SE3d和Isometry是一样的表达；
      3. 计算RMSE；

4. 课后习题

1. 验证$SO(3)、SE(3)和Sim(3)$关于乘法成群。

   1. 理解$SO(3)、SE(3)和Sim(3)$的定义；
   2. 乘法成群的意思是，满足群的四个性质：封闭性、结合率、幺元、逆；

2. 验证$(R^3，R，\ast )$构成李代数。*表示叉乘

   1. $R^3$表示一个三维向量$(a_1,a_2,a_3)$；
   2. 构成李代数的意思是，满足李代数的四个性质：封闭性、双线性、自反性、雅可比等价性；

3. 验证$so(3)和se(3)$满足李代数要求的性质。

   1. 理解$so(3)、se(3)$的定义；
   2. 李代数的四个性质；

4. 验证性质(4.20)和(4.21)。

   由于$\varphi$是三维向量。其模长和方向，分别记作$\theta$和$a$，于是有$\varphi =\theta a$；

   1. (4.20)公式推导：

      $a^{\land}a{\land}=\left[ \begin{matrix}
      0& -a_3& a_2\
      a_3& 0& -a_1\
      -a_2& a_1& 0\
      \end{matrix} \right] \left[ \begin{matrix}
      0& -a_3& a_2\
      a_3& 0& -a_1\
      -a_2& a_1& 0\
      \end{matrix} \right] $

   2. (4.21)公式推导：

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

   3. 这两个式子，提供了处理$a^{\wedge }$高阶项的方法。

5. 证明：

   ${\mathbf{Rp}}^{\wedge }{\mathbf{R}}^T={\left(\mathbf{Rp}\right)}^{\wedge }$

   第四讲课后习题-参考

6. 证明：

   $\boldsymbol{R}\exp \left( \boldsymbol{p}^{\land} \right) \boldsymbol{R}^T=\exp \left( \left( \boldsymbol{Rp} \right) ^{\land} \right) $

   该式称为$SO(3)$上的伴随性质。同样地，在$SE(3)$上也有伴随性质：

   $\boldsymbol{T}\exp \left( \boldsymbol{\xi }^{\land} \right) \boldsymbol{T}^{-1}=\exp \left( \left( Ad\left( T \right) \xi \right) ^{\land} \right) $

   $Ad\left( T \right) =\left[ \begin{matrix}
   \boldsymbol{R}& \boldsymbol{t}^{\land}\boldsymbol{R}\
   0& \boldsymbol{R}\
   \end{matrix} \right] $

   这个伴随性质有啥意义或用处？

7. 仿照左扰动的推导，推导$SO(3)和SE(3)$在右扰动下的导数。

   1. $SO(3)$右扰动模型：
      $$\frac{\partial \left(\mathbf{Rp}\right)}{\partial \left(\mathbf{\delta \varphi }\right)}=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\varphi }^{\wedge }\right)\exp \left({\left(\delta \varphi \right)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

      $$=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\varphi }^{\wedge }\right)\left(\mathbf{I}+{\left(\delta \varphi \right)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

      $$=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\varphi }^{\wedge }\right){\left(\delta \varphi \right)}^{\wedge }p}{\delta \varphi }$$

      $$=\underset{\delta \varphi \to 0}{\lim }\frac{{\left(\exp \left({\varphi }^{\wedge }\right)\left(\delta \varphi \right)\right)}^{\wedge }\left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\delta \varphi }$$

      $$=\underset{\delta \varphi \to 0}{\lim }\frac{-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }\left(\exp \left({\varphi }^{\wedge }\right)\left(\delta \varphi \right)\right)}{\delta \varphi }$$

      $$=-{\left(\exp \left({\varphi }^{\wedge }\right)p\right)}^{\wedge }\exp \left({\varphi }^{\wedge }\right)=-{\left(\mathbf{Rp}\right)}^{\wedge }\mathbf{R}$$

   2. $SE(3)$右扰动模型：
      $$\frac{\partial \left(\mathbf{Tp}\right)}{\partial \left(\mathbf{\delta \xi }\right)}=\underset{\delta \xi \to 0}{\lim }\frac{\exp \left({\xi }^{\wedge }\right)\exp \left({\left(\delta \xi \right)}^{\wedge }\right)\mathbf{p}-\exp \left({\xi }^{\wedge }\right)\mathbf{p}}{\delta \xi }$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\exp \left({\xi }^{\wedge }\right)\left(\mathbf{I}+{\left(\delta \xi \right)}^{\wedge }\right)\mathbf{p}-\exp \left({\xi }^{\wedge }\right)\mathbf{p}}{\delta \xi }$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\exp \left({\xi }^{\wedge }\right){\left(\delta \xi \right)}^{\wedge }\mathbf{p}}{\delta \xi }$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{\left(\mathbf{\delta \varphi }\right)}^{\wedge }& \mathbf{\delta \rho }\\ 0^T& 0\end{array}\right]\mathbf{p}}{\delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{\left(\mathbf{\delta \varphi }\right)}^{\wedge }& \mathbf{\delta \rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{p}\\ 1\end{array}\right]}{\delta \xi }$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{\left(\mathbf{\delta \varphi }\right)}^{\wedge }\mathbf{p}+\mathbf{\delta \rho }\\ 0\end{array}\right]}{\delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\mathbf{R}{\left(\mathbf{\delta \varphi }\right)}^{\wedge }\mathbf{p}+\mathbf{R\delta \rho }\\ 0\end{array}\right]}{\delta \xi }$$

      $$=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\mathbf{R}{\left(\mathbf{\delta \varphi }\right)}^{\wedge }\mathbf{p}+\mathbf{R\delta \rho }\\ 0\end{array}\right]}{{\left[\begin{array}{cc}\mathbf{\delta \rho }& {\left(\mathbf{\delta \varphi }\right)}^{\wedge }\end{array}\right]}^T}=\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}-{\left(\mathbf{Rp}\right)}^{\wedge }\mathbf{R\delta \varphi }+\mathbf{R\delta \rho }\\ 0\end{array}\right]}{{\left[\begin{array}{cc}\mathbf{\delta \rho }& {\left(\mathbf{\delta \varphi }\right)}^{\wedge }\end{array}\right]}^T}$$

      $$=\left[\begin{array}{cc}\mathbf{R}& -{\left(\mathbf{Rp}\right)}^{\wedge }\mathbf{R}\\ 0& 0\end{array}\right]$$

8. 搜索cmake的find_package指令是如何运作的。它有哪些可选的参数？为了让cmake找到某个库，需要哪些先决条件？

   参考