最近在学高翔大神的SLAM，工作之余才能有空拜读，本文内的东西大都是摘录自大神的《视觉SLAM十四讲》，本文仅仅是作为学习笔记，增加记忆。

群、李群

首先上一下群的定义，群(Group)是一种集合加上一种运算的代数结构，将其中的集合称作$A$，运算为$\cdot$，所以群就可以记做$G=(A,\cdot )$。群的相关运算需要满足以下性质：

• 封闭性：$\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_2)$
• 幺元：$\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$

从上面几个群的相关性质，不难发现，旋转矩阵和矩阵的乘法可以构成群，同样的，变换矩阵和矩阵乘法也可以构成群。另外，常见的群有，整数和加法构成的群，去掉0后的有理数和乘法（0不存在幺元）构成的群等。

上面的八股文还是要理解一下的，下面结合之前看过的四元素以及坐标变换，理解一下群是什么东西。

接下来说点非八股文的东西，坐标转换时，会从一个坐标系$O_A$转换到另一个坐标系$O_B$，需要经常用到连续的旋转矩阵相乘，或者变换矩阵的连续相乘，经过变换后，旋转矩阵仍然是旋转矩阵，变换矩阵也仍然是变换矩阵。旋转矩阵相乘和转移矩阵相乘完全符合群的定义以及各个条件，故，三维旋转矩阵构成了特殊正交群$SO(3)$，变换矩阵构成了特殊欧式群$SE(3)$。
$$SO(3)=\left\{\begin{array}{c}R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,detR=1\end{array}\right\}\text{\hspace{1em}(1)}$$

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

旋转矩阵$SO(3)$和转移矩阵$SE(3)$对于加法是不封闭的，只关于乘法封闭，
$$R_1{R}_2\in SO(3),T_1{T}_2\in SE(3)\text{\hspace{1em}(3)}$$

好，对于旋转矩阵和转移矩阵这种相乘仍然是旋转矩阵和转移矩阵，这种只有一种运算的集合，就是群。

矩阵中常见的群有：

• 一般线性群GL(n) 指$n\times n$的可逆矩阵，对矩阵乘法成群
• 特殊正交群SO(n) 旋转矩阵群，例如$SO(2),SO(3)$
• 特殊欧式群SE(n) 转移矩阵群，例如$SE(2),SE(3)$

李群则是指具有连续性质的群，刚体在空间中的运动都是连续的，所以$SO(2),SO(3),SE(2),SE(3)$都是李群。每个李群都会有一个对应的李代数，下面来说一下李代数。

李代数

对于任意传感器的旋转矩阵$R$，会随着时间t连续的变化，有
$$R(t)R^T(t)=I\text{\hspace{1em}(4)}$$
再对等式两边求导，则有
$$\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0\text{\hspace{1em}(5)}$$
整理后，有
$$\dot{R}(t)R(t{)}^T=-(\dot{R}(t)R(t{)}^T{)}^T\text{\hspace{1em}(6)}$$

可以看得出，$\dot{R}(t)R(t{)}^T$是一个反对称矩阵。

反对成矩阵可以转换成一个向量，任一反对称矩阵都可以找出一个与其对应的向量，由这个向量转到反对称矩阵用符号$\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={\left\{\begin{array}{c}{a}_1,a_2,a_3\end{array}\right\}}^T\text{\hspace{1em}(7)}$$

因此，反对称矩阵$\dot{R}(t)R(t{)}^T$也可以化为一个对应的三维向量$\varphi (t)\in R$，如下
$$\dot{R}(t)R(t{)}^T=\varphi (t{)}^{\wedge }\text{\hspace{1em}(8)}$$

等式两边同时右乘$R(t)$，由于$R(t)$为正交阵，有
$$\dot{R}(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)\text{\hspace{1em}(9)}$$

由上式，可以看出旋转矩阵$R(t)$求一次导数，就是只需要左乘一个$\varphi (t{)}^{\wedge }$即可。

取$t_0=0$，并且设此时旋转矩阵为$R(0)=I$，按照导数定义，将$R(t)$在0附近进行一阶泰勒展开，则有，
$$R(t)\approx R(t_0)+\dot{R}(t_0)(t-t_0)=I+\varphi (t_0{)}^{\wedge }t\text{\hspace{1em}(10)}$$

从上式可以看出，$\varphi$反映了$R$的导数性质，因此$\varphi$被称为$SO(3)$原点附近的正切空间上。

同时在$t_0$附近，设$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$，那么根据式(9)，有
$$\dot{R}(t)=\varphi (t_0{)}^{\wedge }R(t)={\varphi }_0^{\wedge }R(t)\text{\hspace{1em}(11)}$$
求解上述微分方程，并且因为初始值$R(0)=I$，求解后，有
$$R(t)=exp({\varphi }_0^{\wedge }t)\text{\hspace{1em}(12)}$$

到此可以看出，旋转矩阵$R$与另一个反对称矩阵${\varphi }_0^{\wedge }$的指数运算有了联系。

书中上述的这一番推导十分精彩，有空可以细细琢磨一下。

下面，自己做一些补充，其实$\varphi$就是旋转矩阵$R$对应的李群$SO(3)$上的李代数$\mathfrak{s}\mathfrak{o}(3)$。

另外矩阵的指数计算，如下，
$$exp(At)=I+At+\frac{1}{2!}{A}^2{t}^2+\dots =\sum _{k=0}^{\infty }\frac{1}{k!}{A}^k{t}^k\text{\hspace{1em}(13)}$$

下面就介绍下李代数的概念，
每个李群都有一个对应的李代数，李代数的定义：李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算$[,]$组成，另外，还必须满足以下几条性质：

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$

则称$[\mathbb{V},\mathbb{F},[,]]$为一个李代数，记为$\mathfrak{g}$。举个例子，三维向量${\mathbb{R}}^3$上定义的叉积$\times$是一种李括号，因此，$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数。

结合之前的推导，每个$\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}\text{\hspace{1em}(14)}$$

在此定义下，两个向量${\varphi }_1,{\varphi }_2$ 的李括号为
$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }\text{\hspace{1em}(15)}$$

$\mathfrak{s}\mathfrak{o}(3)$是一个三维向量，或者一个三维反对称矩阵
$$\mathfrak{s}\mathfrak{o}(3)=\left\{\begin{array}{c}\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }{\mathbb{R}}^{3\times 3}\end{array}\right\}\text{\hspace{1em}(16)}$$
李代数$\mathfrak{s}\mathfrak{o}(3)$与李群$SO(3)$的关系，可以由指数映射给定，参考式(12)。

而李群$SE(3)$也有对应的李代数$\mathfrak{s}\mathfrak{e}(3)$，
$$\mathfrak{s}\mathfrak{e}(3)=\left\{\begin{array}{c}\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),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\end{array}\right\}\text{\hspace{1em}(17)}$$
每个李代数$\mathfrak{s}\mathfrak{e}(3)$元素记做$\xi$，是一个六维向量，上面的三维为平移向量，记做$\rho$，注意与变换矩阵$T$中的平移向量$t$不同，不是同一个东西；下面三维为李群$\mathfrak{s}\mathfrak{o}(3)$。注意，这里的$\wedge$与之前的不同，在这同样使用$\wedge$，将一个六维向量转换为一个四维矩阵，但是在此处不再表示反对称。

同样，李代数$\mathfrak{s}\mathfrak{e}(3)$也有相应的李括号
$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }\text{\hspace{1em}(18)}$$

指数和对数映射

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

上面说过矩阵指数的求解，在李群和李代数中，称为指数映射。

矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下会有结果，类似式(13)
$$exp(A)=I+A+\frac{1}{2!}{A}^2+\dots =\sum _{k=0}^{\infty }\frac{1}{k!}{A}^k\text{\hspace{1em}(19)}$$

对$\mathfrak{s}\mathfrak{o}(3)$中的任意元素$\varphi$，它的指数映射同样为
$$exp({\varphi }^{\wedge })=\sum _{k=0}^{\infty }\frac{1}{k!}{A}^k\text{\hspace{1em}(20)}$$

由于$\varphi$是一个三维向量，定义它的模长为$\theta$，方向为向量$\mathbf{a}$，于是有$\varphi =\theta \mathbf{a}$，这里的方向向量$\mathbf{a}$为长度为１的单位向量。然后，对于${\mathbf{a}}^{\wedge }$，有以下两条性质，
$$\begin{gathered} {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-I \\ {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge }\text{\hspace{1em}(21)} \end{gathered}$$
再结合式(20)，有
$$\begin{array}{rl}exp({\varphi }^{\wedge })& =exp(\theta {\mathbf{a}}^{\wedge })=\sum _{k=0}^{\infty }\frac{1}{k!}(\theta {\mathbf{a}}^{\wedge }{)}^k\\ & =I+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4+\dots \\ & =\mathbf{a}{\mathbf{a}}^T-{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }-\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }-\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^2+\dots \\ & =\mathbf{a}{\mathbf{a}}^T+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\dots ){\mathbf{a}}^{\wedge }-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\dots ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+I+\sin \theta {\mathbf{a}}^{\wedge }-\cos \theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =(1-\cos \theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+I+\sin \theta {\mathbf{a}}^{\wedge }\\ & =\cos \theta I+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }\end{array}$$

最终，得到下面这个公式
$$exp({\varphi }^{\wedge })=exp(\theta {\mathbf{a}}^{\wedge })=\cos \theta I+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }\text{\hspace{1em}(22)}$$

再看下罗德里格斯公式
$$R=\cos \theta I+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }$$
两者一致，由此可知，李代数$\mathfrak{s}\mathfrak{o}(3)$就是由旋转向量组成的空间，而指数映射即罗德里格斯公式。通过此公式，可以把$\mathfrak{s}\mathfrak{o}(3)$中的任一向量对应到$SO(3)$中的旋转矩阵。反之，也可以通过对数映射将$SO(3)$中的旋转矩阵对应到相应的$\mathfrak{s}\mathfrak{o}(3)$中的一个向量
$$\varphi =\ln (R^{\vee })=[\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{]}^{\vee }$$

不过，按照泰勒展开去计算对数映射，计算量太大了，也很麻烦，一般都是用下面的方式进行计算
$$\{\begin{array}{l}\theta =\arccos \frac{tr(R)-1}{2}\\ Rn=n\end{array}\text{\hspace{1em}(23)}$$
式中，$tr(R)$为旋转矩阵$R$的迹，
$n$为旋转矩阵$R$特征值为１对应的特征向量，就是求解式(23)下面的公式，再归一化。

$SE(3)$上的指数映射

$\mathfrak{s}\mathfrak{e}(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]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\rho \\ 0^T& 1\end{array}\right]=\mathbf{T}\text{\hspace{1em}(24)}$$

从上式中可以看出，$\xi ＝[\rho ,\varphi {]}^T$的指数映射左上角的$R$就是$SO(3)$中的元素，与$\mathfrak{s}\mathfrak{e}(3)$当中旋转部分$\varphi$对应；
右上角的$J$可以整理为
$$J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(25)}$$
其中，$\varphi =\theta \mathbf{a}$；
$\rho$是平移部分，这里再次提醒下，与变换矩阵$T$中的平移向量$t$不同，$\rho$与$t$之间的关系为
$$t=J\rho \text{\hspace{1em}(26)}$$

好了，到此为止，可知

1. 李群$SO(3)$通过对数映射得到李代数$\mathfrak{s}\mathfrak{o}(3)$，李代数$\mathfrak{s}\mathfrak{o}(3)$通过指数映射得到李群$SO(3)$；
2. 李群$SE(3)$通过对数映射得到李代数$\mathfrak{s}\mathfrak{e}(3)$，李代数$\mathfrak{s}\mathfrak{e}(3)$通过指数映射得到李群$SE(3)$。

这里是我的理解，李群和李代数之间的互相转换，第一步就需要找到旋转轴和旋转角度，可以通过旋转矩阵得到，有了这两个量之后，在进行其他相关量的计算就可以了。

BCH公式与近似公式

引入李群李代数的主要目的就是为了进行优化，在进行优化过程中，不可避免的就要进行求导。

上文说了李群$SO(3)$的指数映射，和$SE(3)$的对数映射，如果在$\mathfrak{s}\mathfrak{o}(3)$上做加法运算时，正好对应$SO(3)$上的两个矩阵的乘积，$$\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })=\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })\text{\hspace{1em}(27)}$$
不难发现，如果是标量，式(27)一定成立，但是现在计算的是矩阵，上式不应适用于矩阵的情形。

两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hausdorff公式给出，简称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]]+\dots \text{\hspace{1em}(28)}$$

其中，$[]$为李括号。不难看出，当处理两个矩阵指数之积时，会产生一些由李括号组成的余项。

考虑$SO(3)$上的李代数$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }$，当${\varphi }_1$或${\varphi }_2$为小量时，小量二次以上的项可以忽略掉，BCH线性近似可以表达为
$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}({\varphi }_1)+{\varphi }_2当{\varphi }_1为小量时\\ J_r({\varphi }_1{)}^{-1}({\varphi }_2)+{\varphi }_1当{\varphi }_2为小量时\end{array}\text{\hspace{1em}(29)}$$

从上式(29)可以看出，当对一个旋转矩阵$R_2$(李代数为${\varphi }_2$)左乘一个微小的旋转矩阵$R_1$(李代数为${\varphi }_1$)时，可以近似等于，在原有李代数${\varphi }_2$的基础上加了一项$J_l({\varphi }_2{)}^{-1}({\varphi }_1)$；同理，当对一个旋转矩阵$R_1$(李代数为${\varphi }_1$)右乘一个微小的旋转矩阵$R_2$(李代数为${\varphi }_2$)时，可以近似等于，在原有李代数${\varphi }_1$的基础上加了一项$J_l({\varphi }_1{)}^{-1}({\varphi }_2)$。实际使用时，需要注意使用的是左乘模型还是右乘模型。

上式中，左乘BCH近似雅克比$J_l$为
$$J_l=J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(30)}$$
其逆为
$$J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+(1-\frac{\theta }{2}\cot \frac{\theta }{2})-\frac{\theta }{2}{\mathbf{a}}^{\wedge }\text{\hspace{1em}(31)}$$

而右乘雅克比仅需要对自变量取负号即可，
$$J_r(\varphi )=J_l(-\varphi )\text{\hspace{1em}(32)}$$

为了便于理解，这里总结一下，如果现有一个旋转矩阵$R$(对应李代数为$\varphi$)，然后再加上一个微小旋转$\Delta R$(对应的李代数为$\Delta \varphi$)。在李群上得到的$\Delta R\cdot R$，在李代数上，根据BCH近似有$J_l^{-1}(\varphi )\Delta \varphi +\varphi$，写成公式则为，
$$\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp ((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })\text{\hspace{1em}(33)}$$

反之，在李代数上进行加法运算，让$\varphi$加上$\Delta \varphi$，则可以近似为李群上带左右雅克比的乘法
$$\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ((J_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })\text{\hspace{1em}(34)}$$

这是后面李代数做微积分的理论基础。

至于$SE(3)$，也有类似的BCH近似公式：
$$\begin{gathered} \exp (\Delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\approx \exp (({\mathcal{J}}_l^{\wedge }\Delta \xi +\xi {)}^{\wedge }) \\ \exp ({\xi }^{\wedge })\exp (\Delta {\xi }^{\wedge })\approx \exp (({\mathcal{J}}_r^{\wedge }\Delta \xi +\xi {)}^{\wedge })\text{\hspace{1em}(35)} \end{gathered}$$

这里的${\mathcal{J}}_l$和${\mathcal{J}}_r$为一个$6\times 6$矩阵，这里就不详细描述了。