[视觉SLAM十四讲]学习笔记5-李群与李代数基础及指数/对数映射

在之前的学习中，我们知道了三维世界刚体运动的一些描述方式，例如：旋转矩阵、欧拉角、四元数等表示方式，但是SLAM中除了表示这些运动，还要进行估计和优化。因为在SLAM中位姿是未知的，我们需要解决例如 “相机处于一个什么样的位姿才能观测出最真实的数据” 这样的问题，这是一个典型的最优化问题，求解最优的旋转矩阵 $R$和平移向量 $t$，使误差变得最小。
我们知道，旋转矩阵自身是带有正交且横列式需为1的约束，若把旋转矩阵当作优化变量，这会增加额外的约束条件，使优化变得困难。而通过李群—李代数间的转换关系，我们希望可以把位姿估计变成无约束优化问题，这将简化求解问题。下面我们进行李群—李代数相关知识的介绍和学习。

1 群和李群

1.1 群的引出

在之前的学习中，我们知道了旋转矩阵和变换矩阵的定义。同时，我们说三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3)，让我们再看一下他们的形式： S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } . (1.1) \rm{SO(3)}=\{\bm{R}\in \mathbb{R}^{3\times 3}|\bm{RR}^{T}=I,\rm{det}(\bm{R})=1\}.\tag{1.1} SO(3)={R∈R3×3∣RRT=I,det(R)=1}.(1.1)$$\mathrm{SE}(3)=\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\}.\text{\hspace{1em}(1.2)}$$我们通过简单的例子计算可以发现，旋转矩阵和变换矩阵对加法运算是不封闭的。即对于两个任意的旋转矩阵${\mathbf{R}}_1$和${\mathbf{R}}_2$，按照矩阵加法运算法则得到的新的矩阵，不再满足旋转矩阵的定义：$${\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{SO}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{SE}(3)\text{\hspace{1em}(1.3)}$$换一种说法，我们也可以说没有找到一种合适的加法运算的定义，或者常规的矩阵加法对这两个集合不封闭。不过，他们的乘法运算是较好的，并且只有这一种。$\mathrm{SO}(3)$和$\mathrm{SE}(3)$关于乘法是封闭的：$${\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{SO}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{SE}(3).\text{\hspace{1em}(1.4)}$$同时，我们也可以对任何一个旋转矩阵或变换矩阵在乘法的意义上求逆。而乘法所对应的是旋转或者变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个(合适的/保证集合封闭)运算的集合，我们称之为群。

1.2 群的性质及李群定义

群(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_3).$
• 幺元 $\exists a_0\in A,s.t.\forall \in \mathrm{A},{\mathrm{a}}_0\cdot \mathrm{a}=\mathrm{a}\cdot {\mathrm{a}}_0=a.$
• 逆 $\forall a\in A,\exists a^{-1}\in A,s.t.\mathrm{a}\cdot {\mathrm{a}}^{-1}={\mathrm{a}}_0.$

通过以上的条件，我们可以验证旋转/变换矩阵和矩阵乘法都构成群。矩阵中常见的群有：

• 一般线性群$GL(n)$ \quad 指$n\times n$的可你矩阵，它们对矩阵乘法成群。
• 特殊正交群$SO(n)$ \quad 即所谓的旋转矩阵群，最为常见的是$\mathrm{SO}(2)$和$\mathrm{SO}(3)$。
• 特殊欧式群$SE(n)$ \quad 即$n$维欧式变换，如$\mathrm{SE}(2)$和$\mathrm{SE}(3)$。

下面介绍一下李群。
李群是指具有连续(光滑)性质的群。整数群$\mathbb{Z}$是离散的群没有连续性质，所以不是李群。而$\mathrm{SO}(\mathrm{n})$和$\mathrm{SE}(\mathrm{n})$由于在实数空间上是连续的，所以我们能够直观地想象一个刚体能够连续地在空间中运动，因此它们都是李群。$\mathrm{SO}(\mathrm{n})$和$\mathrm{SE}(\mathrm{n})$对相机姿态估计尤其重要，我们主要讨论这两个李群。
在视觉SLAM十四讲中，作者从旋转矩阵引出李代数，从简单的$\mathrm{SO}(3)$进行讨论，引出$\mathrm{SO}(3)$上面的李代数$\mathfrak{so}(3)$。

2 李代数

2.1 李代数的引出

对于任意的旋转矩阵$\mathbf{R}$，满足：$$\mathbf{R}{\mathbf{R}}^T=\mathbf{I}\text{\hspace{1em}(2.1)}$$当$\mathbf{R}$是某个相机的旋转时，它将是一个随时间连续变化的时间函数：$\mathbf{R}(t)$，这时仍然满足：$$\mathbf{R}(t){\mathbf{R}}^T(t)=\mathbf{I}\text{\hspace{1em}(2.2)}$$同时对等式两边对时间求导，得到$$\dot{\mathbf{R}}(t){\mathbf{R}}^T(t)+\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^T=0⟺\dot{\mathbf{R}}(t){\mathbf{R}}^T(t)=-(\dot{\mathbf{R}}(t){\mathbf{R}}^T(t){)}^T\text{\hspace{1em}(2.3)}$$显然，$\dot{\mathbf{R}}(t){\mathbf{R}}^T(t)$是一个反对称矩阵。在之前旋转变换的知识中，引入的${}^{\wedge }$符号，可以将向量变成反对称矩阵。同理，我们也可以找到一个唯一与反对称矩阵对应的矩阵，我们可以用${}^{\vee }$这个符号表示：$$a^{\wedge }=\mathbf{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{A}}^{\vee }=a\text{\hspace{1em}(2.4)}$$对于反对称矩阵$\dot{\mathbf{R}}(t){\mathbf{R}}^T(t)$，我们同样可以找到一个三维向量$\mathbf{\varphi }(t)\in {\mathbb{R}}^3$与之对应：$$\dot{\mathbf{R}}(t){\mathbf{R}}^T(t)=\mathbf{\varphi }(t{)}^{\wedge }.\text{\hspace{1em}(2.5)}$$等式两边右乘$\mathbf{R}(t)$，有：$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t{)}^{\wedge }\mathbf{R}(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\mathbf{R}(t)\text{\hspace{1em}(2.6)}$$显然，对旋转矩阵每次求导，只需要左乘一个$\mathbf{\varphi }(t{)}^{\wedge }$即可。现在我们考虑$t_0=0$的情况，设此时的旋转矩阵$\mathbf{R}(0)=\mathbf{I}$。根据导数的定义，我们将$\mathbf{R}(t)$在$t=0$附近一阶泰勒展开：$$\mathbf{R}(t)\approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\mathbf{\varphi }(t_0{)}^{\wedge }(t).\text{\hspace{1em}(2.7)}$$这里，$\mathbf{\varphi }$反映了$\mathbf{R}$的导数性质，我们称它在$\mathrm{SO}(3)$原点附近的正切空间(Tangent Space)上。在$t_0$附近，设$\mathbf{\varphi }$保持为常数$\mathbf{\varphi }(t_0)={\mathbf{\varphi }}_0$。根据式(2.6)，有：$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t_0{)}^{\wedge }\mathbf{R}(t)={\varphi }_0\mathbf{R}(t).\text{\hspace{1em}(2.8)}$$上式为一个关于$\mathbf{R}$的微分方程，初值为$\mathbf{R}(0)=\mathbf{I}$，解得：$$\mathbf{R}(t)=\exp ({\mathbf{\varphi }}_0^{\wedge }\mathrm{t}).\text{\hspace{1em}(2.9)}$$这个结果说明在$t=0$附近，旋转矩阵可以由$\exp ({\mathbf{\varphi }}_0^{\wedge }\mathrm{t})$计算出来。同时，旋转矩阵$\mathbf{R}$与另一个反对称矩阵${\mathbf{\varphi }}_0^{\wedge }t$之间通过指数关系联系，这里有两个问题需要我们来解释：

• 给定某时刻的$\mathbf{R}$，有一个对应的$\mathbf{\varphi }$，它描述了$\mathbf{R}$在某时刻局部的导数关系，而$\mathbf{\varphi }$正是对应到$\mathrm{SO}(3)$上的李代数$\mathfrak{so}(3)$。
• 给定某个向量$\mathbf{\varphi }$时，矩阵指数$\exp ({\mathbf{\varphi }}^{\wedge })$如何计算得到？反之，给定$\mathbf{R}$时，能否有相反的运算来计算$\mathbf{\varphi }$?这正是李群与李代数之间的指数/对数映射关系。
  下面我们通过李代数的定义及$\mathfrak{so}(3)$和$\mathfrak{se}(3)$两个李代数的介绍来引出李群与李代数之间的指数/对数映射关系。

2.2 李代数的定义

在《旋量代数与李群、李代数》这本书中，给出了李代数的概念如下：
李代数是研究李群、微分流形和微小变换等几何体的代数工具. 瞬时旋量是李群$\mathrm{SE}(3)$的射影李代数的元素。在20 世纪40 年代德国数学家与理论物理学家$HermannWeyl$(1944) 引进术语"李代数" 之前，李代数一直被称作"无穷小群"，即群元素非常接近恒等， 为李群流形在单位元处的切空间。
每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位圆附近的正切空间。一般的李代数定义如下：
李代数由一个集合$\mathbb{V}$、一个数域$\mathbb{F}$和一个二元运算[,]组成。如果他们满足以下几条性质，则称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$\mathfrak{g}$。它具有以下性质：

• 封闭性 $\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}.$
• 双线性 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F},$有$$[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}].$$
• 自反性 $\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0$.
• 雅可比等价 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0.$

这里的二元运算被称为李括号。和群中简单的二元运算相比，李括号表达了两个元素的差异，它不遵循结合律，而是要求元素和自己做李括号最后为零的性质。一个很简单的例子是三维向量${\mathbb{R}}^3$上定义的叉乘(外积、叉积)是一种李括号，所以$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数。

2.3 李代数$\mathfrak{so}(3)$和$\mathfrak{se}(3)$

2.3.1 李代数$\mathfrak{so}(3)$

我们在上一节引入的$\mathbf{\varphi }$实际上是一种李代数，$\mathrm{SO}(3)$对应的李代数是定义在${\mathbb{R}}^3$上的向量，我们可以记作$\mathbf{\varphi }$。根据上一节的推导，每个$\mathbf{\varphi }$都可以生成一个反对称矩阵：$$\Phi ={\mathbf{\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}(2.10)}$$在此定义下的两个向量${\mathbf{\varphi }}_1$，${\mathbf{\varphi }}_2$的李括号为$$[{\mathbf{\varphi }}_1,{\mathbf{\varphi }}_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\wedge }.\text{\hspace{1em}(2.11)}$$由于向量$\mathbf{\varphi }$和反对称矩阵一一对应的事实，我们可以在不引起歧义的情况下，说$\mathfrak{so}(3)$的元素是三维向量或三维反对称矩阵，不加区别： s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } . (2.12) \mathfrak{so}(3)=\{\phi \in \mathbb{R}^3,\Phi=\bm{\phi}^{\land}\in\mathbb{R}^{3 \times 3} \}.\tag{2.12} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}.(2.12)现在，我们清楚了$\mathfrak{so}(3)$的内容：它们是一个由三维向量组成的集合，每个向量都对应一个反对称矩阵，可以用来表达旋转矩阵的导数。它和$\mathrm{SO}(3)$之间的关系通过指数映射：$$\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge })\text{\hspace{1em}(2.13)}$$指数映射将在第三节介绍。

2.3.2 李代数$\mathfrak{se}(3)$

对于$\mathrm{SE}(3)$，它对应的李代数为$\mathfrak{se}(3)$。与$\mathfrak{so}(3)$相似，$\mathfrak{se}(3)$位于${\mathbb{R}}^6$空间中：$$\mathfrak{se}(3)=\{\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\mathbf{\varphi }\in \mathfrak{so}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}.\text{\hspace{1em}(2.14)}$$我们可以用$\mathbf{\xi }$来表示$\mathfrak{se}(3)$的每个元素，它是一个六维向量。它的前三维是平移，但不同于之前变换矩阵中的平移，记作$\mathbf{\rho }$；后三维是旋转，记作$\mathbf{\varphi }$,实质上是$\mathfrak{so}(3)$元素。在$\mathfrak{so}(3)$中，我们同样使用${}^{\wedge }$符号，表示将一个六维向量转换成四维矩阵，不在表示反对称：$${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}.\text{\hspace{1em}(2.15)}$$我们这里仍然使用${}^{land}$和${}^{\vee }$符号来表示"从向量到矩阵"和"从矩阵到向量"的关系，来保持和$\mathfrak{so}(3)$的一致性。并且仍然是一一对应的关系。我们可以简单的把$\mathfrak{se}(3)$理解为"由一个平移加上一个$\mathfrak{so}(3)$元素构成的向量"(注意：这里的$\mathbf{\rho }$不是直接的平移)。类似于$\mathfrak{so}(3)$的李括号，李代数$\mathfrak{se}(3)$的李括号：$$[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2]=({\mathbf{\xi }}_1^{\wedge }{\mathbf{\xi }}_2^{\wedge }-{\mathbf{\xi }}_2^{\wedge }{\mathbf{\xi }}_1^{\wedge }{)}^{\wedge }.\text{\hspace{1em}(2.16)}$$

3 指数与对数映射

现在我们来解决第二个问题，即如何计算$\exp ({\mathbf{\varphi }}^{\wedge })$?我们可以看出，这是一个矩阵的指数，在李群和李代数中称为指数映射(Exponential Map)。我们首先讨论$\mathfrak{so}(3)$上的指数映射。

3.1 $\mathrm{SO}(3)$上的指数映射

我们知道，任意一个矩阵的指数映射可以由泰勒展开表示，但是只有当收敛的情况下才会有结果，其结果仍然是一个矩阵：$$\exp (\mathbf{A})=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}{\mathbf{A}}^{\mathrm{n}}.\text{\hspace{1em}(3.1)}$$对于$\mathfrak{so}(3)$中的任意元素$\mathbf{\varphi }$，可以按照同样的定义表示它的指数映射：$$\exp ({\mathbf{\varphi }}^{\wedge })=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}({\mathbf{\varphi }}^{\wedge }{)}^{\mathrm{n}}.\text{\hspace{1em}(3.2)}$$这里的矩阵无穷次幂计算起来非常麻烦，这里采用一种计算指数映射的简便方法。我们通过定义旋转向量的方式，定义$\mathbf{\varphi }$(三维向量)，定义他的模长为$\theta$，方向为$\mathbf{a}$(长度为1的单位向量，$\Vert \mathbf{a}\Vert =1,a_1^2+a_2^2+a_3^2=1$)，所以有$\mathbf{\varphi }=\theta \mathbf{a}$。对于${\mathbf{a}}^{\wedge }$，我们有以下两条性质：$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\end{array}\right]=\mathbf{a}{\mathbf{a}}^T-\mathbf{I}.\text{\hspace{1em}(3.3)}$$$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\wedge }(\mathbf{a}{\mathbf{a}}^T-\mathbf{I})=-{\mathbf{a}}^{\wedge }.\text{\hspace{1em}(3.4)}$$便于理解，下面我们来简单推导一下这个性质$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\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]=\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\end{array}\right]$$$$=\left[\begin{array}{ccc}{a}_1^2-1& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& a_2^2-1& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& a_3^2-1\end{array}\right]=\left[\begin{array}{ccc}{a}_1^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& a_2^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& a_3^2\end{array}\right]-\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$$$=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\left[\begin{array}{ccc}{a}_1& a_2& a_3\end{array}\right]-\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=\mathbf{a}{\mathbf{a}}^T-\mathbf{I}\text{\hspace{1em}(3.4)}$$$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\wedge }(\mathbf{a}{\mathbf{a}}^T-\mathbf{I})=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\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]\left[\begin{array}{ccc}{a}_1^2-1& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& a_2^2-1& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& a_3^2-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{a}}^{\wedge }\text{\hspace{1em}(3.5)}$$显然，这两个式子可以为我们处理${\mathbf{a}}^{\wedge }$的高阶项问题。我们可以把指数映射写成：$$\exp ({\mathbf{\varphi }}^{\wedge })=\exp (\theta {\mathbf{a}}^{\wedge })=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}(\theta {\mathbf{a}}^{\wedge }{)}^{\mathrm{n}}$$ $$=\mathbf{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+\cdots$$ $$={\mathbf{aa}}^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+\cdots$$ $$={\mathbf{aa}}^T+\underset{sin\theta 的泰勒展开式}{\underbrace{(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\cdots )}}{\mathbf{a}}^{\wedge }-\underset{cos\theta 的泰勒展开式}{\underbrace{(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\cdots )}}{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }$$ $$={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+sin\theta {\mathbf{a}}^{\wedge }-cos\theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=(1-cos\theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+sin\theta {\mathbf{a}}^{\wedge }$$ $$=cos\theta \mathbf{I}+(1-cos\theta ){\mathbf{aa}}^T+sin\theta {\mathbf{a}}^{\wedge }.\text{\hspace{1em}(3.6)}$$ 这里$\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4=\frac{1}{4!}{\theta }^4{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=({\mathbf{aa}}^T-\mathbf{I}){\mathbf{a}}^{\wedge }=-({\mathbf{a}}^{\wedge }{)}^2$，其他高阶项同理进行降阶变形。
我们可以看到，式(3.6)我们很熟悉：$$\exp (\theta {\mathbf{a}}^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta ){\mathbf{aa}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }.\text{\hspace{1em}(3.7)}$$它和我们之前推导的罗德里格斯公式，如出一辙。这个结果表明，$\mathfrak{so}(3)$实际上就是由之前学习的旋转向量组成的空间，指数映射即罗德里格斯公式。通过它们，我们可以把$\mathfrak{so}(3)$中任意一个向量对应到一个位于$\mathrm{SO}(3)$中的旋转向量。若定义的是对数映射，也可以把$\mathrm{SO}(3)$中的元素对应到$\mathfrak{so}(3)$中：$$\mathbf{\varphi }=\ln (\mathbf{R}{)}^{\vee }=\{\sum _{\mathrm{n}=0}^{\infty }\frac{(-1{)}^{\mathrm{n}}}{\mathrm{n}+1}(\mathbf{R}-\mathbf{I}{)}^{\mathrm{n}+1}{\}}^{\vee }.\text{\hspace{1em}(3.8)}$$类似的方法，我们可以利用之前学的第三讲的知识"旋转矩阵到旋转向量的转换"，通过迹的性质求解转角和转轴，很容易求出来。

3.2 $\mathrm{SE}(3)$上的指数映射

同理，$\mathfrak{se}(3)$上的指数映射也可以由式(3.1)展开，下面我们进行结果的推导：$$\exp ({\mathbf{\xi }}^{\wedge })=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}({\mathbf{\xi }}^{\wedge }{)}^{\mathrm{n}}=\sum _{\mathrm{n}=0}^{\infty }\frac{1}{\mathrm{n}!}{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]}^{\mathrm{n}}$$ $$=\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]}^n.\text{\hspace{1em}(3.9)}$$ $${\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]}^2=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}({\mathbf{\varphi }}^{\wedge }{)}^2& {\mathbf{\varphi }}^{\wedge }\mathbf{\rho }\\ 0^T& 0\end{array}\right].\text{\hspace{1em}(3.10)}$$ $${\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]}^3=\left[\begin{array}{cc}({\mathbf{\varphi }}^{\wedge }{)}^2& {\mathbf{\varphi }}^{\wedge }\mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}({\mathbf{\varphi }}^{\wedge }{)}^3& ({\mathbf{\varphi }}^{\wedge }{)}^2\mathbf{\rho }\\ 0^T& 0\end{array}\right]\text{\hspace{1em}(3.11)}$$由(3.9-3.11)可知：$$\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]}^n=\mathbf{I}+\sum _{n=1}^{\infty }\frac{1}{n!}\left[\begin{array}{cc}({\mathbf{\varphi }}^{\wedge }{)}^n& {(\mathbf{\varphi }}^{\wedge }{)}^{n-1}\mathbf{\rho }\\ 0^T& 0\end{array}\right]$$ $$=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\mathbf{\varphi }}^{\wedge }{)}^n& {\sum }_{n=1}^{\infty }\frac{1}{n!}({\mathbf{\varphi }}^{\wedge }{)}^{n-1}\mathbf{\rho }\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\mathbf{\varphi }}^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{\varphi }}^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]\text{\hspace{1em}(3.12)}$$这里，第二项中令$n=n-1$，这时候求和的初值和分母都会改变，这样就和左上角的元素进行了求和类型的统一。
然后，我们最后进行整理：$$\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\mathbf{\varphi }}^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{\varphi }}^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]\triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J\rho }\\ 0^T& 1\end{array}\right]=\mathbf{T}.\text{\hspace{1em}(3.13)}$$和推导$\mathfrak{so}(3)$类似，我们令$\mathbf{\varphi }=\theta \mathbf{a}$，$\mathbf{a}$为单位向量，对指数矩阵进行泰勒展开得：$$\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{\varphi }}^{\wedge }{)}^n=\mathbf{I}+\frac{1}{2!}\theta {\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^2(a^{\wedge }{)}^2+\frac{1}{4!}{\theta }^3(a^{\wedge }{)}^3+\frac{1}{5!}{\theta }^4(a^{\wedge }{)}^4+\cdots$$ $$=\frac{1}{\theta }\underset{(1-cos\theta )的泰勒展开}{\underbrace{(\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4+\cdots )}})({\mathbf{a}}^{\wedge })+\frac{1}{\theta }\underset{(\theta -sin\theta )的泰勒展开}{\underbrace{(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdots )}}({\mathbf{a}}^{\wedge }{)}^2+\mathbf{I}$$ $$=\frac{1}{\theta }(1-cos\theta )({\mathbf{a}}^{\wedge })+\frac{\theta -sin\theta }{\theta }({\mathbf{aa}}^T-\mathbf{I})+\mathbf{I}$$ $$=\frac{sin\theta }{\theta }\mathbf{I}+(1-\frac{sin\theta }{\theta }){\mathbf{aa}}^T+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }\stackrel{\mathrm{def}}{=}\mathbf{J}\text{\hspace{1em}(3.14)}$$从式(3.13)可以看出，$\mathbf{\xi }$的指数映射左上角的$\mathbf{R}$式我们熟知的$\mathrm{SO}(3)$，与$\mathfrak{se}(3)$中的旋转部分$\mathbf{\varphi }$对应。右下角的$\mathbf{J}$由式(3.14)给出：$$\mathbf{J}=\frac{sin\theta }{\theta }\mathbf{I}+(1-\frac{sin\theta }{\theta }){\mathbf{aa}}^T+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(3.15)}$$这个结果和罗德里格斯公式相似，但不完全一样，平移部分经过指数映射之后，发生了一次以$\mathbf{J}$为系数矩阵的线性变换。后面我们将经常用到这个$\mathbf{J}$。
同理，我们也可以推导出对数映射，同时根据变换矩阵$\mathbf{T}$求$\mathfrak{se}(3)$上对应的向量有更容易的办法：根据左上角的$\mathbf{R}$计算旋转向量，右上角的$\mathbf{t}$满足：$$\mathbf{t}=\mathbf{J\rho }\text{\hspace{1em}(3.16)}$$这里$J$可以由$\mathbf{\varphi }$得到，因此$\mathbf{\rho }$也可以由式(3.16)这个线性方程组求解。
下面我们对李群李代数之间的转换关系进行总结，关系如图。

---

创作不易，希望大家支持，多多点赞收藏！！！！非常感谢！！！！

参考资料：
视觉SLAM十四讲：从理论到实践(第2版)(ISBN：9787121369421)
旋量代数与李群、李代数 戴建生(ISBN：9787040318456)