视觉SLAM笔记(二)-李群与李代数基础

在前面的刚体运动中我们提到了两个群 $SO(3)$和 $SE(3)$，这两个群为所谓的李群。注意到这两个群对于加法是不封闭的，但对于乘法是封闭的，即两个旋转矩阵相加不再是一个旋转矩阵，相乘则还是一个旋转矩阵(从 $det(R)=1$这点上很好理解)。下面给出群的定义: $Group=set(G)+operation(\cdot )$，其满足如下几个条件:

1. 封闭性: $g_1\cdot g_2\in G,\forall g_1,g_2\in G$
2. 结合律: $(g_1\cdot g_2)\cdot g_3=g_1\cdot (g_2\cdot g_3),\forall g_1,g_2,g_3\in G$
3. 幺元: $\exists e\in G:e\cdot g=g\cdot e=g,\forall g\in G$
4. 逆: $\exists g^{-1}\in G:g\cdot g^{-1}=g^{-1}\cdot g=e,\forall g\in G$

李群与李代数

李群是指具有连续光滑性质的群，由$R{R}^T=I$，我们可以得到
$$\frac{d}{dt}(R{R}^T)=\dot{R}{R}^T+R{\dot{R}}^T\Rightarrow \dot{R}{R}^T=-(\dot{R}{R}^T{)}^T$$
所以可以看到$\dot{R}{R}^T$是一个反对称矩阵，可以用$\hat{w}$表示，$\hat{}$符号将一个三维的向量映射为一个$3\times 3$的反对称矩阵，若$\varphi =[{\varphi }_1,{\varphi }_2,{\varphi }_3]$，则
$$\hat{\varphi }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$$
所以可得$\dot{R}(t)R^T(t)=\hat{\varphi }(t)$，即$\dot{R}(t)=\hat{\varphi }(t)R(t)$
这里的$\hat{\varphi }$即为$SO(3)$所对应的李代数$so(3)$，可以看到，李代数实际上反映了李群的正切空间，反映了李群的局部导数，这一点在后面的优化问题中十分重要，因为优化问题经常需要求变量的导数。
现在给出李代数的定义，李代数由一个集合$V$，一个数域$F$和一个二元运算$[,]$(李括号)组成，满足如下几条性质

1. 封闭性: $\forall X,Y\in V,[X,Y]\in V$
2. 双线性: $\forall X,Y,Z\in V,a,b\in F,[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$
3. 自反性: $\forall X\in V,[X,X]=0$
4. 雅可比等价: $\forall X,Y,Z\in V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$
   对应于$SO(3)$的李代数如下:
   s o ( 3 ) = { Φ = ϕ ^ ∈ R 3 × 3 ∣ ϕ ∈ R 3 } so(3)=\lbrace \Phi=\hat{\phi} \in R^{3\times3}| \phi \in R^3 \rbrace so(3)={Φ=ϕ^​∈R3×3∣ϕ∈R3}
   李括号如下
   $$[{\varphi }_1,{\varphi }_2]=[{\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{]}^{\bigvee }$$
   对应于$SE(3)$的李代数如下:
   s e ( 3 ) = { ξ ^ = [ ϕ ^ ρ 0 T 0 ] ∈ R 4 × 4 ∣ ρ ∈ R 3 , ϕ ^ ∈ s o ( 3 ) } se(3) = \lbrace \hat{\xi} = \begin{bmatrix} \hat{\phi} & \rho\\ 0^T & 0 \end{bmatrix} \in R^{4\times4} | \rho \in R^3, \hat{\phi} \in so(3) \rbrace se(3)={ξ^​=[ϕ^​0T​ρ0​]∈R4×4∣ρ∈R3,ϕ^​∈so(3)}
   李括号如下
   $$[{\xi }_1,{\xi }_2]=(\widehat{{\xi }_1}\widehat{{\xi }_2}-\widehat{{\xi }_2}\widehat{{\xi }_1}{)}^{\bigvee }$$

Exponential and Logarithm

SO(3)与so(3)

由
$$\begin{gathered} \dot{R}(t)=\hat{\varphi }R(t) \\ R(0)=I \end{gathered}$$
解得
$$R(t)=exp(\hat{\varphi }t)$$
由指数函数的泰勒展开:
$$exp(\hat{\varphi })=\sum _{n=0}^{\infty }\frac{1}{n!}(\hat{\varphi }{)}^n$$
令$\varphi =\theta a,\theta =|\varphi |$，这里$a$有如下性质
$${\hat{a}}^2=a{a}^T-I,{\hat{a}}^3=-\hat{a}$$
带入上式中得
$$exp(\hat{\varphi })=exp(\theta \hat{a})=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta \hat{a}{)}^n$$
由上述$a$的性质可得(中间主要是展开公式并合并$\hat{a}$和${\hat{a}}^2$项，并利用了三角函数的泰勒展开形式)
$$exp(\hat{\varphi })=cos\theta +(1-cos\theta )a{a}^T+sin\theta \hat{a}$$
此时便完成了由李代数到李群的映射，由李群到李代数的映射通常用trace的性质计算，得到其旋转向量和转角
若$R=(r_{ij})$，则
$$\begin{gathered} \theta =co{s}^{-1}(\frac{trace(R)-1}{2}) \\ a=\frac{1}{2sin\theta }\left[\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right] \end{gathered}$$

SE(3)与se(3)

这里仅给出公式:
$$exp(\hat{\xi })=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]$$
$R$可以通过$\xi$中的$\varphi$计算得到，而$J$的形式如下
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }\hat{a}$$

李代数求导

BCH近似

BCH近似的目的是如何用李代数上的加法来代替李群上的矩阵乘法，当对一个旋转矩阵$R_2$左乘一个微小量$R_1$，近似看作在原有李代数${\varphi }_2$基础上加上一项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$，当对一个旋转矩阵$R_1$右乘一个微小量$R_2$，近似看作在原有李代数${\varphi }_1$基础上加上一项$J_r({\varphi }_1{)}^{-1}{\varphi }_2$，其中
$$J_l=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }\hat{a}$$
$$J_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}\hat{a}$$
$$J_r(\varphi )=J_l(-\varphi )$$
假设对某个旋转$R$，对应的李代数为$\varphi$，做成一个$\Delta R$，对应李代数为$\Delta \varphi$，则在李代数上
$$exp(\Delta \hat{\varphi })exp(\hat{\varphi })=exp((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })$$
同理，
$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp(\hat{\varphi })exp((J_r\Delta \varphi {)}^{\wedge })$$
对于$SE(3)$，也有如上公式
$$exp(\Delta \hat{\xi })exp(\hat{\xi })=exp((\varphi +J_l^{-1}(\xi )\Delta \xi {)}^{\wedge })$$

$$exp((\xi +\Delta \xi {)}^{\wedge })=exp(\hat{\xi })exp((J_r\Delta \xi {)}^{\wedge })$$

李代数上的求导

使用李代数解决求导问题的思路分为两种:

1. 用李代数表示姿态，根据李代数加法对李代数求导
2. 对李群左乘或右乘微小扰动，对该扰动求导

李代数求导

$$\frac{\partial (RP)}{\partial R}=\frac{\partial (exp(\hat{\varphi })p)}{\partial \varphi }=-(Rp{)}^{\wedge }{J}_l$$
由于结果中含有 $J_l$，这个矩阵形式较为复杂，故我们使用下面的扰动模型

扰动模型

以左扰动为例，在$R$的左边乘一个微小量$\Delta R$，对应的李代数为$\phi$，对$\phi$求导
$$\begin{gathered} \frac{\partial (RP)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp(\hat{\phi })exp(\hat{\varphi })p-exp(\hat{\varphi })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(1+\hat{\phi })exp(\hat{\varphi })p-exp(\hat{\varphi })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{\hat{\phi }Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge } \end{gathered}$$
这里相比较上一种求导方式省去了$J_l$的计算

SE(3)上求导

同样的利用扰动模型，在$T$上左乘一个扰动$\Delta T=exp(\delta \hat{\xi })$，设扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$
$$\frac{\partial (Tp)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
最后结果为一个$4\times 6的矩阵$

综上所述，SLAM中李群和李代数的定义主要是为了方便计算变换矩阵的导数，$SO(3)$和$SE(3)$分别代表了旋转和三维刚体运动，$so(3)$和$se(3)$定义了其上的李代数，代表了李群上的正切空间，最后通过扰动模型可以方便的计算出李群上的导数，而避免了计算复杂的雅可比矩阵。

参考文献

[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019
[3] Barfoot T D. State Estimation for Robotics[M]. Cambridge University Press, 2017.