最近在学高翔大神的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)$。
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t R = 1 } (1) SO(3)=

\tag{1} SO(3)={R∈R3×3∣RRT=I,detR=1​}(1)

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } (2) SE(3)=

\tag{2} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3​}(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 ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a = { a 1 , a 2 , a 3 } T 　 　 (7) a^{\land}=A=

,\enspace A^{\lor}=a=^{T}　　\tag{7} a∧=A=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​,A∨=a={a1​,a2​,a3​​}T　　(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)$为正交阵，有
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) 　 　 　 (9) \dot{R}(t)=\phi(t)^{\land}R(t)=

R(t)　　　\tag{9} R˙(t)=ϕ(t)∧R(t)=⎣⎡​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​⎦⎤​R(t)　　　(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$可以生成一个反对称矩阵
Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 (14) \Phi=\phi^{\land}=

\in \Bbb{R}^{3\times 3} \tag{14} Φ=ϕ∧=⎣⎡​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​⎦⎤​∈R3×3(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)$是一个三维向量，或者一个三维反对称矩阵
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ R 3 × 3 } (16) \frak{so}(3)=

so(3)={ϕ∈R3,Φ=ϕ∧R3×3​}(16)
李代数$\mathfrak{s}\mathfrak{o}(3)$与李群$SO(3)$的关系，可以由指数映射给定，参考式(12)。

而李群$SE(3)$也有对应的李代数$\mathfrak{s}\mathfrak{e}(3)$，
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } (17) \frak{se}(3)=

\tag{17} se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]∈R4×4​}(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)，有
e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = ∑ k = 0 ∞ 1 k ! ( θ a ∧ ) k = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 ( a ∧ ) 4 + … = a a T − a ∧ a ∧ + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 ( a ∧ ) 2 + … = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − … ) a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − … ) a ∧ a ∧ = a ∧ a ∧ + I + sin ⁡ θ a ∧ − cos ⁡ θ a ∧ a ∧ = ( 1 − cos ⁡ θ ) a ∧ a ∧ + I + sin ⁡ θ a ∧ = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧

exp(ϕ∧)​=exp(θa∧)=k=0∑∞​k!1​(θa∧)k=I+θa∧+2!1​θ2a∧a∧+3!1​θ3a∧a∧a∧+4!1​θ4(a∧)4+…=aaT−a∧a∧+θa∧+2!1​θ2a∧a∧−3!1​θ3a∧−4!1​θ4(a∧)2+…=aaT+(θ−3!1​θ3+5!1​θ5−…)a∧−(1−2!1​θ2+4!1​θ4−…)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧​

最终，得到下面这个公式
$$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 }$$

不过，按照泰勒展开去计算对数映射，计算量太大了，也很麻烦，一般都是用下面的方式进行计算
{ θ = arccos ⁡ t r ( R ) − 1 2 R n = n (23)

\tag{23} {θ=arccos2tr(R)−1​Rn=n​(23)
式中，$tr(R)$为旋转矩阵$R$的迹，
$n$为旋转矩阵$R$特征值为１对应的特征向量，就是求解式(23)下面的公式，再归一化。

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

$\mathfrak{s}\mathfrak{e}(3)$上的指数映射，形式如下，
e x p ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] = [ R J ρ 0 T 1 ] = T (24) exp(\xi^{\land})=

= =\bold{T} \tag{24} exp(ξ∧)=⎣⎢⎡​n=0∑∞​n!1​(ϕ∧)n0T​n=0∑∞​(n+1)!1​(ϕ∧)nρ1​⎦⎥⎤​=[R0T​Jρ1​]=T(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 ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ( ϕ 1 ) + ϕ 2 当 ϕ 1 为 小 量 时 J r ( ϕ 1 ) − 1 ( ϕ 2 ) + ϕ 1 当 ϕ 2 为 小 量 时 (29) \ln (\exp(\phi^{\land}_{1})\exp(\phi^{\land}_{2}))^{\lor} \approx

\tag{29} ln(exp(ϕ1∧​)exp(ϕ2∧​))∨≈{Jl​(ϕ2​)−1(ϕ1​)+ϕ2​当ϕ1​为小量时Jr​(ϕ1​)−1(ϕ2​)+ϕ1​当ϕ2​为小量时​(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$矩阵，这里就不详细描述了。