视觉SLAM十四讲|【二】李群与李代数

视觉SLAM十四讲|【二】李群与李代数

李群与李代数基础

群的性质

• 封闭性： $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
• 结合律： $\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)$
• 么元： $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
• 逆： $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

$$R_1{R}_2\in SO(3)$$
$$T_1{T}_2\in SE(3)$$

李代数的引出

定义

设一个三维向量$\varphi (t)\in {\mathbb{R}}^3$，有
$$\varphi (t{)}^{\wedge }=\dot{R}(t)R(t{)}^T$$
注意，$\wedge$为矩阵化符号，得到一关于$\varphi$的反对称矩阵，因此该等式要求$\dot{R}(t)R(t{)}^T$为反对称矩阵。

证明

三维向量$\varphi (t)$有什么意义呢？我们要联系一下旋转向量的性质：
$$R{R}^T=I$$
$$R(t)R(t{)}^T=I$$
两边对时间求导，可以得到如下形式
$$\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0$$
$$\dot{R}(t)R(t{)}^T=-R(t)\dot{R}(t{)}^T$$
$$\dot{R}(t)R(t{)}^T=-(\dot{R}(t)R(t{)}^T{)}^T$$
得证$\dot{R}(t)R(t{)}^T$为反对称矩阵。

性质

$$\varphi (t{)}^{\wedge }=\dot{R}(t)R(t{)}^T$$
两边同时右乘$R(t)$，有
$$\varphi (t{)}^{\wedge }R(t)=\dot{R}(t)R(t{)}^{T}R(t)$$
由于
$$R(t{)}^{T}R(t)=I$$
得到
$$\varphi (t{)}^{\wedge }R(t)=\dot{R}(t)$$
即对$\varphi (t{)}^{\wedge }$通过右乘$R(t)$，或者对$R(t)$左乘$\varphi (t{)}^{\wedge }$，我们可以实现对旋转矩阵的求导
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)$$
设$t=0$时有$R(0)=I$，$t_0=0$进行一阶泰勒展开有如下形式
$$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\varphi (t_0{)}^{\wedge }(t)$$
又有
$$\dot{R}(t)=\varphi (t_0{)}^{\wedge }R(t)$$
解该微分方程，可以得到四元数与旋转矩阵的关系如下所示
$$R(t)=exp({\varphi }_0^{\wedge }t)$$

指数与对数映射

SO(3)上的指数映射

对于三维向量$\varphi$，我们令$\varphi =\theta a$，$\theta$为模长，$a$为单位方向向量，有以下性质：
$$a^{\wedge }{a}^{\wedge }=a{a}^T-I$$
$$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$$
$$exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }$$
反回来，旋转向量的对数映射有
$$\varphi =ln(R{)}^{\vee }=(\sum _{n=0}\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }$$

SE(3)上的指数映射

$$exp({ϵ}^{\wedge })=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]=T$$
其中，
$$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$$

李代数求导与扰动模型

${\varphi }_1$为小量，有
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx J_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2$$

${\varphi }_2$为小量，有
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1$$

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

$$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}{a}^{\wedge }$$
$$J_r(\varphi )=J_l(-\varphi )$$

补充性质

$$ln(Rexp({\varphi }^{\wedge }){)}^{\vee }=ln(R{)}^{\vee }+J_r^{-1}\varphi$$
SO(3)伴随性质：
$$R^{T}exp({\varphi }^{\wedge })R=exp((R^T\varphi {)}^{\wedge })$$
$$a^{\wedge }b=-b^{\wedge }a$$