SLAM基础：李群李代数

叉乘与反对称矩阵

$$\mathbf{a}\times \mathbf{b}=\left|\begin{array}{ccc}{\vec{\mathbf{e}}}_1& {\vec{\mathbf{e}}}_2& {\vec{\mathbf{e}}}_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right|=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\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]\mathbf{b}\stackrel{\mathrm{def}}{=}{\mathbf{a}}^{\wedge }\mathbf{b}$$
对于外积运算，引入${}^{\wedge }$符号，把$\mathbf{a}$写成一个反对称矩阵。这样就把外积$\mathbf{a}\times \mathbf{b}$写成了矩阵与向量的乘法${\mathbf{a}}^{\wedge }\mathbf{b}$

李群与李代数基础

群

群是一种集合加上一种运算的代数结构。把集合记作$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,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆：$\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

李群是指具有连续(光滑)性质的群。引入李群李代数是为了解决旋转矩阵不可导的问题： ${\mathbf{R}}_1+{\mathbf{R}}_2\notin SO(3)$。

李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。一般的李代数定义如下：
李代数由一个集合$\mathbb{V}$、一个数域$\mathbb{F}$和一个二元运算$[,]$组成。如果它们满足如下几条性质，则称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$\mathfrak{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$

其中二元运算被称为李括号。

旋转矩阵与$\mathfrak{so}(3)$

对于旋转矩阵$\mathbf{R}$，其为正交矩阵，故
$$\mathbf{R}\left(t\right){\mathbf{R}}^T\left(t\right)=\mathbf{I}$$
等式两边对t求导得
$$\begin{gathered} \dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)+\mathbf{R}\left(t\right)\dot{\mathbf{R}}{\left(t\right)}^T=0 \\ \dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)=-{\left(\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)\right)}^T \end{gathered}$$
这是反对称矩阵的性质。对于任意反对称矩阵，都能找到唯一与之对应的向量$\mathbf{\omega }$，使得其对应的反对称矩阵
$$\begin{gathered} {\mathbf{\omega }}^{\wedge }\left(t\right)=\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right) \\ {\mathbf{\omega }}^{\wedge }=\Omega =\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right] \\ {\Omega }^{\vee }=\mathbf{\omega } \end{gathered}$$
则
$$\begin{gathered} \dot{\mathbf{R}}\left(t\right)={\mathbf{\omega }}^{\wedge }\left(t\right)\mathbf{R}\left(t\right) \\ ⟹\mathbf{R}(t)=e^{{\mathbf{\omega }}^{\wedge }(t-t_0)}\mathbf{R}(t_0) \end{gathered}$$
设$\mathbf{R}(t_0)=\mathbf{I}$，则$\dot{\mathbf{R}}(t_0)={\mathbf{\omega }}^{\wedge }(t_0)$
$$\mathbf{R}\left(t_0+dt\right)\approx \mathbf{I}+\dot{\mathbf{R}}\left(t_0\right)dt=\mathbf{I}+{\mathbf{\omega }}^{\wedge }(t_0)dt$$
这些反对称矩阵对应的向量就组成了李代数 s o ( 3 ) = { ω ∈ R 3 ∣ ω ∧ ∈ R 3 × 3 } so(3)=\{ \bm{\omega}\in \mathbb{R}^3|{\bm{\omega}^{\wedge}}\in \mathbb{R}^{3\times 3}\} so(3)={ω∈R3∣ω∧∈R3×3}
李群SO(3)为 S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , det ⁡ ( R ) = + 1 } SO(3)=\{\boldsymbol{R}\in\mathbb{R}^{3\times 3}\vert \boldsymbol{R}^T\boldsymbol{R}=\bold{I},\det(\boldsymbol{R})=+1\} SO(3)={R∈R3×3∣RTR=I,det(R)=+1}
由前面的推导公式知道如果R是单位矩阵，那它的导数就是一个反对称矩阵，所以反对称矩阵对应的向量组成的空间，即 so(3)，我们称之为在在单位矩阵处的正切空间tangent space.
在此定义下：两个向量${\mathbf{\varphi }}_1,{\mathbf{\varphi }}_2$的李括号为
$$[{\mathbf{\varphi }}_1,{\mathbf{\varphi }}_2]=({\mathbf{\varphi }}_1^{\wedge }{\mathbf{\varphi }}_2^{\wedge }-{\mathbf{\varphi }}_2^{\wedge }{\mathbf{\varphi }}_1^{\wedge }{)}^{\vee }$$

指数映射

由前所述
$$\dot{\mathbf{R}}(t)={\mathbf{\omega }}^{\wedge }(t)\mathbf{R}(t)$$
令$\mathbf{R}(0)=\mathbf{I}$，求解该微分方程得到
$$\mathbf{R}(t)=e^{{\mathbf{\omega }}^{\wedge }t}$$
可以验证，这个指数函数是旋转矩阵
$${\left(e^{{\mathbf{\omega }}^{\wedge }t}\right)}^{-1}=e^{-{\mathbf{\omega }}^{\wedge }t}=e^{{\mathbf{\omega }}^{\wedge T}t}={\left(e^{{\mathbf{\omega }}^{\wedge }t}\right)}^T$$
所以，我们能够说指数映射将反对称矩阵映射到了SO(3)：

令${\mathbf{\omega }}^{\wedge }t=\Vert \mathbf{\omega }t\Vert \frac{{\mathbf{\omega }}^{\wedge }}{\Vert \mathbf{\omega }\Vert }=\theta {\mathbf{a}}^{\wedge }$ ，其中$\Vert \mathbf{a}\Vert =1$是单位向量，有如下性质
$$\begin{gathered} {{\mathbf{a}}^{\wedge }}^2={\mathbf{aa}}^T-\mathbf{I} \\ {{\mathbf{a}}^{\wedge }}^3=-{\mathbf{a}}^{\wedge } \end{gathered}$$
指数映射的泰勒展开式能够整理成如下形式：
$$\begin{gathered} e^{{\mathbf{\omega }}^{\wedge }t}=\sum _{n=0}^{\infty }\frac{{\left(\theta {\mathbf{a}}^{\wedge }\right)}^n}{n!}=\mathbf{I}+\left(\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\dots \right){\mathbf{a}}^{\wedge }+\left(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\frac{{\theta }^6}{6!}-\dots \right){{\mathbf{a}}^{\wedge }}^2 \\ =\mathbf{I}+\sin (\theta ){\mathbf{a}}^{\wedge }+\left(1-\cos (\theta )\right){{\mathbf{a}}^{\wedge }}^2=\cos (\theta )\mathbf{I}+\sin (\theta ){\mathbf{a}}^{\wedge }+\left(1-\cos (\theta )\right){\mathbf{aa}}^T \end{gathered}$$
这和罗德里格斯公式如出一辙
$$\mathbf{R}=\cos (\theta )\mathbf{I}+\sin (\theta ){\mathbf{a}}^{\wedge }+\left(1-\cos (\theta )\right){\mathbf{aa}}^T$$
这表明，$so(3)$实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式
$$\begin{gathered} \mathrm{tr}(\mathbf{R})=3\cos (\theta )+3\sin (\theta )\mathrm{tr}({\mathbf{a}}^{\wedge })+(1-\cos (\theta ))\mathrm{tr}({\mathbf{aa}}^T) \\ =3\cos (\theta )+0+(1-\cos (\Vert \mathbf{\omega }\Vert ))\mathrm{tr}({\mathbf{a}}^T\mathbf{a}) \\ =3\cos (\theta )+(1-\cos (\theta ))=1+2\cos (\theta ) \\ ⟹\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2} \end{gathered}$$

大写指数映射

大写指数映射将旋转向量so(3)转换为旋转矩阵SO(3)：
$$\mathrm{Exp}:{\mathbb{R}}^3\to SO\left(3\right);\mathbf{v}\to \mathrm{Exp}\left(\mathbf{v}\right)=e^{{\mathbf{v}}^{\wedge }}$$

刚体变换与$\mathfrak{se}(3)$

除了旋转，刚体变换还有平移。所以，和只有旋转矩阵构成的李群$SO(3)$ 一样，我们也可以有由刚体变换得到的李群$SE(3)$
$$SE\left(3\right)=\left\{\mathbf{g}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{T}\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in SO\left(3\right),\mathbf{T}\in {\mathbb{R}}^3\right\}$$

$$\begin{gathered} \dot{\mathbf{g}}\left(t\right){\mathbf{g}}^{-1}\left(t\right)=\left[\begin{array}{cc}\dot{\mathbf{R}}\left(t\right)& \dot{\mathbf{T}}\left(t\right)\\ 0& 0\end{array}\right]\left[\begin{array}{cc}{\mathbf{R}}^T\left(t\right)& -{\mathbf{R}}^T\left(t\right)\mathbf{T}\left(t\right)\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)& \dot{\mathbf{T}}\left(t\right)-\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)\mathbf{T}(t),\\ 0& 0\end{array}\right] \\ \dot{\mathbf{g}}\left(t\right)=\left[\begin{array}{cc}\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)& \dot{\mathbf{T}}\left(t\right)-\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)\mathbf{T}\left(t\right)\\ 0& 0\end{array}\right]\mathbf{g}\left(t\right) \end{gathered}$$
对这个矩阵的每部分进行分析可知，存在一个反对称矩阵${\mathbf{\omega }}^{\wedge }(t),\mathbf{\omega }(t)\in so(3)$和三维向量$\mathbf{v}(t)\in {\mathbb{R}}^3$
$$\begin{gathered} {\mathbf{\omega }}^{\wedge }\left(t\right)=\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right) \\ \mathbf{v}\left(t\right)=\dot{\mathbf{T}}\left(t\right)-\dot{\mathbf{R}}\left(t\right){\mathbf{R}}^T\left(t\right)\mathbf{T}\left(t\right)=\dot{\mathbf{T}}\left(t\right)-{\mathbf{\omega }}^{\wedge }\left(t\right)\mathbf{T}\left(t\right) \end{gathered}$$
故
$$\dot{\mathbf{g}}\left(t\right)=\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }\left(t\right)& \mathbf{v}\left(t\right)\\ 0& 0\end{array}\right]\mathbf{g}\left(t\right)$$
可以和反对称矩阵一样，定一个矩阵 ${\mathbf{\xi }}^{\wedge }$，注意这里这个帽子不代表反对称矩阵了
$$\begin{gathered} {\mathbf{\xi }}^{\wedge }\left(t\right)=\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }\left(t\right)& \mathbf{v}\left(t\right)\\ 0& 0\end{array}\right] \\ {\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right]}^{\vee }=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]\in {\mathbb{R}}^6 \end{gathered}$$
该矩阵也可以表示为一个向量$\mathbf{\xi }(t)=\left[\begin{array}{c}\mathbf{v}(t)\\ \mathbf{\omega }(t)\end{array}\right]$，可以称之为在曲线$g(t)$处的”正切向量”,而在机器人领域，我们称它为twist。这个twist呢，就像我们开葡萄酒塞时螺旋的角速度和前进的线速度。于是把这些twist集合起来就有了刚体变换的李代数$\mathfrak{se}(3)$:
$$\mathfrak{se}\left(3\right)=\left\{\mathbf{\xi }=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]|\mathbf{\omega }\in SO\left(3\right),\mathbf{v}\in {\mathbb{R}}^3,{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
同样，李代数$\mathfrak{se}(3)$也有类似于$\mathfrak{so}(3)$的李括号：
$$[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2]=({\mathbf{\xi }}_1^{\wedge }{\mathbf{\xi }}_2^{\wedge }-{\mathbf{\xi }}_2^{\wedge }{\mathbf{\xi }}_1^{\wedge }{)}^{\vee }$$

和旋转矩阵一样的思路
$$\begin{gathered} \dot{\mathbf{g}}\left(t\right)={\mathbf{\xi }}^{\wedge }\left(t\right)\mathbf{g}\left(t\right) \\ \mathbf{g}\left(t+dt\right)\approx \mathbf{g}\left(t\right)+{\mathbf{\xi }}^{\wedge }\left(t\right)\mathbf{g}\left(t\right)dt\approx \left(\mathbf{I}+{\mathbf{\xi }}^{\wedge }\left(t\right)dt\right)\mathbf{g}\left(t\right) \\ \mathbf{g}\left(t\right)=e^{{\mathbf{\xi }}^{\wedge }t}\mathbf{g}\left(0\right) \end{gathered}$$

$$\begin{gathered} e^{{\mathbf{\xi }}^{\wedge }t}=\sum _{n=0}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right]}^n{t}^n=\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}\left[\begin{array}{cc}({\mathbf{\omega }}^{\wedge }t{)}^n& ({\mathbf{\omega }}^{\wedge }t{)}^{n-1}(\mathbf{v}t)\\ 0& 0\end{array}\right] \\ =\left[\begin{array}{cc}\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}(\hat{w}t{)}^n& \left(\sum _{n=1}^{\infty }\frac{1}{n!}({\mathbf{\omega }}^{\wedge }t{)}^{n-1}\right)(\mathbf{v}t)\\ 0& 1\end{array}\right]=\left(\begin{array}{cc}{e}^{{\mathbf{\omega }}^{\wedge }t}& \mathbf{J}(\mathbf{v}t)\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}\mathbf{R}& \mathbf{J}\mathbf{\rho }\\ 0& 1\end{array}\right) \end{gathered}$$
其中$\rho =vt$，令$\mathbf{\omega }t=\theta a,\Vert a\Vert =1$
$$\begin{gathered} J=\sum _{n=0}^{\infty }\frac{1}{\left(n+1\right)!}({\mathbf{\omega }}^{\wedge }t{)}^n=\sum _{n=0}^{\infty }\frac{1}{\left(n+1\right)!}(\theta \hat{a}{)}^n=\mathbf{I}+\frac{1}{2!}\theta \hat{a}+\frac{1}{3!}{\theta }^2{\hat{a}}^2+\dots = \\ \mathbf{I}+\frac{1}{\theta }\left(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\dots \right)\hat{a}+\frac{1}{\theta }\left(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\dots \right){\hat{a}}^2 \\ =\mathbf{I}+\frac{1-\cos \theta }{\theta }\hat{a}+\frac{\theta -\sin \theta }{\theta }{\hat{a}}^2 \\ =\mathbf{I}+\frac{1-\cos \theta }{\theta }\hat{a}+\left(1-\frac{\sin 0}{\theta }\right)\left(a{a}^T-\mathbf{I}\right) \\ =\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }\hat{a} \end{gathered}$$
使用齐次坐标$X=\left(\begin{array}{c}x\\ 1\end{array}\right)$
$$\begin{gathered} X\left(t\right)=g\left(t\right)X_0 \\ \dot{X}\left(t\right)=\dot{g}\left(t\right)X_0=\dot{g}\left(t\right)g^{-1}\left(t\right)X_0 \\ {V}^{\wedge }\left(t\right)=g\left(t\right)g^{-1}\left(t\right)=\left(\begin{array}{cc}{\mathbf{\omega }}^{\wedge }\left(t\right)& v\left(t\right)\\ 0& 0\end{array}\right) \\ \dot{X}\left(t\right)=V^{\wedge }\left(t\right)X\left(t\right) \end{gathered}$$
$V^{\wedge }(t)$代表点移动的速度，转换成普通坐标系：
$$x\left(t\right)={\mathbf{\omega }}^{\wedge }\left(t\right)x\left(t\right)+v\left(t\right)$$
考虑与X坐标系固连的坐标系Y
$$\begin{gathered} Y={\mathbf{g}}_{xy}x\left(t\right) \\ \dot{Y}\left(t\right)={\mathbf{g}}_{xy}\dot{X}\left(t\right)={\mathbf{g}}_{xy}{V}^{\wedge }\left(t\right)x\left(t\right)={\mathbf{g}}_{xy}{V}^{\wedge }{\mathbf{g}}_{xy}^{-1}Y\left(t\right) \\ {V^{\wedge }}_y={\mathbf{g}}_{xy}{V}^{\wedge }{\mathbf{g}}_{xy}^{-1}=a{d}_{{\mathbf{g}}_{xy}}\left(V^{\wedge }\right) \end{gathered}$$
这里的$ad$是$se(3)$上的伴随映射：
$$a{d}_g:se\left(3\right)\to se\left(3\right):{\mathbf{\xi }}^{\wedge }\to g{\mathbf{\xi }}^{\wedge }{g}^{-1}$$

SO(3)和SE(3)的伴随性质

$$\begin{array}{l}a=Rb,a,b\in {\mathbb{R}}^{3\times 3},R^{T}R=1,\det (R)=1\\ 则有a^{\wedge }=(Rb{)}^{\wedge }=R{b}^{\wedge }{R}^T\\ 证明:(Rb{)}^{\wedge }=R{b}^{\wedge }{R}^T\\ ⟺(Rb{)}^{\wedge }R=R{b}^{\wedge }{R}^{T}R=R{b}^{\wedge }\\ ⟺\forall a\in {\mathbb{R}}^3,(Rb{)}^{\wedge }Ra=R{b}^{\wedge }a\\ ⟺\forall a\in {\mathbb{R}}^3,(Rb)\times (Ra)=R(b\times a)\\ 最后一个公式利用向量叉乘的旋转不变性可证\end{array}$$
由上述定理可得
$$\mathbf{R}{e}^{\hat{p}}{\mathbf{R}}^T=e^{\mathbf{R}\hat{\mathbf{p}}{\mathbf{R}}^T}=e^{(\mathbf{Rp}{)}^{\wedge }}$$
该式被称为$SO(3)$上的伴随性质。同样的，在$SE(3)$上也有伴随性质：
$$\mathbf{g}{e}^{{\mathbf{\xi }}^{\wedge }}{\mathbf{g}}^{-1}=e^{(\mathbf{g}{\mathbf{\xi }}^{\wedge }{\mathbf{g}}^{-1})}=e^{{\left(Ad\left(\mathbf{g}\right)\mathbf{\xi }\right)}^{\wedge }}$$
$$\begin{gathered} \mathbf{g}{\mathbf{\xi }}^{\wedge }{\mathbf{g}}^{-1}=\left(\begin{array}{cc}\mathbf{R}& \mathbf{T}\\ 0& 1\end{array}\right)\left(\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right)\left(\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^{T}T\\ 0& 1\end{array}\right) \\ =\left(\begin{array}{cc}\mathbf{R}{\mathbf{\omega }}^{\wedge }& \mathbf{Rv}\\ 0& 0\end{array}\right)\left(\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{T}\\ 0& 1\end{array}\right) \\ =\left(\begin{array}{cc}\mathbf{R}{\mathbf{\omega }}^{\wedge }{\mathbf{R}}^T& \mathbf{Rv}-\mathbf{R}{\mathbf{\omega }}^{\wedge }{\mathbf{R}}^T\mathbf{T}\\ 0& 0\end{array}\right) \\ =\left[\begin{array}{cc}{\left(\mathbf{R}\mathbf{\omega }\right)}^{\wedge }& \mathbf{Rv}-{\left(\mathbf{R}\mathbf{\omega }\right)}^{\wedge }\mathbf{T}\\ 0& 0\end{array}\right] \\ =\left(\begin{array}{cc}{\left(\mathbf{R}\mathbf{\omega }\right)}^{\wedge }& \mathbf{Rv}+{\mathbf{T}}^{\wedge }\mathbf{R}\mathbf{\omega }\\ 0& 0\end{array}\right)={\left(\begin{array}{c}\mathbf{Rv}+{\mathbf{T}}^{\wedge }\mathbf{R}\mathbf{\omega }\\ \mathbf{R}\mathbf{\omega }\end{array}\right)}^{\wedge } \\ ={\left[\left(\begin{array}{cc}\mathbf{R}& {\mathbf{T}}^{\wedge }\mathbf{R}\\ 0& \mathbf{R}\end{array}\right)\left(\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right)\right]}^{\wedge }={\left[Ad\left(\mathbf{g}\right)\mathbf{\xi }\right]}^{\wedge } \end{gathered}$$

李代数求导与扰动模型

BCH公式与近似形式

$$\exp ({\mathbf{\varphi }}_1^{\wedge })\exp ({\mathbf{\varphi }}_2^{\wedge })=\exp (({\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2{)}^{\wedge })?$$
对于标量成立，但对于矩阵的指数函数不成立。

$$\ln \left(\exp \left(\mathbf{A}\right)\exp \left(\mathbf{B}\right)\right)=\mathbf{A}+\mathbf{B}+\frac{1}{2}\left[\mathbf{A},\mathbf{B}\right]+\frac{1}{12}\left[\mathbf{A},\left[\mathbf{A},\mathbf{B}\right]\right]-\frac{1}{12}\left[\mathbf{B},\left[\mathbf{A},\mathbf{B}\right]\right]+\cdots$$
其中$[]$为李括号。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑$\mathrm{SO}(3)$上的李代数$\ln {\left(\exp \left({\mathbf{\varphi }}_1^{\wedge }\right)\exp \left({\mathbf{\varphi }}_2^{\wedge }\right)\right)}^{\vee }$ ，当或为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达：
$$\begin{gathered} \ln {\left(\exp \left({\mathbf{\varphi }}_1^{\wedge }\right)\exp \left({\mathbf{\varphi }}_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l({\mathbf{\varphi }}_2{)}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2当{\mathbf{\varphi }}_1为小量\\ {\mathbf{J}}_r({\mathbf{\varphi }}_1{)}^{-1}{\mathbf{\varphi }}_2+{\mathbf{\varphi }}_1当{\mathbf{\varphi }}_2为小量\end{array} \\ {\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta }){\mathbf{aa}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge } \\ {\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2}){\mathbf{aa}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge } \end{gathered}$$
也可以写成：
$$\begin{gathered} \exp (({\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2{)}^{\wedge })=\exp \left(({\mathbf{J}}_l({\mathbf{\varphi }}_2)\cdot {\mathbf{\varphi }}_1{)}^{\wedge }\right)\exp ({\mathbf{\varphi }}_2^{\wedge }) \\ =\exp ({\mathbf{\varphi }}_1^{\wedge })\exp \left(({\mathbf{J}}_r({\mathbf{\varphi }}_1)\cdot {\mathbf{\varphi }}_2{)}^{\wedge }\right) \end{gathered}$$
而右乘模型雅可比仅需要对自变量取负号即可：
$${\mathbf{J}}_r(-\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$$
这样就可以讨论李群乘法和李代数加法的关系了。
假设对某个旋转$\mathbf{R}$，对应的李代数为$\mathbf{\varphi }$。我们给它一个微笑旋转，记作$\Delta \mathbf{R}$ ，对应的李代数为$\Delta \mathbf{\varphi }$。那么，在李群上得到的结果为$\Delta \mathbf{R}\cdot \mathbf{R}$，而在李代数上，根据BCH近似，为${\mathbf{J}}_l^{-1}(\mathbf{\varphi })\Delta \mathbf{\varphi }+\mathbf{\varphi }$
$$\exp (\Delta {\mathbf{\varphi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })=\exp \left({\left({\mathbf{J}}_l^{-1}(\mathbf{\varphi })\Delta \mathbf{\varphi }+\mathbf{\varphi }\right)}^{\wedge }\right)$$
反之，如果我们在李代数上进行加法，那么可以近似为李群上带左右雅可比的乘法：
$$\exp \left({\left(\mathbf{\varphi }+\Delta \mathbf{\varphi }\right)}^{\wedge }\right)=\exp \left({\left({\mathbf{J}}_l\Delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\left({\mathbf{J}}_r\Delta \mathbf{\varphi }\right)}^{\wedge }\right)$$

李代数求导

我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而$\mathrm{SO}(3)$和$\mathrm{SE}(3)$上并没有良好定义的加法，它们只是群。如果我们把$\mathbf{T}$当成一个普通矩阵来处理优化，就必须对它加以约束。而从李代数的角度来说，由于李代数由向量组成，具有良好的加法运算，因此使用李代数解决求导问题的思路分为两种：

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

$$\begin{gathered} \frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\varphi }}=\frac{\partial \left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}{\partial \mathbf{\varphi }}=\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left({\left(\mathbf{\varphi }+\delta \mathbf{\varphi }\right)}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }} \\ =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }} \\ =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\left[\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right]\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }} \\ =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }} \\ =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\left[\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right]}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}=-{\left(Rp\right)}^{\wedge }{\mathbf{J}}_l \end{gathered}$$

扰动模型(左乘)

SO(3)

另一种求导方式是对$\mathbf{R}$进行一次扰动$\Delta \mathbf{R}$，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在
$$\frac{\partial (\mathbf{Rp})}{\partial \mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp ({\mathbf{\phi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\mathbf{\phi }}$$
该式的求导更简单，省去了一个雅可比${\mathbf{J}}_l$计算

$$\begin{gathered} \frac{\partial \left(\mathbf{R}\mathbf{p}\right)}{\partial \mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }} \\ =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-{\left(\mathbf{R}\mathbf{p}\right)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-{\left(\mathbf{R}\mathbf{p}\right)}^{\wedge } \end{gathered}$$

SE(3)

假设某空间点$\mathbf{p}$经过一次变换$\mathbf{T}$(对应的李代数为$\mathbf{\xi }$，)得到$\mathbf{Tp}$。现在给$\mathbf{T}$左乘一个扰动$\Delta \mathbf{T}=\exp (\delta {\mathbf{\xi }}^{\wedge })$，设扰动项的李代数为$\delta \mathbf{\xi }=[\delta \mathbf{\varphi },\delta \mathbf{\rho }]$，那么：

$$\begin{gathered} \frac{\partial \left(\mathbf{T}\mathbf{p}\right)}{\partial \delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }} \\ \begin{array}{l}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }\left(\mathbf{R}\mathbf{p}+\mathbf{t}\right)+\delta \mathbf{\rho }\\ 0^T\end{array}\right]}{{\left[\delta \mathbf{\varphi },\delta \mathbf{\rho }\right]}^T}\\ =\left[\begin{array}{cc}-{\left(\mathbf{R}\mathbf{p}+\mathbf{t}\right)}^{\wedge }& \mathbf{I}\\ 0^T& 0^T\end{array}\right]\stackrel{\mathrm{def}}{=}(\mathbf{Tp}{)}^{\odot }\end{array} \end{gathered}$$

把最后的结果定义成一个算符${}^{\odot }$，它把一个齐次坐标的空间点变换成一个$4\times 6$的矩阵。