详解SLAM中的李群和李代数（中）

1 概述

在上一篇文章《详解SLAM中的李群和李代数（上）》中，我们已经通过对李群求导引出了李代数。在这篇文章中，我们就系统总结一下李代数的相关知识。

2 李代数

2.1 定义

李代数是一个向量空间$\mathfrak{g}$与一个二元运算的组合。其中的二元运算称为李括号（Lie bracket），记为$[\cdot ,\cdot ]:\mathfrak{g}\times \mathfrak{g}\to \mathfrak{g}$。在这个公式中：

• $[\cdot ,\cdot ]$是一个二元运算符，表示两个元素之间的某种“乘法”操作。当然它不是普通的乘法，常见的运算操作是向量叉乘 $\times$ 或矩阵的换位子$[A,B]=AB-BA$。
• $\mathfrak{g}$ 是李代数的集合（通常是一个向量空间），$\mathfrak{g}\times \mathfrak{g}$ 就是说我们要从这个集合里取两个元素做运算，$\to \mathfrak{g}$表示运算的结果仍然属于同一个集合 $\mathfrak{g}$。也就是说李括号运算把两个李代数中的元素变成另一个李代数中的元素。其实就是封闭性的意思。

李代数需要满足以下三个公理：

1. 双线性：对于所有$x,y,z\in \mathfrak{g}$和所有标量$a,b$，有：
   $$[ax+by,z]=a[x,z]+b[y,z],[z,ax+by]=a[z,x]+b[z,y]$$

2. 反对称性：对于所有$x,y\in \mathfrak{g}$，有：
   $$[x,y]=-[y,x]$$

   特别地，这意味着$[x,x]=0$。

3. 雅可比恒等式：对于所有$x,y,z\in \mathfrak{g}$，有：
   $$[x,[y,z]]+[y,[z,x]]+[z,[x,y]]=0$$

如果是第一次接触到李代数这个概念，这三个特性公理并不太好理解，因为缺少实际的使用场景。但是这里还是举一个例子来加深印象。例如，三维向量空间${\mathbb{R}}^3$和其上定义的叉乘$\times$构成了一个李代数$({\mathbb{R}}^3,\times )$，李括号为$[\mathbf{u},\mathbf{v}]=\mathbf{u}\times \mathbf{v}$。它符合李代数的三个公理：

1. 双线性：假设有向量$\mathbf{u}=(1,0,0)$，$\mathbf{v}=(0,1,0)$，$\mathbf{w}=(0,0,1)$，以及标量$a=2$，$b=3$。

   • 计算$[a\mathbf{u}+b\mathbf{v},\mathbf{w}]$：
     $$[(2,0,0)+(0,3,0),(0,0,1)]=[(2,3,0),(0,0,1)]=(3,-2,0)$$
   • 计算$a[\mathbf{u},\mathbf{w}]+b[\mathbf{v},\mathbf{w}]$：
     $$2[(1,0,0),(0,0,1)]+3[(0,1,0),(0,0,1)]=2(0,1,0)+3(-1,0,0)=(3,-2,0)$$

   两式结果相等，符合双线性公理。

2. 反对称性：假设$\mathbf{u}=(1,0,0)$和$\mathbf{v}=(0,1,0)$。

   • 计算$[\mathbf{u},\mathbf{v}]$：
     $$(1,0,0)\times (0,1,0)=(0,0,1)$$
   • 计算 $[\mathbf{v},\mathbf{u}]$：
     $$(0,1,0)\times (1,0,0)=(0,0,-1)$$

   可以看到$[\mathbf{u},\mathbf{v}]=-([\mathbf{v},\mathbf{u}])$，这符合反对称性。

3. 雅可比恒等式：假设$\mathbf{u}=(1,0,0)$, $\mathbf{v}=(0,1,0)$, $\mathbf{w}=(0,0,1)$。

   • 计算$[\mathbf{u},[\mathbf{v},\mathbf{w}]]$：
     $$[\mathbf{u},(0,1,0)\times (0,0,1)]=[(1,0,0),(1,0,0)]=(0,0,0)$$
   • 计算 $[\mathbf{v},[\mathbf{w},\mathbf{u}]]$：
     $$[\mathbf{v},(0,0,1)\times (1,0,0)]=[(0,1,0),(0,1,0)]=(0,0,0)$$
   • 计算 $[\mathbf{w},[\mathbf{u},\mathbf{v}]]$：
     $$[\mathbf{w},(1,0,0)\times (0,1,0)]=[(0,0,1),(0,0,1)]=(0,0,0)$$

   因此，
   $$[\mathbf{u},[\mathbf{v},\mathbf{w}]]+[\mathbf{v},[\mathbf{w},\mathbf{u}]]+[\mathbf{w},[\mathbf{u},\mathbf{v}]]=(0,0,0)+(0,0,0)+(0,0,0)=(0,0,0)$$

   这验证了雅可比恒等式成立。

注意，笔者这里对于李代数的定义与《视觉SLAM十四讲》上的定义略有不同，但是表达的实际的含义是一样的，读者可以自行体会。

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

在上一篇文章《详解SLAM中的李群和李代数（上）》中笔者实现了对特殊正交群$SO(3)$的求导：
$$R(t)=\exp ([\mathbf{\omega }(t){]}_{\times }t)$$

其中$\exp$表示矩阵指数运算，$\mathbf{\omega }(t)$是表达旋转矩阵的旋转向量，$[\mathbf{\omega }(t){]}_{\times }$是其对应的反对称矩阵。去掉用于求导的自变量$t$，那么可以定义$SO(3)$对应的李代数是：
$$\mathfrak{so}(3)=\left\{[\mathbf{\omega }{]}_{\times }\in {\mathbb{R}}^{3\times 3}\mid \omega \in {\mathbb{R}}^3\right\}$$

也就是说，$\mathfrak{so}(3)$是所有实$3\times 3$反对称矩阵构成的集合，对应的李括号是两个元素之间的换位子（commutator）。

2.3 $\mathfrak{so}(3)$的李括号

这里的李括号也就是换位子运算具体定义是：对于任意$X,Y\in \mathfrak{so}(3)$，有：
$$[X,Y]=XY-YX$$

例如，在$\mathfrak{so}(3)$中，如果$X=[{\mathbf{\omega }}_1{]}_{\times }$和$Y=[{\mathbf{\omega }}_2{]}_{\times }$，则：
$$[[{\mathbf{\omega }}_1{]}_{\times },[{\mathbf{\omega }}_2{]}_{\times }]=[{\mathbf{\omega }}_1{]}_{\times }[{\mathbf{\omega }}_2{]}_{\times }-[{\mathbf{\omega }}_2{]}_{\times }[{\mathbf{\omega }}_1{]}_{\times }$$

这个表达式的结果仍然是一个反对称矩阵，并且它对应于向量${\mathbf{\omega }}_1\times {\mathbf{\omega }}_2$的反对称矩阵，即：
$$[[{\mathbf{\omega }}_1{]}_{\times },[{\mathbf{\omega }}_2{]}_{\times }]=[{\mathbf{\omega }}_1\times {\mathbf{\omega }}_2{]}_{\times }$$

这意味着在$\mathfrak{so}(3)$中，李括号运算等价于三维向量空间中的叉积运算，$\mathfrak{so}(3)$与上一小节中介绍的李代数$({\mathbb{R}}^3,\times )$的含义基本相同。

举例验证一下，假设有两个向量${\mathbf{\omega }}_1=(1,0,0{)}^T$和${\mathbf{\omega }}_2=(0,1,0{)}^T$，它们对应的反对称矩阵分别是：
$$[{\mathbf{\omega }}_1{]}_{\times }=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right],[{\mathbf{\omega }}_2{]}_{\times }=\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]$$

计算它们的李括号：
$$[[{\mathbf{\omega }}_1{]}_{\times },[{\mathbf{\omega }}_2{]}_{\times }]=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]-\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]$$

$$=\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]-\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]=\left[\begin{array}{ccc}0& 0& 1\\ -1& 0& 0\\ 0& -1& 0\end{array}\right]$$

这正是$[0,0,1{]}^T$向量对应的反对称矩阵，也就是${\mathbf{\omega }}_1\times {\mathbf{\omega }}_2=(0,0,1{)}^T$。

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

与引出特殊正交群$SO(3)$的李代数$\mathfrak{so}(3)$的推导一样，也可以对特殊欧式群 $SE(3)$对时间$t$微分来引出对应的李代数$\mathfrak{se}(3)$。假设一个刚体在三维空间随时间做欧式运动（即刚体运动，同时包括旋转和平移），那么欧式变换矩阵可以表示为：
$$T(t)=\left[\begin{array}{cc}R(t)& \mathbf{p}(t)\\ 0^T& 1\end{array}\right]\in SE(3)$$

其中：

• $R(t)\in SO(3)$ 表示旋转；
• $\mathbf{p}(t)\in {\mathbb{R}}^3$ 表示平移；
• 整个矩阵是一个$4\times 4$的齐次变换矩阵。

对$T(t)$的自变量时间$t$进行微分，即：
$$\frac{d}{dt}T(t)=\frac{d}{dt}\left[\begin{array}{cc}R(t)& \mathbf{p}(t)\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}\frac{d}{dt}R(t)& \frac{d}{dt}\mathbf{p}(t)\\ 0^T& 0\end{array}\right]$$

其中$\frac{d}{dt}R(t)=[\mathbf{\omega }(t){]}_{\times }R(t)$，这一点我们在前面引出李代数$\mathfrak{so}(3)$的时候推导过。$\mathbf{p}(t)$表示位移，那么$\mathbf{p}(t)$对时间$t$求导，即$\frac{d}{dt}\mathbf{p}(t)=\mathbf{v}(t)$是瞬时线速度。

因此：
$$\frac{d}{dt}T(t)=\left[\begin{array}{cc}[\mathbf{\omega }(t){]}_{\times }R(t)& \mathbf{v}(t)\\ 0^T& 0\end{array}\right]\text{\hspace{1em}(1)}$$

需要进一步说明的是，$\mathbf{v}(t)$ 是空间坐标系下的线速度。我们知道速度是一种矢量，而欧式变换包含旋转和平移两种变换，那么空间坐标系下线速度$\mathbf{v}(t)$可以分解成：

1. 由于旋转引起的“附加速度”：即使没有自身的平移，只要它在旋转，它上面的点也会因为旋转而产生一个速度。
2. 由于自身平移产生的速度：这是物体本身在自己坐标系下的移动速度。

数学表达如下：
$$\mathbf{v}(t)=[\mathbf{\omega }(t){]}_{\times }\mathbf{p}(t)+{\mathbf{v}}_{\text{body}}(t)\text{\hspace{1em}(2)}$$

这个公式其实就是刚体运动学中的速度合成公式：

• ${\mathbf{v}}_{\text{body}}$就是刚体自身平移产生的速度，即体坐标系下的线速度；
• $\mathbf{\omega }(t)$是刚体的角速度，通常也在体坐标系下表示；
• $\mathbf{p}(t)$表示相对于刚体原点的位置向量。
• $[\mathbf{\omega }(t){]}_{\times }\mathbf{p}(t)$就是由于旋转引起的附加速度，其是一种典型的圆周运动的速度：$\mathbf{\omega }(t)$方向垂直于圆周面，$\mathbf{p}(t)$定义了到旋转中心的距离半径，两者叉乘，即得因刚体旋转而具有的切向速度。

将式(2)代入式(1)，有：
$$\begin{array}{rl}\frac{d}{dt}T(t)& =\left[\begin{array}{cc}[\mathbf{\omega }(t){]}_{\times }R(t)& [\mathbf{\omega }(t){]}_{\times }\mathbf{p}(t)+{\mathbf{v}}_{\text{body}}(t)\\ 0^T& 0\end{array}\right]\\ & =\left[\begin{array}{cc}[\mathbf{\omega }(t){]}_{\times }& {\mathbf{v}}_{\text{body}}(t)\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}R(t)& \mathbf{p}(t)\\ 0^T& 1\end{array}\right]\end{array}$$

如果令：
$$\xi (t)=\left[\begin{array}{cc}[\mathbf{\omega }(t){]}_{\times }& {\mathbf{v}}_{\text{body}}(t)\\ 0^T& 0\end{array}\right]$$

那么有：
$$\frac{d}{dt}T(t)=\xi (t)T(t)$$

显然，在初始时刻欧式变换矩阵为单位矩阵，即$T(0)=I_4$，那么有微分方程：
$$\frac{d}{dt}T(t)=\xi (t)T(t),T(0)=I_4$$

这就又回到了上一篇文章《详解SLAM中的李群和李代数（上）》中介绍的一阶线性微分方程，其解为：
$$T(t)=\exp (\xi (t)t)$$

这就是从李代数$\mathfrak{se}(3)$到李群$SE(3)$的指数映射，矩阵$\xi (t)$就是$SE(3)$的李代数中集合的元素。

进一步的，去掉自变量$t$，将${\mathbf{v}}_{\text{body}}$重命名为$\mathbf{v}$，李代数 $\mathfrak{se}(3)$ 是所有形如：
$$\xi =\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

的矩阵构成的集合，其中 $\mathbf{\omega },\mathbf{v}\in {\mathbb{R}}^3$。

将其简记为：
$$\mathfrak{se}(3)=\left\{\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]|\mathbf{\omega },\mathbf{v}\in {\mathbb{R}}^3\right\}$$

如果，$\mathfrak{se}(3)$也可以用一个六维向量来表示：
$$\mathbf{\eta }=\left[\begin{array}{c}\mathbf{\omega }\\ \mathbf{v}\end{array}\right]\in {\mathbb{R}}^6$$

并定义对应的映射：
$$\hat{\mathbf{\eta }}=\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]$$

所以$\mathfrak{se}(3)$也可以表示为：
$$\mathfrak{se}(3)=\left\{\hat{\mathbf{\eta }}|\mathbf{\eta }\in {\mathbb{R}}^6\right\}$$

2.5 $\mathfrak{se}(3)$的李括号

$\mathfrak{se}(3)$ 中的李括号仍然是矩阵换位子。对于任意两个 ${\xi }_1,{\xi }_2\in \mathfrak{se}(3)$，有：
$$[{\xi }_1,{\xi }_2]={\xi }_1{\xi }_2-{\xi }_2{\xi }_1$$

如果写成向量形式 ${\mathbf{\eta }}_1=({\mathbf{\omega }}_1,{\mathbf{v}}_1{)}^T$, ${\mathbf{\eta }}_2=({\mathbf{\omega }}_2,{\mathbf{v}}_2{)}^T$，则可以推出：
$$[{\mathbf{\eta }}_1,{\mathbf{\eta }}_2]=\left[\begin{array}{c}{\mathbf{\omega }}_1\times {\mathbf{\omega }}_2\\ {\mathbf{\omega }}_1\times {\mathbf{v}}_2-{\mathbf{\omega }}_2\times {\mathbf{v}}_1\end{array}\right]$$

这是通过直接计算矩阵乘积和利用叉积与反对称矩阵的关系推导出来的，估计也不是很常用，因此略掉推导过程。

3 指数映射

3.1 预备知识

3.1.1 泰勒公式

设一个函数 $f(x)$ 在点 $x=a$ 处无穷可导，那么它可以展开为如下形式的泰勒级数：
$$f(x)=\sum _{n=0}^{\infty }\frac{f^{(n)}(a)}{n!}(x-a{)}^n$$

这个式子的意思是：用无穷多个多项式项来逼近原函数 $f(x)$。

如果取 $a=0$，则称为麦克劳林级数（Maclaurin series）：
$$f(x)=\sum _{n=0}^{\infty }\frac{f^{(n)}(0)}{n!}{x}^n$$

根据这个定义，有：
$$\begin{gathered} e^x=\sum _{n=0}^{\infty }\frac{1}{n!}{x}^n=1+x+\frac{x^2}{2!}+\frac{x^3}{3!}+\cdots \\ \sin (x)=x-\frac{x^3}{3!}+\frac{x^5}{5!}-\frac{x^7}{7!}+\cdots =\sum _{n=0}^{\infty }(-1{)}^n\frac{x^{2n+1}}{(2n+1)!} \\ \cos (x)=1-\frac{x^2}{2!}+\frac{x^4}{4!}-\frac{x^6}{6!}+\cdots =\sum _{n=0}^{\infty }(-1{)}^n\frac{x^{2n}}{(2n)!} \end{gathered}$$

3.1.2 反对称矩阵的幂次规律

给定一个单位三维向量 $\mathbf{\omega }=({\omega }_1,{\omega }_2,{\omega }_3{)}^T$，其对应的反对称矩阵 $[\mathbf{\omega }{]}_{\times }$ 为：
$$[\mathbf{\omega }{]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

假设 $A=[\mathbf{\omega }{]}_{\times }$，我们首先计算 $A^2$：
$$A^2=[\mathbf{\omega }{]}_{\times }^2=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

进行矩阵乘法运算后得到：
$$A^2=\left[\begin{array}{ccc}-{\omega }_2^2-{\omega }_3^2& {\omega }_1{\omega }_2& {\omega }_1{\omega }_3\\ {\omega }_1{\omega }_2& -{\omega }_1^2-{\omega }_3^2& {\omega }_2{\omega }_3\\ {\omega }_1{\omega }_3& {\omega }_2{\omega }_3& -{\omega }_1^2-{\omega }_2^2\end{array}\right]$$

因为这里$\mathbf{\omega }$ 是单位向量（即 ${\omega }_1^2+{\omega }_2^2+{\omega }_3^2=1$），则：
$$A^2=\left[\begin{array}{ccc}-(1-{\omega }_1^2)& {\omega }_1{\omega }_2& {\omega }_1{\omega }_3\\ {\omega }_1{\omega }_2& -(1-{\omega }_2^2)& {\omega }_2{\omega }_3\\ {\omega }_1{\omega }_3& {\omega }_2{\omega }_3& -(1-{\omega }_3^2)\end{array}\right]$$

进一步简化为：
$$A^2=\left[\begin{array}{ccc}-{\omega }_1^2& {\omega }_1{\omega }_2& {\omega }_1{\omega }_3\\ {\omega }_1{\omega }_2& -{\omega }_2^2& {\omega }_2{\omega }_3\\ {\omega }_1{\omega }_3& {\omega }_2{\omega }_3& -{\omega }_3^2\end{array}\right]=\mathbf{\omega }{\mathbf{\omega }}^T-\mathbf{I}$$

这里 $\mathbf{I}$ 是单位矩阵，$\mathbf{\omega }{\mathbf{\omega }}^T$ 是外积矩阵。

接下来计算 $A^3$：
$$A^3=A\cdot A^2=[\mathbf{\omega }{]}_{\times }\cdot (\mathbf{\omega }{\mathbf{\omega }}^T-\mathbf{I})$$

注意到 $[\mathbf{\omega }{]}_{\times }\mathbf{\omega }=0$ （因为反对称矩阵作用于自身方向的向量结果为零向量），因此：
$$A^3=-[\mathbf{\omega }{]}_{\times }+[\mathbf{\omega }{]}_{\times }(\mathbf{\omega }{\mathbf{\omega }}^T)=-[\mathbf{\omega }{]}_{\times }=-A$$

所以：
$$A^3=-A$$

再来看计算$A^4$：
$$A^4=A\cdot A^3=A\cdot (-A)=-A^2$$

根据前面的结果 $A^2=\mathbf{\omega }{\mathbf{\omega }}^T-\mathbf{I}$，于是：
$$A^4=-(\mathbf{\omega }{\mathbf{\omega }}^T-\mathbf{I})=-A^2$$

因此：
$$A^4=-A^2$$

最后计算 $A^5$：
$$A^5=A\cdot A^4=A\cdot (-A^2)=-A^3=-(-A)=A$$

所以：
$$A^5=A$$

通过上述推导，我们可以总结出反对称矩阵 $A=[\mathbf{\omega }{]}_{\times }$ 的一些重要幂次规律：

• $A=[\mathbf{\omega }{]}_{\times }$
• $A^2=\mathbf{\omega }{\mathbf{\omega }}^T-\mathbf{I}$
• $A^3=-A$
• $A^4=-A^2$
• $A^5=A$
• $A^6=A^2$
• $A^7=-A$
• $A^8=-A^2$

这些规律表明，对于反对称矩阵 $A=[\mathbf{\omega }{]}_{\times }$，其高次幂具有周期性，并且可以归约为 $A$、$A^2$ 和 $-A$ 的线性组合。

3.1.3 Chasles定理

从$SO(3)$李代数的推导可以知道，任何旋转变换可以等效为绕某个固定轴的旋转来表示，那么欧式变换（刚体变换）是不是也有类似的性质呢？确实如此，刚体运动学的Chasles定理指出：

任何一个刚体的位移都可以视为绕某条直线（称为螺旋轴或瞬时转动轴）的旋转加上沿这条直线的平移。

换句话说，任何刚体的位移可以分解为绕一个特定方向的旋转加上沿着这个方向的平移。这种组合运动被称为螺旋运动，这条特定方向确定的直线就是螺旋轴。在刚体运动过程中，这条直线是保持不变的。

根据前面$SE(3)$李代数推导可知，欧式变换可以表示为：
$$T=\exp (\xi )$$

如同旋转变换的旋转向量可以分解成单位轴向量和旋转角度一样，螺旋运动也可以由以下两个量来确定：

1. 单位螺旋轴 $\hat{s}$：描述运动的方向（包括旋转轴和平移方向）。
2. 标量 $\theta$：描述沿这个螺旋轴运动了多远。

即：
$$\xi =\hat{s}\theta$$

于是整个运动就可以写成：
$$T=\exp (\hat{s}\theta )$$

又因为：
$$\xi =\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

对这个矩阵提取$\theta$，很显然：

• $[\mathbf{\omega }{]}_{\times }=\theta \cdot [{\mathbf{\omega }}_{\text{unit}}{]}_{\times }$
• $0^T=\theta \cdot 0^T$
• $0=\theta \cdot 0$

那么必然存在单位螺旋轴 $\hat{s}$ 的矩阵：

$$\hat{s}=\left[\begin{array}{cc}[{\mathbf{\omega }}_{\text{unit}}{]}_{\times }& {\mathbf{v}}_{\text{unit}}\\ 0^T& 0\end{array}\right]$$

能满足$\mathbf{v}=\theta \cdot {\mathbf{v}}_{\text{unit}}$。这里的${\mathbf{\omega }}_{\text{unit}}$ 是单位旋转轴，${\mathbf{v}}_{\text{unit}}$ 是单位平移速度向量。

可能在这里有的读者会觉得有点难以理解，平移和旋转是两个独立的运动，为什么会共享同一个参数$\theta$？其实这里把螺旋运动理解成向一个木板拧螺丝就行了，拧螺丝拧的角度越大，打进木板的距离就越远。这里的$\theta$是一个统一的“运动参数”，它决定了旋转了多少、平移了多少，就像是“沿某个固定运动方向走多远”的度量。

数学上的表达总结如下：
$$\xi =\theta \cdot \hat{s}=\theta \cdot \left[\begin{array}{cc}[{\mathbf{\omega }}_{\text{unit}}{]}_{\times }& {\mathbf{v}}_{\text{unit}}\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]$$

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

根据前面的推导，已知旋转矩阵 $R\in SO(3)$ 可以通过李代数 $\mathfrak{so}(3)$ 的指数映射来表示：
$$R=\exp ([\mathbf{\omega }{]}_{\times })$$

这里的$\mathbf{\omega }$就是旋转向量，我们知道向量都等于自身的单位向量与模长的乘积，那么可以令$a$为$\mathbf{\omega }$的单位向量，$\theta$ 为 $\mathbf{\omega }$的模长。也即是：
$$\mathbf{\omega }=\theta a$$

取反对称矩阵，有：
$$[\mathbf{\omega }{]}_{\times }=[\theta a{]}_{\times }=\theta [a{]}_{\times }$$

令$A=[a{]}_{\times }$，根据预备知识介绍的泰勒公式，将标量指数函数推广到矩阵指数函数上，有：
$$R=\exp (\theta A)=\sum _{n=0}^{\infty }\frac{(\theta A{)}^n}{n!}$$

将 $\exp (\theta A)$ 展开为泰勒级数：
$$\exp (\theta A)=I+\theta A+\frac{{\theta }^2}{2!}{A}^2+\frac{{\theta }^3}{3!}{A}^3+\frac{{\theta }^4}{4!}{A}^4+\cdots$$

利用预备知识中提到的反对称矩阵的（如 $A^3=-A$, $A^4=-A^2$ 等），我们可以把所有项都写成 $I,A,A^2$ 的线性组合：
$$\begin{array}{rl}\exp (\theta A)& =I+\theta A+\frac{{\theta }^2}{2!}{A}^2-\frac{{\theta }^3}{3!}A-\frac{{\theta }^4}{4!}{A}^2+\frac{{\theta }^5}{5!}A+\cdots \\ & =I+\left(\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdots \right)A\\ & +\left(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots \right)A^2\end{array}$$

再利用预备知识中的正弦和余弦的泰勒公式展开：

• $\sin \theta =\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdots$
• $1-\cos \theta =\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots$

所以：
$$\exp (\theta A)=I+\sin \theta \cdot A+(1-\cos \theta )\cdot A^2$$

也可以表达为：
$$\exp (\theta [a{]}_{\times })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta [a{]}_{\times }$$

其中，$a$为$\mathfrak{so}(3)$集合中元素$\mathbf{\omega }$的单位向量，$A$为$a$的反对称矩阵，$\theta$ 为$\mathbf{\omega }$的模长。其实这里的$\mathbf{\omega }$就是旋转矩阵的旋转向量，这个公式也就是著名的罗德里格公式。

通过以上推导表明，$\mathfrak{so}(3)$其实就是旋转向量组成的向量空间，指数映射就是罗德里格公式。通过罗德里格公式，任意一个向量都可以对应到$SO(3)$中的旋转矩阵。反之，将$SO(3)$对应到$\mathfrak{so}(3)$就是对数映射，当然这里就是不用列出对数表达式，在《视觉SLAM十四讲》的第3讲中介绍了更简便的计算方法：

$$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),Ra=a$$

在书上没有给出$Ra=a$的具体解的过程，因为可能有多个解的情况，表述起来比较麻烦，大概的求解过程是：

步骤	内容
1.	计算旋转角度：$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right)$
2.	若 $\theta =0$：返回 $\mathbf{\omega }=0$ 或任意方向
3.	若 $\theta =\pi$：构造 $\mathbf{\omega }{\mathbf{\omega }}^T=\frac{1}{2}(R+I)$，取最大特征值对应的单位特征向量
4.	否则：计算反对称部分 $S=\frac{1}{2}(R-R^T)$，提取 $\mathbf{\omega }=\frac{1}{2\sin \theta }\left[\begin{array}{c}{S}_{32}\\ S_{13}\\ S_{21}\end{array}\right]$，然后归一化

其实从求解对数映射的解就可以看出，指数映射是一个满射，而不是单射。也就是任意$SO(3)$集合中的元素，都能在$\mathfrak{so}(3)$中找到一个元素与之对应；但是可能存在多个$\mathfrak{so}(3)$中的元素对应于一个$SO(3)$集合中的元素。其实这个不难理解，旋转是有周期性的，旋转360度和不转是一样的，只有范围限定在$(-\pi ,+\pi ]$之间，两者才可以一一对应。

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

根据前面的推导，特殊欧式群$SE(3)$的李代数$\mathfrak{se}(3)$，可以用一个4维矩阵表示，包含旋转和平移信息：
$$\xi =\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

其中 $\mathbf{\omega }$ 就是特殊正交群$SO(3)$的李代数的向量表示，$\mathbf{v}$ 表示平移速度。

根据准备知识中介绍的Chasles定理，有：
$$T=\exp (\xi )=\exp (\hat{s}\theta )$$

其中$\hat{s}$是单位螺旋轴矩阵：
$$\hat{s}=\left[\begin{array}{cc}[{\mathbf{\omega }}_{\text{unit}}{]}_{\times }& {\mathbf{v}}_{\text{unit}}\\ 0^T& 0\end{array}\right]$$

$\theta$则描述沿这个螺旋轴运动了多远。使用泰勒公式展开指数函数，有：
$$\exp (\xi )=\sum _{n=0}^{\infty }\frac{(\hat{s}\theta {)}^n}{n!}=I+\hat{s}\theta +\frac{(\hat{s}\theta {)}^2}{2!}+\frac{(\hat{s}\theta {)}^3}{3!}+\cdots$$

因此关键在于计算每一项$(\hat{s}\theta {)}^n$。对于第0项：
$$I=\left[\begin{array}{cc}I& 0\\ 0^T& 1\end{array}\right]$$

对于第1项，前面求$SO(3)$的时候已经定义$A=[{\mathbf{\omega }}_{\text{unit}}{]}_{\times }$，则：
$$\hat{s}\theta =\left[\begin{array}{cc}\theta A& \theta {\mathbf{v}}_{\text{unit}}\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}\theta A& \mathbf{v}\\ 0^T& 0\end{array}\right]$$

对于第2项：
$$\frac{(\hat{s}\theta {)}^2}{2!}=\frac{1}{2!}\left[\begin{array}{cc}(\theta A{)}^2& \theta A\mathbf{v}\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}\frac{(\theta A{)}^2}{2!}& \frac{(\theta A)\mathbf{v}}{2!}\\ 0^T& 0\end{array}\right]$$

对于第3项：
$$\frac{(\hat{s}\theta {)}^3}{3!}=\frac{1}{3!}\left[\begin{array}{cc}(\theta A{)}^3& (\theta A{)}^2\mathbf{v}\\ 0^T& 0\end{array}\right]=\left[\begin{array}{cc}\frac{(\theta A{)}^3}{3!}& \frac{(\theta A{)}^2\mathbf{v}}{3!}\\ 0^T& 0\end{array}\right]$$

以此类推，把所有项加起来，分块相加，有：
$$\begin{array}{rl}\exp (\xi )& =I+\hat{s}\theta +\frac{(\hat{s}\theta {)}^2}{2!}+\frac{(\hat{s}\theta {)}^3}{3!}+\cdots \\ & =\left[\begin{array}{cc}I& 0\\ 0^T& 1\end{array}\right]+\left[\begin{array}{cc}\theta A& \mathbf{v}\\ 0^T& 0\end{array}\right]+\left[\begin{array}{cc}\frac{(\theta A{)}^2}{2!}& \frac{(\theta A)\mathbf{v}}{2!}\\ 0^T& 0\end{array}\right]+\left[\begin{array}{cc}\frac{(\theta A{)}^3}{3!}& \frac{(\theta A{)}^2\mathbf{v}}{3!}\\ 0^T& 0\end{array}\right]+\cdots \\ & =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{(\theta A{)}^n}{n!}& {\sum }_{n=1}^{\infty }\frac{(\theta A{)}^{n-1}\mathbf{v}}{n!}\\ 0^T& 1\end{array}\right]\end{array}$$

对于这个结果的左上角元素，其实就是$SO(3)$的指数映射：
$$\exp (\theta A)=\sum _{n=0}^{\infty }\frac{(\theta A{)}^n}{n!}$$

而对于右上角元素，可以将索引平移一下，令 $k=n-1$，则：
$$\sum _{k=0}^{\infty }\frac{(\theta A{)}^k\mathbf{v}}{(k+1)!}$$

因此，$SE(3)$指数映射可以写成：
$$\exp (\xi )=\left[\begin{array}{cc}\exp (\theta A)& {\sum }_{n=0}^{\infty }\frac{(\theta A{)}^n\mathbf{v}}{(n+1)!}\\ 0^T& 1\end{array}\right]$$

但是为了进一步方便表达右上角元素的表达式，引入左不变雅可比矩阵$J$：
$$J=\sum _{n=0}^{\infty }\frac{(\theta A{)}^n}{(n+1)!}$$

接下来利用预备知识中介绍的反对称矩阵的周期性规律计算$J$：
$$\begin{array}{rl}J& =I+\frac{\theta }{2!}A+\frac{{\theta }^2}{3!}{A}^2+\frac{{\theta }^3}{4!}{A}^3+\cdots \\ & =I+\frac{1}{\theta }(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots )A+\frac{1}{\theta }(\frac{{\theta }^3}{3!}-\frac{{\theta }^5}{5!}+\cdots )A^2\end{array}$$

利用预备知识中的正弦和余弦的泰勒公式展开：

• $\theta -\sin \theta =\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdots$
• $1-\cos \theta =\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots$

可以得到：
$$J=I+\frac{1-\cos \theta }{\theta }A+\frac{\theta -\sin \theta }{\theta }{A}^2$$

由于$A=[a{]}_{\times }$，因此$J$也可以表达为：
$$\begin{array}{rl}J& =I+\frac{1-\cos \theta }{\theta }[a{]}_{\times }+\frac{\theta -\sin \theta }{\theta }(a{a}^T-\mathbf{I})\\ & =\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }[a{]}_{\times }\end{array}$$

$SE(3)$的指数映射最终结果如下：
$$\exp (\xi )=\left[\begin{array}{cc}\exp ([\mathbf{\omega }{]}_{\times })& J\mathbf{v}\\ 0^T& 1\end{array}\right]$$

其中：

• 旋转部分 $R=\exp ([\mathbf{\omega }{]}_{\times })$即$SO(3)$的指数映射。
• 平移部分 $\mathbf{t}=J\mathbf{v}$, 其中J是左不变雅可比矩阵：
  $$J=I+\frac{1-\cos \theta }{\theta }A+\frac{\theta -\sin \theta }{\theta }$$
  或
  $$J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }[a{]}_{\times }$$

与$SO(3)$类似，也可以推导出$SE(3)$到$\mathfrak{se}(3)$的对数映射。旋转部分可以按照之前介绍的$SO(3)$到$\mathfrak{so}(3)$的对数映射求出；平移部分由于$J$已知（通过左不变雅可比矩阵求得），可以解$\mathbf{t}=J\mathbf{v}$这个线性方程组来解得。具体数学表达式如下：
$$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),Ra=a,\mathbf{t}=J\mathbf{v}$$

4 总结

作为本篇的结束，整理了一张特殊正交群的内容的表格，如下所示：

内容	特殊正交群
李群	S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , det ⁡ ( R ) = 1 } SO(3) = \{ R \in \mathbb{R}^{3\times3} \mid R^T R = I, \det(R) = 1\} SO(3)={R∈R3×3∣RTR=I,det(R)=1}
李代数	$\mathfrak{so}(3)=\left\{[\mathbf{\omega }{]}_{\times }\in {\mathbb{R}}^{3\times 3}\mid \omega \in {\mathbb{R}}^3\right\}$
向量表示	$\omega \in {\mathbb{R}}^3$
李括号	$[[{\mathbf{\omega }}_1{]}_{\times },[{\mathbf{\omega }}_2{]}_{\times }]=[{\mathbf{\omega }}_1{]}_{\times }[{\mathbf{\omega }}_2{]}_{\times }-[{\mathbf{\omega }}_2{]}_{\times }[{\mathbf{\omega }}_1{]}_{\times }$ 或者 叉乘$\times$
指数映射	$\exp (\theta [a{]}_{\times })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta [a{]}_{\times }$
对数映射	$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),Ra=a$

特殊欧式群的内容则如下表所示：

内容	特殊欧式群
李群	$SE(3)=\{T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid R\in SO(3),t\in {\mathbb{R}}^3\}$
李代数	$\mathfrak{se}(3)=\left\{\xi =\left[\begin{array}{cc}[\mathbf{\omega }{]}_{\times }& \mathbf{v}\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{\omega },\mathbf{v}\in {\mathbb{R}}^3\right\}$
向量表示	$\mathbf{\eta }=\left[\begin{array}{c}\mathbf{\omega }\\ \mathbf{v}\end{array}\right]\in {\mathbb{R}}^6$
李括号	$[{\xi }_1,{\xi }_2]={\xi }_1{\xi }_2-{\xi }_2{\xi }_1$ 或者 $[{\mathbf{\eta }}_1,{\mathbf{\eta }}_2]=\left[\begin{array}{c}{\mathbf{\omega }}_1\times {\mathbf{\omega }}_2\\ {\mathbf{\omega }}_1\times {\mathbf{v}}_2-{\mathbf{\omega }}_2\times {\mathbf{v}}_1\end{array}\right]$
指数映射	$$\begin{gathered} \exp (\xi )=\left[\begin{array}{cc}\exp ([\mathbf{\omega }{]}_{\times })& J\mathbf{v}\\ 0^T& 1\end{array}\right] \\ J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }[a{]}_{\times } \end{gathered}$$
对数映射	$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),Ra=a,\mathbf{t}=J\mathbf{v}$

注意可能符号与书上的符号略有差异，读者可以自行对照参考。

另外，笔者也想说一点内容之外的感受。除非具有相当的抽象能力，第一次接触到像李群和李代数这样的知识是很难理解到位的，因为你完全没有应用这些知识的场景，再怎么认真看书，也不过是泛泛而过。在这种情况下，不如好好推导一遍其中的原理，举一些实际的例子来加深映像。另外，这些数学知识既高深又抽象（笔者大概翻阅了一下《李理论》的相关知识，本文的这些推导在行家面前估计不值一哂），我们也不用过多去纠结，主打一个能自己说服自己即可。如果读者只是像我一样的普通人，可能需要在以后的应用和实践中来更深入地理解这些知识了；目前我们需要做的，就是不要排斥这个新的工具，然后理解它，利用它——真理之道，就在其中。