SLAM数学基础

三维空间的刚体运动

刚体：刚体是指在运动中和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体。

旋转矩阵

$$ SO(n)={R \in \mathbb{R}^{n \times n}|R R^T = I, det(R)=1} $$

$SO(n)$是特殊正交群，刚体在两个坐标系之间的变换可以公式化表述为

$$ a’=Ra+t $$

$t$表示的是从a坐标系原点到b坐标系原点的向量

三维旋转矩阵的计算如下

$$
\begin{bmatrix}
e^T_1 e’_1 & e^T_1 e’_2 & e^T_1 e’_3\
e^T_2 e’_1 & e^T_2 e’_2 & e^T_2 e’_3 \
e^T_3 e’_1 & e^T_3 e’_2 & e^T_3 e’_3\
\end{bmatrix}
$$

$e_i$是某一坐标系的基向量

齐次坐标

连续变换坐标系时，使用上述的变换公式不是线性的表述，写起来会很麻烦，如

$$
c=R_2(R_1a+t_1)+t_2
$$

因而在三维向量的末尾加上一个1，就变为了四维向量，称为齐次坐标，变换式变为如下形式

$$
\begin{bmatrix}a’\1\\end{bmatrix}=\begin{bmatrix}R & t\o^T&1\\end{bmatrix}\begin{bmatrix}a\1\\end{bmatrix}=T\begin{bmatrix}a\1\\end{bmatrix}
$$

矩阵$T$称为变换矩阵，这样连续变换坐标系就变成了线性表示$\begin{bmatrix}c\1\\end{bmatrix}=T_2T_1\begin{bmatrix}a\1\\end{bmatrix}$

反向变换为

$$
T^{-1}=\begin{bmatrix}R^T & -R^Tt\o^T&1\\end{bmatrix}
$$

反对称矩阵

$$
a^{\wedge}=\begin{bmatrix}a_1 \ a_2\a_3\\end{bmatrix}^{\wedge}=\begin{bmatrix}0 & -a_3 & a_2 \ a_3 & 0 & -a_1 \ -a_2 & a_1 & 0\ \end{bmatrix}
$$

旋转向量和欧拉角

矩阵方式表达旋转至少有以下两个缺点

$SO(3)$的旋转矩阵有9个量，但是一次旋转只有3个自由度，这种表达方式是冗余的
旋转矩阵自身带有约束：它必须是个正交矩阵，而且行列式为1，这些约束会使求解变得困难

旋转向量

任意的旋转都可以用一个旋转轴和一个旋转角来刻画，我们使用一个向量，方向与旋转轴一致，长度等于旋转角，称之为旋转向量$n$。

旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明

$$
R = cos\theta I + (1 - cos\theta)nn^T + sin\theta n^{\wedge}
$$

旋转矩阵到旋转向量的转换过程有

$$
tr(R) = cos \theta tr(I) + (1 - cos\theta)tr(nn^T) + sin\theta tr(n^-) = 3 cos\theta + (1 - cos \theta) = 1 + 2 cos\theta
$$

$$
\theta=arccos \frac{tr(R) - 1}{2}
$$

关于转轴$n$，旋转轴上的向量在旋转后不发生变化，表明

$$
Rn=n
$$

欧拉角

以ZYX为例子，可以把任意旋转分解为一下3个轴上的转角

偏航角yaw
俯仰角pitch
滚转角roll

使用$\begin{bmatrix}r & p & y\end{bmatrix}^T$向量描述，但是欧拉角有一个重大的缺点就是会碰到著名的万向锁问题：在俯仰角为$\pm90^{\circ}$时，第一次旋转与第三次旋转将使用同一个轴，使得系统失去一个自由度，这被称为奇异性问题。由于这种原理欧拉角不适用于插值和迭代，往往只应用于人机交互中。

四元数

定义

一个四元数拥有一个实部和三个虚部：$q=q_0+q_1 i+q_2 j+q_3 k$，这三个虚部满足如下关系

$$
\begin{cases}
i^2=j^2=k^2=-1 \
ij=k,ji=-k \ jk=i,kj=-i \ki=j, ik=-j
\end{cases}
$$

$i,j,k$对应三个坐标轴，乘一次单位$i,j,k$表示旋转$180^{\circ}$

用四元数表示旋转

假设有一个空间三维点$q=\begin{bmatrix}x,y,z\end{bmatrix} \in R^3$，以及一个由单位四元数$q$指定的旋转，那么旋转之后的点变为$q’$公式表示为

$$
p=\begin{bmatrix}0,x,y,z\end{bmatrix}^T = \begin{bmatrix}0, v\end{bmatrix}^T, p’=qpq^{-1}
$$

变换的种类

李群和李代数

前言

在SLAM中，除了表达3D旋转与位移之外，我们还要对它们进行估计，因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事，需要对变换矩阵进行插值、求导、迭代等操作。例如，在经典ICP问题中，给定了两组3D点，我们要计算它们之间的变换矩阵。假设第一组的3D点为$\mathbf{P}={ \mathbf{p}_i | i = [1,2, \ldots, N] }$，第二组3D点为$\mathbf{Q}={ \mathbf{q}_i | i = [1,2, \ldots, N] }$，那我们实际要做的事情是求一个欧氏变换$\mathbf{T}$，使得$\mathbf{T}$满足

$$ \forall i, \quad \mathbf{q}_i = \mathbf{T} \mathbf{p}_i $$

注意这里使用了齐次坐标表示。通常，这许多个匹配过的点是通过特征匹配得到的，构成了一个超定方程。而由于噪声的存在，这个方程往往是无解的。因此我们转而计算一个最小二乘：

$$ \mathop {\min }\limits_{\mathbf{T} } u\left( {\mathbf{T} } \right) = \sum\limits_{i = 1}^N { { { \left| { {\mathbf{q}_i} - \mathbf{T} {\mathbf{p}_i} } \right| }^2 } } $$

这时问题就来了：如果用迭代方式求解这个优化时（尽管可以不用迭代方式来求），如何求目标函数$u$相对于$\mathbf{T}$的导数呢？首先，$\mathbf{T}$只有6 个自由度，最好能够在一个六维空间表达它，那么$u(\mathbf{T})$相对于这个六维空间的导数（雅可比矩阵）是一个$6 \times 6$的矩阵。其次，$\mathbf{T}$对于乘法是封闭的，但对加法不封闭，即任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭的。
出于这两个原因，我们希望有更好的数学工具帮助我们做这些事，而李群与李代数理论正好提供了这样的工具。李群与李代数广泛地用于机器人与计算机视觉领域，并在机器人动力学推导上占据重要地位。不过，由于SLAM不涉及过多的动力学推导。我们重点介绍它在SLAM中相关的几个重要的结果，而略去许多数学性质的证明。特别地，重点介绍$SO(3)$和$SE(3)$这两个李群与对应的李代数。

李代数基础

首先，我们来讨论较为简单的三维旋转群。为了说明它的结构，首先介绍群的概念。
群（Group）是一种集合加上一种运算的代数结构，记作$(A,\cdot)$。其中$A$代表集合，$\cdot$是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称$G=(A, \cdot)$为群。

封闭性： $\quad \forall a_1, a_2, \quad a_1 \cdot a_2 \in A$
结合律： $\quad \forall a_1, a_2, a_3, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot ( a_2 \cdot a_3)$
幺元： $\quad \exists a_0 \in A, \quad s.t. \quad \forall a \in A, \quad a_0 \cdot a = a \cdot a_0 = a$
逆： $\quad \forall a \in A, \quad \exists a^{-1} \in A, \quad s.t. \quad a \cdot a^{-1} = a_0$

读者可以记作“封结幺逆”（谐音凤姐咬你），并可以把一些常见的群放进去验证。例如整数的加法（幺元为0），去掉0后的有理数的乘法（幺元为1）。对于矩阵，可以找到一些常见的矩阵群，例如：

一般线性群$GL(n)$ 指$n \times n$的可逆矩阵，它们对矩阵乘法成群。
特殊正交群$SO(n)$也就是所谓的旋转矩阵群，其中$SO(2)$ 和$SO(3)$最为常见。正式的记法是：$SO(n) = { \mathbf{R} \in \mathbb{R}^{n \times n} | \mathbf{R R}^T = \mathbf{I}, det(\mathbf{R})=1 }$
特殊欧氏群$SE(n)$ 也就是前面提到的$n$维欧氏变换，如$SE(2)$和$SE(3)$。这里给出$SE(3)$的记法：$SE(3)=\left{ T = \begin{bmatrix}R & t\o^T&1\\end{bmatrix} \in \mathbb{R}^{4 \times 4}|\mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3 \right}$

群结构保证了在群上的运算具有良好的性质，而群论则研究群的各种结构和性质。

李群是指具有连续性质的群。并且，一般连续群上的运算还是无限可微，乃至解析的（解析比无限可微更强，它还要求任意点邻域的泰勒展开都收敛）。这个问题在20世纪初被称为希尔伯特第五问题，并已得到了解决。而李群，则指实数空间上的连续群。常见的李群包括上边提到的$GL(n), SO(n), SE(n)$，以及其他的如酉群$U(n)$，辛群$Sp(2n)$等等。

三维旋转群$SO(3)$

三维旋转群$SO(3)$是特殊正交群$SO(n)$在$n=3$时的特例，它们可以用来描述三维空间的旋转，其元素都是$3 \times3$ 的正交且行列式为$+1$的矩阵。假设有这样一个矩阵$\mathbf{R}$，满足$\mathbf{R} \mathbf{R}^T＝\mathbf{I}$。现在，考虑它随时间发生变化，即从$\mathbf{R}$ 变成了$\mathbf{R}(t)$，仍有$\mathbf{R}(t) \mathbf{R}(t) ^T = \mathbf{I}$。在等式两边对时间求导，得到：

$$
\mathbf{\dot{R} } (t) \mathbf{R} {(t)^T} + \mathbf{R} (t) \mathbf{\dot{R} } {(t)^T} = 0
$$

于是

$$
\mathbf{\dot{R} } (t) \mathbf{R} {(t)^T} = - \left( \mathbf{\dot{R} } (t) \mathbf{R} {(t)^T} \right)^T
$$

可以看出$\mathbf{\dot{R} } (t) \mathbf{R} {(t)^T}$是一个反对称矩阵。注意到对于任意一个$3 \times 3$的反对称矩阵，我们记它为$\mathbf{A}$。由于$\mathbf{A}^T=-\mathbf{A}$，所以它主对角线元素必为$0$，而非对角线元素则只有三个自由度。我们可以把它对应到一个向量$\mathbf{a}=[a_1, a_2, a_3]^T$中去：

$$
{\mathbf{a}^ \wedge } = \mathbf{A} = \begin{bmatrix}0 & a_3 & a_2\a_3&0&-a_1\-a_2&a_1&0 \end{bmatrix}
$$

其中$^{\wedge}$符号表示由向量转换为矩阵，反之我们也可以用符号$^{\vee}$定义由矩阵转换为向量的方式：

注意到这样定义的好处之一，是它与叉积的兼容性。我们可以直接把矩阵与任意向量的乘积$\mathbf{A} \mathbf{b}$ 写成 $\mathbf{a} \times \mathbf{b}$。读者可以自行验证这个兼容性。除此之外，这样定义的向量还有一些较好的性质，后文会提到。

现在，由于$\mathbf{\dot{R} } (t) \mathbf{R} {(t)^T}$是一个反对称矩阵，我们可以找到一个三维向量$\mathbf{\phi} (t) \in \mathbb{R}^3$与之对应。于是有：

$$
\mathbf{ \dot{R} } (t) \mathbf{R}(t)^T = \mathbf{\phi} (t) ^ {\wedge}
$$

左右各右乘$\mathbf{R}(t)$，由于$\mathbf{R}$为正交阵，有：

$$
\mathbf{ \dot{R} } (t) = \mathbf{\phi} (t)^{\wedge} \mathbf{R}(t) = \begin{bmatrix}0&- \phi_3&\phi_2\\phi_3&0&-\phi_1\-\phi_2&\phi_1&0 \end{bmatrix} \mathbf{R}(t)
$$

可以看到，每对旋转矩阵求一次导数，只需左乘一个$\mathbf{\phi}$矩阵即可。由于$\mathbf{\phi}$反映了$\mathbf{R}$的导数性质，故称它在$SO(3)$的正切空间(tangent space)上。同时，将上式类比于一个关于$\mathbf{R}$的微分方程，可得：

$$
\mathbf{R}(t) = \exp \left( \mathbf{\phi} (t) ^\wedge \right) \mathbf{R}(t_0)
$$

由此我们可以引出两个概念。

求$\mathbf{\phi}$的方法以及它的结构？——$\mathbf{\phi}$是对应到$SO(3)$上的李代数$\mathfrak{so}(3)$
$\exp( \mathbf{\phi})$如何计算？——李群与李代数间的指数/对数映射。下面我们一一加以介绍。

李代数

对于$SO(3)$和$SE(3)$，李代数可定义于李群的正切空间上，描述了李群中元素局部性质，分别把它们记作小写的$\mathfrak{so}(3)$和$\mathfrak{se}(3)$。首先，给出通用的李代数的定义。

李代数由一个集合$\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,Y,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} ] \quad [\mathbf{Z}, a \mathbf{X}+b\mathbf{Y}] = a [\mathbf{Z} \mathbf{X} ]+ b [\mathbf{ZY}]$
自反性：$\forall \mathbf{X} \in \mathbb{V}, [\mathbf{X} \mathbf{X}] = \mathbf{0}$
雅可比等价：$\forall \mathbf{X,Y,Z} \in \mathbb{V}, [\mathbf{X}, [\mathbf{YZ}] ] + [\mathbf{Z}, [\mathbf{YX}] ] + [\mathbf{Y}, [\mathbf{ZX}]]$

从表面上来看，李代数所需要的性质还是挺多的。其中二元运算被称为李括号。相比于群中的较为简单的二元运算，李括号表达了两个集合元素的差异。它不要求结合律，而满足反对称性，以及元素和自己做李括号之后为零的性质。作为类比，三维向量$\mathbb{R}^3$ 上定义的叉积$\times$是一种李括号，因此$\mathfrak{g} = (\mathbb{R}^3, \mathbb{R}, \times)$构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。

三维旋转群与对应的李代数

$SO(3)$对应的李代数是定义在$\mathbb{R}^3$上的向量，我们记作$\mathbf{\phi}$（注意这是个向量，虽然希腊字母的粗体不明显）。根据前面的推导，每个$\mathbf{\phi}$都可以生成一个反对称矩阵：

$$
\mathbf{\Phi} = \mathbf{\phi}^{\wedge} = \begin{bmatrix} 0&-\phi_3&\phi_2\\phi_3&0&-\phi_1\-\phi_2&\phi_1&0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}
$$

在此定义下，两个向量$\mathbf{\phi}_1, \mathbf{\phi}_2$的李括号为：$[\mathbf{\phi}_1, \mathbf{\phi}_2] = \mathbf{ \Phi }_1 \mathbf{ \Phi }_2 - \mathbf{ \Phi }_2 \mathbf{ \Phi }_1$

读者可以去验证该定义下的李括号满足上面的几条性质。由于$\mathbf{\phi}$ 与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$\mathfrak{so}(3)$的元素是3维向量或者3维反对称矩阵，不加区别：$\mathfrak{so}(3) = \left{ \Phi = \mathbf{\phi^\wedge} \in \mathbb{R}^{3 \times 3} | \mathbf{\phi} \in \mathbb{R}^3 \right}$

反对称矩阵有一些重要的性质，重点包括以下两条：$\mathbf{\phi} \mathbf{\phi}^T = \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} + | \mathbf{\phi} |^2 \mathbf{I}_{3 \times 3}$

当$\mathbf{\phi}$为单位向量时，进而有：$\mathbf{\phi} \mathbf{\phi}^T = \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} + \mathbf{I}1$
$\mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} = - \mathbf{\phi}^{\wedge}$

这两条性质读者也可以自行验证，我们在指数映射中会用到。

至此，我们已清楚了$\mathfrak{so}(3)$的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。现在来考虑$\exp ( \mathbf{\phi}^{\wedge} )$是如何计算的，为此我们引入指数映射。

指数映射

首先，回忆任意矩阵的指数映射。它可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。

$$
\exp(\mathbf{A}) = \sum\limits_{n = 0}^\infty {\frac{1}{ { n! } }{ \mathbf{A}^n} }
$$

同样地，对$\mathfrak{so}(3)$中任意一元素$\mathbf{\phi}$，我们亦可按此方式定义它的指数映射：

$$
\exp(\mathbf{\phi}^\wedge) = \sum\limits_{n = 0}^\infty {\frac{1}{ {n!} }{ (\mathbf{\phi}^{\wedge})^n} }
$$

现在我们来仔细看看它的含义。由于$\mathbf{\phi}$是三维向量，我们可以定义它的模长和它的方向，分别记作$\theta$和$\mathbf{a}$（注意这里记号是有含义的，此时$\mathbf{a}$是一个单位长度的向量），那么按照上式，可以推出如下公式，注意中间使用了上面讲到了两个反对称矩阵的性质：

$$
\exp \left( { {\mathbf{\phi} ^ \wedge } } \right) = \exp \left( {\theta {\mathbf{a}^ \wedge } } \right) = \sum\limits_{n = 0}^\infty {\frac{1}{ {n!} }{ {\left( {\theta {\mathbf{a}^ \wedge } } \right)}^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}{\left( { {\mathbf{a}^ \wedge } } \right)^4} + …\ =\mathbf{a} {\mathbf{a}^T} - {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + \theta {\mathbf{a}^ \wedge } + \frac{1}{ {2!} }\theta {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } - \frac{1}{ {3!} }{\theta ^3}{\mathbf{a}^ \wedge } + \frac{1}{ {4!} }{\theta ^4}{\left( { {\mathbf{a}^ \wedge } } \right)^4} + …\= \mathbf{a}{\mathbf{a}^T} + \left( {\theta - \frac{1}{ {3!} }{\theta ^3} + \frac{1}{ {5!} }{\theta ^5} - …} \right){\mathbf{a}^ \wedge } - \left( {1 - \frac{1}{ {2!} }{\theta ^2} + \frac{1}{ {4!} }{\theta ^4} - …} \right){\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 } + I + \sin \theta {\mathbf{a}^ \wedge }\= \cos \theta \mathbf{I} + (1 - \cos \theta )\mathbf{a}{\mathbf{a}^T} + \sin \theta {\mathbf{a}^ \wedge }
$$

最后我们得到了一个似曾相识的式子：

$$
\exp( \theta \mathbf{a} ) = \cos \theta \mathbf{I} + (1 - \cos \theta )\mathbf{a}{\mathbf{a}^T} + \sin \theta {\mathbf{a}^ \wedge }
$$

回忆前一节内容，它和罗德里格斯公式如出一辄。这表明，$\mathfrak{so}(3)$实际上就是由所谓的旋转向量组成的空间。特别地，当转轴取一定顺序时，李代数$\mathfrak{so}(3)$还会变为对应的欧拉角。通过罗德里格斯公式或者指数映射，我们把$\mathbb{R}^3$ 中的一个向量对应到了一个位于$SO(3)$中的3D旋转。

反之，如果定义对数映射，我们也能把$SO(3)$中的元素对应到$\mathfrak{so}(3)$中

$$
\mathbf{\phi} = \ln {\left( \mathbf{R} \right)^ \vee } = {\left( {\sum\limits_{n = 0}^\infty {\frac{ { { {\left( { - 1} \right)}^n } } }{ {n + 1} }{ {\left( { \mathbf{R} - \mathbf{I} } \right)}^{n + 1} } } } \right)^ \vee }
$$

其中$^\vee$表示从反对称矩阵到向量的对应关系，为$^\wedge$的逆运算。

读者可能会问，指数映射性质如何呢？它是一个双射吗？很遗憾，它只是一个满射。每个$SO(3)$中的元素，都可以找到$\mathfrak{so}(3)$中至少一个与之对应；但是可能存在多个$\mathfrak{so}(3)$中的元素，对应到同一个$SO(3)$元素上。至少对于旋转角$\theta$，我们知道它具有周期性。

$SO(3)$与$\mathfrak{so}(3)$的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算。

三维欧氏群与对应的李代数

下面我们来介绍三维欧氏群$SE(3)$以及对应的李代数$\mathfrak{se}(3)$。有了前面的基础，我们可以直接介绍它们的结构及运算了。$SE(3)$的结构已经在前面介绍群的时候给出：

$$
SE(3) = \left{ \mathbf{T} =\begin{bmatrix}\mathbf{R} & \mathbf{t} \{ {\mathbf{0}^T} } & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} | \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3\right}
$$

每个变换矩阵有六个四由度，故对应的李代数位于$\mathbb{R}^6$中：$\mathfrak{se}(3) = \left{ \mathbf{ \Xi } = \mathbf{\xi}^\wedge \in \mathbb{R}^{4 \times 4} | \mathbf{\xi} \in \mathbb{R}^6 \right}$

但是$^\wedge$不再对应到一个反对称关系，而是

$$
\mathbf{\xi}^\wedge = \begin{bmatrix}\mathbf{\rho} \ \mathbf{\phi} \end{bmatrix}^ \wedge = \begin{bmatrix} { {\mathbf{\phi} ^ \wedge } }&\mathbf{\rho} \{ {\mathbf{0}^T} }&0\end{bmatrix} = \mathbf{\Xi}
$$

可以看到，$\mathbf{\xi}$ 的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：$\mathbf{\dot{T} }(t) = \mathbf{\xi}^\wedge(t) \mathbf{T}(t)$，因此$\mathbf{T}(t) = \exp ( \mathbf{\xi}(t)^\wedge ) \mathbf{T}(t)$。那么$\mathfrak{se}(3)$上的指数映射如何呢？略加推导可得：

$$
\exp \left( { { \mathbf{\xi} ^ \wedge } } \right) = \begin{bmatrix} {\sum\limits_{n = 0}^\infty {\frac{1}{ {n!} }{ {\left( { {\mathbf{\phi} ^ \wedge } } \right)}^n} } }&{\sum\limits_{n = 0}^\infty {\frac{1}{ {\left( {n + 1} \right)!} }{ {\left( { {\mathbf{\phi} ^ \wedge } } \right)}^n} \mathbf{\rho} } }\ { {\mathbf{0}^T} }&1 \end{bmatrix}\= \begin{bmatrix} \mathbf{\Phi} &{\mathbf{J\rho} } \{ {\mathbf{0}^T} }&1 \end{bmatrix}
$$

左上角的$\mathbf{\Phi}$是我们熟知的$\mathfrak{so}(3)$中的元素，前文已经介绍过了。而右上角的$\mathbf{J}$则可整理为（设$\mathbf{\phi}=\theta\mathbf{a}$）：

$$
\mathbf{J} = \frac{ {\sin \theta } }{\theta } \mathbf{I} + \left( {1 - \frac{ {\sin \theta } }{\theta } } \right) \mathbf{a} { \mathbf{a}^T} + \frac{ {1 - \cos \theta } }{\theta }{ \mathbf{a}^ \wedge }
$$

因此我们就得到了$\mathfrak{se}(3)$的指数映射的关系。其对数映射亦可类比推得。

李代数求导与扰动模型

BCH公式与近似形式

使用李代数的一大目的是进行优化，在优化过程中导数是非常必要的信息。考虑一个问题，当在$SO(3)$中完成两个矩阵乘法时，李代数中$SO(3)$上发生了什么改变？反过来说，当$SO(3)$上做两个李代数的加法时，$SO(3)$上是否对应着两个矩阵的乘积？如果成立，相当于：$\exp( \phi _ {1}^ {\wedge } )\exp( \phi _ {2}^ {\wedge } )=\exp(( \phi _ {1} + \phi _ {2} ) \wedge )$ ？

如果$\phi_ {1} \phi_{2}$为标量，显然该式成立；但此处计算的是矩阵的指数函数，而非标量的指数。换言之是在研究下式是否成立：$ln(\exp(A)\exp(B)) = A + B$

该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由 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]]+ \cdots
$$

其中 $[]$ 为李括号。BCH 公式说明，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑$SO(3)$上的李代数，当$\phi_ {1}$ 或$\phi_{2}$为小量时，小于二次以上的项都可以被忽略掉。此时，BCH拥有线性近似表达：

$$
\ln (\exp( \phi_{1}^{\wedge} )\exp(\phi_{2}^{\wedge}))^\vee \approx \begin{cases}\mathbf{J}{l}(\phi{2})^{-1}\phi_{1}+\phi_{2} \quad 当\phi_{1}为小量 \\mathbf{J}{r} (\phi{1})^{-1}\phi_{2}+\phi_{1} \quad 当\phi_{2}为小量\end{cases}
$$

以第一个近似为例。该式说明，当对一个旋转矩阵$\mathbf{R}2$（李代数为$\phi_2$）左乘一个微小旋转矩阵$\mathbf{R}1$（李代数为$\phi_1$）时，可以近似地看作，在原有的李代数$\phi_2$上加上了一项$\mathbf{J}{l}(\phi{2})^{-1}\phi_{1}$。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须注意使用的是左乘模型还是右乘模型。

左乘 BCH 近似雅可比

$$
\mathbf{J}_ {l} =\mathbf{J}= \frac {\sin \theta }{\theta } \mathbf{I}+(1- \frac {\sin \theta }{\theta } ) aa^ {T} + \frac {1-\cos \theta }{\theta } a^{\wedge}
$$

它的逆为：

$$
J_ {l}^ {-1} = \frac {\theta }{2} \cot \frac {\theta }{2} I+(1- \frac {\theta }{2} \cot \frac {\theta }{2} ) aa^ {T} - \frac {\theta }{2} a^{\wedge}
$$

右乘雅可比仅需要对自变量取负号即可：

$$
\mathbf{J}r(\phi) = \mathbf{J}{l}(-\phi)
$$

BCH近似的意义

假定对某个旋转$\boldsymbol{R}$，对应的李代数为$\phi$。我们给它左乘一个微小旋转，记作$\Delta \boldsymbol{R}$，对应的李代数为$\Delta \phi$。那么，在李群上，得到的结果就是$\Delta \boldsymbol{R} \cdot \boldsymbol{R}$，而在李代数上，根据BCH近似，为$\mathbf{J}^{-1}_{l}(\phi) \Delta \phi + \phi$。合并起来，可以写成：

$$
\exp( \Delta \phi ^ {\wedge } )\exp( \phi ^{\wedge})=\exp( (\phi +\mathbf{J}_{l}^ {-1}(\phi )\Delta \phi )^ {\wedge } )
$$

反之，如果我们在李代数上进行加法，让一个$\phi$加上 $\Delta \phi$，可以近似为李群上的左右雅可比的乘法：

$$
\exp( (\phi +\Delta \phi )^{\wedge})=\exp(( \mathbf{J}{l} \Delta \phi )^{\wedge})\exp(\phi^ \wedge )=\exp( \phi^\wedge )\exp( (\mathbf{J} {r}\triangle \phi )^ {\wedge })
$$

这为在李代数上做微积分提供了理论基础。同样，对于$SE(3)$，亦有类似的BCH近似：

$$
\exp( \Delta \xi^{\wedge} )\exp (\xi^\wedge)\approx \exp((\mathcal{J}{l}^{-1}\Delta \xi +\xi )^{\wedge }), \ \exp(\xi^\wedge) \exp( \Delta \xi^ \wedge)\approx \exp((\mathcal{J}{r}^{-1}\Delta \xi +\xi )^{\wedge })
$$

这里的$\mathcal{J}_{l}$形式比较复杂，是一个6×6的矩阵。

$**SO(3)$李代数上的求导

在SLAM中，要估计一个相机的位置和姿态，该位姿是由$SO(3)$上的旋转矩阵或$SE(3)$ 上的变换矩阵描述的。设某个时刻机器人的位姿为$T$，它观察到了一个世界坐标位于$p$的点，产生了一个观测数据$z$。由坐标变换关系知：

$$
z = Tp + w
$$

其中$w$为随机噪声。由于它的存在，$z$ 往往不可能精确地满足$z=Tp$的关系。所以通常会计算理想的观测与实际数据的误差：

$$
e =z−Tp
$$

假设一共有$N$个这样的路标点和观测，于是就有$N$个上式。那么，对机器人的位姿估计，相当于是寻找一个最优的$T$，使得整体误差最小化：

$$
\min_\mathbf{T} J(\mathbf{T})= \sum_{i=1}^{N}\parallel z_ {i} - \mathbf{T} \mathbf{p}_ {i}\parallel_{2}^{2}
$$

求解此问题，需要计算目标函数$J$关于变换矩阵T的导数。

重点是构建与位姿有关的函数，讨论该函数关于位姿的导数，以调整当前的估计值。然而$SO(3),SE(3)$上并没有良好定义的加法，它们是群。如果把$\boldsymbol{T}$当成一个普通矩阵来处理优化，那就必须对它加以约束（旋转矩阵的约束是行列式值唯一，计算复杂）。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。

使用李代数解决求导问题的思路分为两种：

用李代数表示姿态，然后根据李代数加法来对李代数求导。
对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。

李代数求导

考虑$SO(3)$上的情况。假设对一个空间点$\boldsymbol{p}$进行了旋转，得到了$\boldsymbol{Rp}$。计算旋转之后点的坐标相对于旋转的导数，记为：$\frac{\partial \boldsymbol{Rp} }{\partial \boldsymbol{R} }$

由于$SO(3)$没有加法，所以该导数无法按照导数的定义进行计算$\boldsymbol{R}$。设对应的李代数为$\phi$，转而计算$\frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi} }$

按照导数的定义，推导出了旋转后的点相对于李代数的导数：$\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\phi} }=(-\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l}$

注：这里并不能按照矩阵微分来定义导数，只是一个记号。

扰动模型（左乘）

另一种求导方式是对$\boldsymbol{R}$进行一次扰动$\Delta \boldsymbol{R}$，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的差异。以左扰动为例，设左扰动$\Delta \boldsymbol{R}$对应的李代数为$\varphi$。对$\varphi$求导，即：

$$
\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\varphi} }=\lim_{\boldsymbol{\varphi} \rightarrow \boldsymbol{0} }\frac{\exp(\boldsymbol{\varphi}^\wedge) \exp(\boldsymbol{\phi}^\wedge)\boldsymbol{p} - \exp(\boldsymbol{\phi}^\wedge)\boldsymbol{p} }{\boldsymbol{\varphi} } \ =\lim_{\boldsymbol{\varphi} \rightarrow \boldsymbol{0} }\frac{(\boldsymbol{I}+{\varphi}^\wedge) \exp(\boldsymbol{\phi}^\wedge)\boldsymbol{p} - \exp(\boldsymbol{\phi}^\wedge)\boldsymbol{p} }{\boldsymbol{\varphi} }\ = \lim_{\boldsymbol{\varphi} \rightarrow \boldsymbol{0} }\frac{\boldsymbol{\varphi}^\wedge \boldsymbol{Rp} }{\boldsymbol{\varphi} } = \lim_{\boldsymbol{\varphi} \rightarrow \boldsymbol{0} }\frac{-(\boldsymbol{Rp})^\wedge \boldsymbol{\varphi} }{\boldsymbol{\varphi} } = -(\boldsymbol{Rp})^\wedge
$$

相比于直接对李代数求导，省去了一个雅可比矩阵的计算。这使得扰动模型更为实用，在位姿估计当中具有重要的意义。

$SE(3)$上的李代数求导

书中只给出$SE(3)$上的扰动模型。假设某空间点$\boldsymbol{p}$经过一次变换$\boldsymbol{T}$（对应李代数为$\boldsymbol{\xi}$），得到$\boldsymbol{Tp}$。给$\boldsymbol{T}$左乘一个扰动$\Delta \boldsymbol{T}= \exp(\delta \boldsymbol{\xi}^\wedge)$，设扰动项的李代数为 $\delta \boldsymbol{\xi} = [\delta \boldsymbol{\rho},\delta \boldsymbol{\phi}]^T$，那么：

$$
\frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi} }=\lim_{\delta \boldsymbol{\xi} \rightarrow \boldsymbol{0} }\frac{\exp(\delta \boldsymbol{\xi}^\wedge) \exp(\boldsymbol{\xi}^\wedge)\boldsymbol{p} - \exp(\boldsymbol{\xi}^\wedge)\boldsymbol{p} }{\boldsymbol{\xi} } \=\lim_{\delta \boldsymbol{\xi} \rightarrow \boldsymbol{0} }\frac{(\boldsymbol{I}+\delta \boldsymbol{\xi}^\wedge) \exp(\boldsymbol{\xi}^\wedge)\boldsymbol{p} - \exp(\boldsymbol{\xi}^\wedge)\boldsymbol{p} }{\delta \boldsymbol{\xi} }\ = \lim_{\delta \boldsymbol{\xi} \rightarrow \boldsymbol{0} }\frac {\begin{bmatrix} \delta\phi^\wedge & \delta \boldsymbol{\rho}\\boldsymbol{0}^T & 0\end{bmatrix}\begin{bmatrix} \boldsymbol{Rp}+\boldsymbol{t}\1\end{bmatrix} }{\delta \boldsymbol{\xi} } \ =\lim_{\delta \boldsymbol{\xi} \rightarrow \boldsymbol{0} }\frac {\begin{bmatrix} \delta\phi^\wedge(\boldsymbol{Rp}+\boldsymbol{t}) + \delta \boldsymbol{\rho}\ \boldsymbol{0}^T \end{bmatrix} }{\begin{bmatrix} \delta \boldsymbol{\rho},\delta \boldsymbol{\phi}\end{bmatrix}^T} \=\begin{bmatrix} \boldsymbol{I} & -(\boldsymbol{Rp}+\boldsymbol{t})^\wedge \ \boldsymbol{0}^T & \boldsymbol{0}^T\end{bmatrix}\xlongequal{def} (\boldsymbol{Tp})^{\bigodot}
$$

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