第四讲:李群和李代数

第四讲:李群和李代数

学习目标

• 理解李群和李代数,掌握SO(3),SE(3)与对应李代数的表示方式.
• 理解BCH近似的意义
• 学会在李代数上的扰动模型
• 使用Sophus对李代数进行运算

通过李群李代数的转换关系,我们希望把位姿估计变成无约束的优化问题,简化求解方式.

1. 李群与李代数基础

回顾:

• 特殊正交群: SO(3)

  S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } , R 为 旋 转 矩 阵 SO(n) = \{R \in R^{n \times n} | RR^T =I, det(R)=1 \} , \quad R为旋转矩阵 SO(n)={R∈Rn×n∣RRT=I,det(R)=1},R为旋转矩阵

• 特殊欧式群: SE(3)

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

R,T矩阵对加法不是封闭的, 例如:
$${\mathbf{R}}_1+{\mathbf{R}}_2\notin SO(3)$$

R,T矩阵对乘法是封闭的, 例如:

$${\mathbf{R}}_1{\mathbf{R}}_2\in SO(3),{\mathbf{T}}_1{\mathbf{T}}_2\in SE(3)$$

乘法对应着旋转或者变化的复合, 两个旋转矩阵相乘表示做了两次旋转. 对于这种只有一个运算的集合我们称之为群.

1.1. 群

群是一种集合加上一种运算的代数结构.

把集合记做A,运算记做$\cdot$,那么群可以记做$G=(A,\cdot )$. 群要求这个运算满足以下条件:

1. 封闭性:$\forall a_1,a_2\in A,a_1\cdot a_2\in A$
2. 结合律:$\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
3. 幺元:$\exists a_0\in A,\mathbf{s}.\mathbf{t}.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆:$\forall a\in A,\exists a^{-1}\in A,\mathbf{s}.\mathbf{t}.a\cdot a^{-1}=a_0$

$\mathbf{s}.\mathbf{t}.$ 表示: 使得...

上述的四种性质简称为:封结幺逆

矩阵中常见的群:

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

李群: 指的是具有连续(光滑)性质的群. 例如,一个一个刚体能够连续的在空间中运动,他们被称为李群.

1.2. 李代数的引出

任意旋转矩阵R,满足:
$$\mathbf{R}{\mathbf{R}}^{\mathbf{T}}=\mathbf{I}$$

现在,认为R是某个相机的旋转,R会随着时间变化,则相机的旋转可以表示为R(t). 它任然满足如下性质:
$$\mathbf{R}(\mathbf{t})\mathbf{R}(\mathbf{t}{)}^{\mathbf{T}}=\mathbf{I}$$

对时间求导得到:

$$\begin{gathered} R^{\prime }(t)R(t{)}^T+R(t)R^{\prime }(t{)}^T=0 \\ \Rightarrow \\ {R}^{\prime }(t)R(t{)}^T=-(R^{\prime }(t)R(t{)}^T{)}^T \end{gathered}$$

由以上推导可知$R^{\prime }(t)R(t{)}^T$是一个反对称矩阵.我们在之前用符号: ^ 表示将一个向量变成反对称.同理,用符号:${}^{\vee }$表示一个反对称矩阵变成一个向量.

• ${}^{\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$$

同理,对于 $R^{\prime }(t)R(t{)}^T$ 可以找到一个三维向量$\varphi (t)\in {\mathbb{R}}^3$与之对应. 于是有:
$$R^{\prime }(t)R(t{)}^T=\varphi (t{)}^{\wedge }$$

等式两遍右乘以 R(t):
$$R^{\prime }(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)$$

可知:每对旋转轴求一次导,只需要左乘一个 $\varphi (t{)}^{\wedge }$ 矩阵即可.

1.3. 李代数的定义

李代数：李代数是一个集合$\mathbb{V}$,一个数域$\mathbb{F}$,和一个二元运算符[,]组成,如果他们满足以下性质,那么称$(\mathbb{V},\mathbb{F},[,])$为一个李代数,记作$g$

1. 封闭性: $\forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}$
2. 双线性: $\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. 自反性: $\forall X\in \mathbb{V},[X,X]=0$
4. 雅可比等价: $\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

其中的二元运算符被称为李括号.

由于三位向量上定义的$\times$满足上述四条性质,所以$g=({\mathbb{R}}^3,\mathbb{R},\times )$ 构成一个李代数.

${\mathbb{R}}^3$表示集合; $\mathbb{R}$表示数域; $\times$ 表示二元运算.

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

之前的$\varphi$ 实际上是一种李代数.

$SO(3)$对应的李代数是定义在${\mathbb{R}}^3$上的向量$\varphi$.

根据之前的定义,$\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\times 3}$$

两个向量的李括号定义为:
$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$

$\mathfrak{s}\mathfrak{o}(3)$的元素是三维向量或者三维反对称矩阵.
$$\mathfrak{s}\mathfrak{o}(3)=\left[\begin{array}{c}\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\end{array}\right]$$

$\mathfrak{s}\mathfrak{o}(3)$的内容为:由一个三维向量组成的集合,每个向量对应一个反对称矩阵,可以表达旋转矩阵的导数.

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

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

指数映射会在后面介绍

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

$\mathfrak{s}\mathfrak{e}(3)$位于${\mathbb{R}}^6$空间中:
$$\mathfrak{s}\mathfrak{e}(3)=\left\{\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),{\rho }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$

$\mathfrak{s}\mathfrak{e}(3)=\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]$为六维向量. $\rho$ 表示平移; $\varphi$ 表示旋转.

将一个六维向量转换成四维矩阵(但是不在表示反对称):
$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

$\mathfrak{s}\mathfrak{e}(3)$ 的李括号为:
$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }\xi 21^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$

2. 指数与对数映射

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

任意矩阵的指数映射可以写成一个泰勒展开,只有在收敛的情况下才有结果,其结果任然是矩阵:
$$exp(A)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n$$

同理,定义$\mathfrak{s}\mathfrak{o}(3)$ 中任意元素 $\varphi$ 的指数映射:
$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$

$\varphi$ 是一个三维向量, 它的模长和方向分别记做:$\theta ,\mathbf{a}$($\mathbf{a}$为长度为1的单位向量),$\mathbf{a}$有如下两条性质:
$$\begin{gathered} {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T=\mathbf{I} \\ {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge } \end{gathered}$$

那么指数映射可以写成:
$$exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=...=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }=\mathbf{R}$$

上式即为第三讲中的罗德里格斯公式

把旋转角度固定在 $\pm \pi$ 之间, 那么李群和李代数元素之间一一对应.

2.2. $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}\mathbf{\rho }\\ 0^T& 1\end{array}\right]=\mathbf{T}$$

李群和李代数之间的相互转化总结:

1. 对于SO(3):

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

   李代数: $\mathfrak{s}\mathfrak{o}(3),\varphi \in R^3,{\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$

   李群–>李代数:
   $$\theta =\arccos \frac{tr(R)-1}{2},Ra=a,\varphi =\theta a$$

   李代数–>李群:
   $$R=\exp (\theta a^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge },\varphi =\theta a(\theta 为\varphi 模长,a为\varphi 的方向)$$

2. 对于SE(3):

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

   李代数:
   $\mathfrak{s}\mathfrak{e}(3),\xi =\left[\begin{array}{c}\rho \\ \theta \end{array}\right]\in {\mathbb{R}}^6,{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$

   李群–>李代数:

   $$\theta =\arccos \frac{tr(R)-1}{2},Ra=a,t=Jp,\varphi =\theta a$$

   李代数–>李群:

   $$T=\exp ({\xi }^{\wedge })=\left[\begin{array}{cc}\exp ({\varphi }^{\wedge })& Jp\\ 0^T& 1\end{array}\right],J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge },\varphi =\theta a$$

3. 李代数的求导与扰动模型

3.1. BCH公式与近似计算

两个李代数指数映射的乘积为(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]]+\cdots$$

其中[]为李括号

当${\varphi }_1或{\varphi }_2$为小量时,小量二次以上的项都可以被忽略:
$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& \text{当ϕ1为小量}\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& \text{当ϕ2为小量}\end{array}$$

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

对$J_l$求导:
$$J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)a{a}^T-\frac{\theta }{2}{a}^{\wedge }$$

右乘雅可比$T_r$:
$$J_r(\varphi )=J_l(-\varphi )$$

BCH的近似意义:
假设对于某个旋转R,对应李代数为$\varphi$.给它左乘一个微小的旋转,记做$\Delta R$ ,对应的李代数记做$\Delta \varphi$.那么在李群上得到的结果为$\Delta R\cdot R$,根据BCH近似,$J_l(\varphi {)}^{-1}\Delta \varphi +\varphi$

李群上的乘法:

$$\Delta R\cdot R=\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp ((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })$$

李代数上的加法:

$$\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ((J_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })$$

3.2. SO(3)李代数上的求导

假设:

• 位姿: T
• 观察到一个世界坐标: p
• 产生观测数据: z

$$z=Tp+w,w为噪声$$

误差值e:
$$e=z-Tp$$

假设有N组数据,使得整体误差最小:

$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$

对位姿进行求导,以为SO(3),SE(3)没有定义良好的加法,所以只能李代数解决求导问题:

分为两种思路:

1. 用李代数表示姿态, 然后根据李代数加法对李代数求导. --对应求导模型
2. 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型. --对应扰动模型

3.3. 李代数求导

假设对空间点p进行旋转,得到:$Rp$. 现在要计算旋转之后的点坐标相对与旋转的导数.记做:
$$\frac{\partial (Rp)}{\partial R}$$
由于SO(3)没有加法,固无法直接计算,所以转换为李代数:
$$\frac{\partial (\exp ({\varphi }^{\wedge })p)}{\partial \varphi }\underset{\delta \varphi \to 0}{=}-(Rp{)}^{\wedge }{J}_l$$

即:

$$\frac{\partial (Rp)}{\partial R}=(-Rp{)}^{\wedge }{J}_l$$

扰动模型将会提供更简单的计算方式.

3.4. 扰动模型

以左扰动为例:
$$\frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }$$

即:
$$\frac{\partial (Rp)}{\partial \phi }=-(Rp{)}^{\wedge }$$

相比之下,扰动模型更为实用.

3.5. 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 }\underset{\delta \xi \to 0}{=}\left[\begin{array}{cc}\mathbf{I}& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (Tp{)}^{\odot }$$

最后的结果定义成一个运算符 $\odot$, 它把一个齐次坐标的空间点变成一个$4\times 6$的矩阵.

4. 实践:Sophus

见code/第四讲/