本文已参与「新人创作礼」活动，一起开启掘金创作之路。

前言

最近在看高翔和张涛写的《视觉SLAM十四讲》，记录一下学习的内容，方便以后复习。这一系列文章更像是这本书的读书笔记，所以结构脉络都与该书相似；更推荐有时间的同学去看原书，写的更加详细易懂。欢迎大家一起探讨学习视觉SLAM相关的知识。

SLAM是干什么的？

SLAM(simultaneous localization and mapping)全称即时定位与地图构建或并发建图与定位),顾名思义可以解决“定位”和“建图“两件事情。 SLAM 问题的本质：对运动主体自身和周围环境空间不确定性的估计。主流的slam技术应用有两种，分别是激光slam(基于激光雷达lidar 来建图导航)和视觉slam(vslam,基于单/双目摄像头视觉建图导航)。

经典的视觉 SLAM 框架是过去十几年内，研究者们总结的成果。这个框架本身，以及它所包含的算法已经基本定型，并且在许多视觉程序库和机器人程序库中已经提供。依靠这些算法，我们能够构建一个视觉 SLAM 系统，使之在正常的工作环境里实时进行定位与建图。如上图所示，我们八视觉slam分为几步：

1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
2. 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。VO 又称为前端（Front End）。过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift）。这是由于视觉里程计（在最简单的情况下）只估计两个图像间运动造成的。我们知道，每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确。为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
3. 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后，又称为后端（Back End）。。后端优化要考虑的问题， 就是如何从这些带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。
4. 回环检测（Loop Closing）。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。通
5. 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。地图的形式随SLAM的应用场合而定。大体上讲，它们可以分为度量地图与拓扑地图两种。度量地图强调精确地表示地图中物体的位置关系，通常我们用稀疏（Sparse）与稠密（Dense）对它们进行分类。相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如A，B点是连通的，而不考虑如何从A点到达B点的过程。

SLAM的数学建模

由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。这就把一段连续时间的运动变成了离散时刻 t = 1, . . . , K 当中发生的事情。在这些时刻，用 $\bm{x}$ 表示小萝卜自身的位置。于是各时刻的位置就记为 x1, . . . , xK，它们构成了机器人的轨迹。地图方面，我们设地图是由许多个路标（Landmark）组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 N 个，用 y1, . . . , yN表示它们。 机器人的运动方程可用下式表示： $\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}, \boldsymbol{w}_{k}\right)$ 其中，$u_k$是运动传感器的读数，$w_k$为噪声。 机器人的观测方程可用下式表示： $\boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}, \boldsymbol{v}_{k, j}\right)$ 这里，$v_{k,j}$是这次观测的噪声。

这两个方程描述了最基本的 SLAM 问题：当我们知道运动测量的读数 u，以及传感器的读数 z 时，如何求解定位问题（估计 x）和建图问题（估计 y）？这时，我们把 SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

机器人学基础

齐次矩阵

在机器人学中，我们习惯用一个坐标系来表达一个刚体的位姿，机器人的位置和姿态，其实就是机器人坐标系在世界坐标系的表达。我们使用齐次矩阵T来表达两个坐标系的变换关系，很显然其次矩阵T应该包括旋转和平移两个部分。 对于刚体的旋转，我们使用旋转矩阵$\boldsymbol R$表示，她是一个行列式为1的正交矩阵。 我们可以吧旋转矩阵的的集合定义如下： $S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\} .$

$S O(n)$ 是特殊正交群 (Special Orthogonal Group) 的意思。“群” 的内容等会儿还会再说。这个集合由 $n$ 维空间的旋转矩阵组成, 特别的, $S O(3)$ 就是三维空间的旋转 了。通过旋转矩阵, 我们可以直接谈论两个坐标系之间的旋转变换, 而不用再从基开始谈 起了。换句话说, 旋转矩阵可以描述刚体的旋转。

对于刚体的平移，我们可以用一个平移向量$\boldsymbol t$来表示。那么把向量$\boldsymbol a$移动到$\boldsymbol a^{\prime}$,可以表达为： $a^{\prime}=R a+t$

对于上面的这个式子，我们用矩阵的形式表达出来就是： $\left[\begin{array}{l}a^{\prime} \\ 1\end{array}\right]=\left[\begin{array}{ll}R & t \\ 0^{T} & 1\end{array}\right]\left[\begin{array}{l}a \\ 1\end{array}\right] \triangleq T\left[\begin{array}{l}a \\ 1\end{array}\right]$ 这里把我们向量的末尾加了一个“1”，变成了四维齐次坐标。这个T就是所谓的齐次变换矩阵，它有比较特殊的结构：左上为旋转矩阵，右侧为平移向量。左下为向量，右下为1。这种矩阵又称为特殊欧式群(Special Euclidean Group)

与 $S O(3)$ 一样, 求解该矩阵的逆表示一个反向的变换:

这里额外补充一点向量外积的知识：对于向量$\boldsymbol a, \boldsymbol b \in \mathbb{R}^{3}$,外积可以表达为： $a \times b=\left[\begin{array}{ccc}i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3}\end{array}\right]=\left[\begin{array}{c}a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{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] b \triangleq a^{\wedge} b .$ $\boldsymbol {a} \times \boldsymbol {b}=\|\boldsymbol {a}\|\|\boldsymbol {b}\| \sin (\theta) \boldsymbol {n}$ 外积的方向垂直于这两个向量, 大小为 $|a||b| \sin \langle a, b\rangle$, 是两个向量张成的四边形的 有向面积。对于外积, 我们引入了 $\wedge$ 符号, 把 $a$ 㝍成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric), 你可以将 ${ }^{\wedge}$ 记成一个反对称符号。这样就把外积 $a \times b$, 写成了矩阵 与向量的乘法 $a^{\wedge} b$, 把它变成了线件运算。这个符号将在后文经常用到, 请记住它。 外积只对三维向量存在定义, 我们还能用外积表示向量的旋转。根据右手法则，从向量a转到向量b，转动轴的方向其实就是a和b外积的方向，大小由a和b的夹角决定。

关于旋转的表示：旋转向量、欧拉角、四元数

关于刚体旋转的表示，除了上面介绍的旋转矩阵，但是旋转矩阵用9个量来表示一个三自由度的旋转，显然有些冗余，其次旋转矩阵自身带有约束，必须是行列式为1的旋转矩阵，这对我们以后的估计和优化会带来非常大的麻烦。因此有必要介绍其他的表示方法。 旋转向量 之前在介绍外积的时候提到说外积可以表示向量间的旋转。其实任意的旋转都可以用一个旋转轴和一个旋转角表示。因此我们定义一个旋转向量，其方向与旋转轴一致，长度等于旋转角。这里的旋转向量其实就是下面要接杀的李代数。对于一个旋转轴为 $\boldsymbol n$, 角度为 $\theta$ 的旋转, 显然, 它对应的旋转向 量为 $\theta \boldsymbol n$ 。由旋转向量到旋转矩阵的过程由罗德里格斯公式 (Rodrigues's Formula) 表明, 由于推导过程比较复杂, 我们不作描述, 只给出转换的结果

这里，符号 $^{\wedge}$ 是向量到反对称的转换符，前面介绍向量外积的时候有提到。 当然我们也可以从旋转矩阵计算旋转向量和转角：

转轴$\boldsymbol n$是矩阵R特征值1对应的特征向量。求解此方程，再归一化就可得到旋转轴。 欧拉角 欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。由于它等价于 ZY X 轴的旋转，我们就以 ZY X 为例。 假设一个刚体的前方（朝向我们的方向）为X 轴，右侧为 Y 轴，上方为 Z 轴，见下图 。那么，ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：

1. 绕物体的 Z 轴旋转，得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

此时，我们可以使用 [r, p, y]T 这样一个三维的向量描述任意旋转。这个向量十分的直观，我们可以从这个向量想象出旋转的过程。但是欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock①）：在俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适于插值和迭代，往往只用于人机交互中。

四元数 事实上，我们找不到不带奇异性的三维向量描述方式 。这有点类似于，当我们想用两个坐标表示地球表面时（如经度和纬度），必定存在奇异性（纬度为 ±90◦ 时经度无意义）。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。 回忆我们以前学习过的复数。我们用复数集 $\mathbb C$ 表示复平面上的向量，而复数的乘法则能表示复平面上的旋转：例如，乘上复数 i 相当于逆时针把一个复向量旋转 90 度。类似的，在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数是 Hamilton 找到的一种扩展的复数. 它既是紧凑的，也没有奇异性。 一个四元数 $q$ 拥有一个实部和三个虚部, 像这样:

其中 $i, j, k$ 为四元数的三个虚部。这三个虚部满足关系式:

由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数:

这里, $s$ 称为四元数的实部, 而 $v$ 称为它的虚部。如果一个四元数虚部为 0 , 称之为实四 元数。反之, 若它的实部为 0 , 称之为虚四元数。 我们可以用四元数表达对一个点的旋转。假设一个空间三维点 $\boldsymbol p=[x, y, z] \in \mathbb{R}^{3}$, 以 及一个由轴角 $\boldsymbol n, \theta$ 指定的旋转。三维点 $\boldsymbol p$ 经过旋转之后变成为 $\boldsymbol p^{\prime}$ 。如果使用矩阵描述, 则为$\boldsymbol p^{\prime} =\boldsymbol R \boldsymbol p$ 如果用四元数来描述,首先把三维空间点用一个虚四元数来描述:

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,用四 元数 $\boldsymbol q$ 表示这个旋转: $\boldsymbol q=\left[\cos \frac{\theta}{2}, \boldsymbol n \sin \frac{\theta}{2}\right] .$ 反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：

对上式的 θ 加上 2π，我们得到一个相同的旋转，但此时对应的四元数变成了 −q。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取 θ 为 0，则得到一个没有任何旋转的实四元数。

回到正题, 旋转后的点 $\boldsymbol p^{\prime}$ 即可表示为这样的乘积: $\boldsymbol p^{\prime}=\boldsymbol q \boldsymbol p \boldsymbol q^{-1}$ 计算结果的实部为 0 , 故为纯虚四元数。其虚部的三个分量表 示旋转后 $3 \mathrm{D}$ 点的坐标。

设四元数 $\boldsymbol q=q_{0}+q_{1} i+q_{2} j+q_{3} k$, 对应的旋转矩阵 $\boldsymbol R$ 为:

反之, 由旋转矩阵到四元数的转换如下。假设矩阵为 $\boldsymbol R=\left\{m_{i j}\right\}, i, j \in[1,2,3]$, 其对 应的四元数 $q$ 由下式给出:

值得一提的是, 由于 $\boldsymbol q$ 和 $-\boldsymbol q$ 表示同一个旋转, 事实上一个 $\boldsymbol R$ 对应的四元数表示并 不是惟一的。同时, 除了上面给出的转换方式之外, 还存在其他几种计算方法, 而本书都 省略了。实际編程中| 当 $q_{0}$ 接近 0 时, 其余三个分量会非常大, 导致解不稳定, 此时我们] 再考虑使用其他的方式进行转换。

李群和李代数基础

李群

前面介绍过，三位旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧式群SE(3)。我们可以观察到这两个"群"对于加法不封闭，二对于乘法封闭，换句话说就是：

我们首先介绍群（Group）的概念：是一种集合加上一种运算的代数结构。我们把集合记作 $A$, 运算记作 . , 那么群可以记作 $G=(A, \cdot)$ 。群要求这个运算满足以下几个条件:

1. 封闭性: $\forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A$.
2. 结合律： $\forall a_{1}, a_{2}, a_{3} \in A, \quad\left(a_{1} \cdot a_{2}\right) \cdot a_{3}=a_{1} \cdot\left(a_{2} \cdot a_{3}\right)$.
3. 么元: $\exists a_{0} \in A$, s.t. $\forall a \in A, \quad a_{0} \cdot a=\alpha \cdot a_{0}=\alpha$.
4. 逆: $\forall a \in A, \quad \exists a^{-1} \in A$, s.t. $a \cdot a^{-1}=a_{0}$.

矩阵中常见的群有：

• 一般线性群 GL(n) 指 n × n 的可逆矩阵，它们对矩阵乘法成群。
• 特殊正交群 SO(n) 也就是所谓的旋转矩阵群，其中 SO(2) 和SO(3) 最为常见。
• 特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换，如 SE(2) 和 SE(3)。 李群是指具有连续（光滑）性质的群。像整数群 Z 那样离散的群没有连续性质，所以不是李群。而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。

李代数

考虑任意旋转矩阵R，有

在等式两边对时间求导, 得到:

整理得:

这里我们引入符号$^{\vee}$，它与符号$^{\wedge}$正相反，可以将反对称矩阵变成与之对应的向量。即

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

等式两边右乘 $R(t)$, 由于 $R$ 为正交阵, 有:

可以看到, 每对旋转矩阵求一次导数, 只需左乘一个 $\phi^{\wedge}(t)$ 矩阵即可。为方便讨论, 我 们设 $t_{0}=0$, 并设此时旋转矩阵为 $\boldsymbol{R}(0)=\boldsymbol{I}$ 。按照导数定义, 可以把 $\boldsymbol{R}(t)$ 在 0 附近进行 一阶泰勒展开:

我们看到 $\phi$ 反应了 $\boldsymbol{R}$ 的导数性质, 故称它在 $S O(3)$ 原点附近的正切空间 (Tangent Space) 上。同时在 $t_{0}$ 附近, 设 $\phi$ 保持为常数 $\phi\left(t_{0}\right)=\phi_{0}$ 。则有

上式是一个关于 $R$ 的微分方程, 而且我们知道初始值 $R(0)=I$, 解之, 得:

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定 义如下: 李代数由一个集合 $\mathbb{V}$, 一个数域 $\mathbb{F}$ 和一个二元运算 [,] 组成。如果它们满足以下几条 性质, 称 $(\mathbb{V}, \mathbb{F},[,]$, 为一个李代数, 记作 $\mathfrak{g}$ 。

1. 尌闭性 $\forall X, Y \in \mathbb{V},[X, Y] \in \mathbb{V}$.
2. 双线性 $\forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}$, 有:

3. 自反性 $\quad \forall X \in \mathbb{V},[X, X]=0$.
4. 雅可比等价 $\forall X, Y, Z \in \mathbb{V},[X,[Y, Z]]+[Z,[Y, X]]+[Y,[Z, X]]=0$.

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

李代数 $\mathfrak{s o}(3)$和 $\mathfrak{s e}(3)$

之前提到的 $\phi$，事实上是一种李代数。SO(3) 对应的李代数是定义在 R3上的向量，我们记作 $\phi$。根据前面的推导，每个 $\phi$都可以生成一个反对称矩阵

在此定义下, 两个向量 $\phi_{1}, \phi_{2}$ 的李括号为:

读者可以去验证该定义下的李括号满足上面的几条性质。由于 $\phi$ 与反对称矩阵关系很 紧密, 在不引起歧义的情况下, 就说 $\mathfrak{s o}(3)$的元素是 3 维向量或者 3 维反对称矩阵, 不加 区别:

至此, 我们已消楚了 $\mathfrak{s o}(3)$的内容。它们是一个由三维向量组成的集合, 每个向量对 应到一个反对称矩阵, 可以表达旋转矩阵的导数。它与 $S O(3)$ 的关系由指数映射给定:

对于SE(3),他也有对应的李代数 $\mathfrak{s e}(3)$：

我们把每个 $\mathfrak{s e}(3)$ 元素记作 $\xi$, 它是一个六维向量。前三维为平移, 记作 $\rho$ : 后三维为 旋转, 记作 $\phi$, 实质上是 $\mathfrak{s o}(3)$ 元素。同时, 我们拓展了 ${ }^{\wedge}$ 符号的含义。在 $\mathfrak{s e}(3)$ 中, 同 样使用 $\wedge$ 符号, 将一个六维向量转换成四维矩阵, 但这里不再表示反对称。

指数和对数映射

上文提到了指数矩阵$exp(\phi^{\wedge} )$，在李群和李代数中又称为指数映射。那它到底该如何计算呢？这里我们直接给出结果：

回忆前一讲内容, 它和罗德里格斯公式, 如出一辑。这表明, $\mathfrak{s o}(3)$实际 上就是由所谓的旋转向量组成的空间, 而指数映射即罗德里格斯公式。通过它们, 我们把 $\mathfrak{s e}(3)$中任意一个向量对应到了一个位于 $S O(3)$ 中的旋转矩阵。反之, 如果定义对数映射, 我们也能把 $S O(3)$ 中的元素对应到$\mathfrak{s o}(3)$ 中:

不过我们通常不按照泰勒展开去计算对数映射。在介绍旋转的表示中，我们已经介绍过如何根据旋转矩阵计算对应的李代数，即利用迹的性质分别求解转角和转轴，采用那种方式更加省事一些。 同理，$\mathfrak{s e}(3)$ 的指数映射形式如下：

BCH公式

虽然我们已经清楚了 SO(3) 和 SE(3)上的李群与李代数关系，但是，当我们在 SO(3) 中完成两个矩阵乘法时，李代数中 $\mathfrak{s 0}(3)$ 上发生了什么改变呢？反过来说，当$\mathfrak{s 0}(3)$ 上做两个李代数的加法时，SO(3) 上是否对应着两个矩阵的乘积？如果成立的话，相当于：

遗憾的是，这个式子对于A,B为标量时成立，但A,B若是矩阵，则不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式（BCH 公式）给出。由于它完整的形式较复杂，我们给出它展开式的前几项：

其中[]为李括号。 $\mathrm{BCH}$ 公式告诉我们, 当处理两个矩阵指数之积时, 它们会产生一些由 李括号组成的余项。特别地, 考虑 $S O(3)$ 上的李代数 $\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee}$, 当 $\phi_{1}$ 或 $\phi_{2}$ 为小量时, 小量二次以上的项都可以被忽略掉。此时, $\mathrm{BCH}$ 拥有线性近似表达:

以第一个近似为例。该式告诉我们，当对一个旋转矩阵 $R_{2}$ （李代数为 $\phi_{2}$ ）左乘一个 微小旋转矩脌 $R_{1}$ (李代数为 $\phi_{1}$ ）时，可以近似地看作，在原在的李代数 $\phi_{2}$ 上, 加上了一 项 $J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}$ 。同理, 第二个近似描述了右乘一个微小位移的情况。于是, 李代数在 $\mathrm{BCH}$ 近似下，分成了左乘近似和右乘近似两种, 在使用时我们须加注意, 使用的是左乘模型还 是右韭模型。 本书以左乘为例。左乘 $\mathrm{BCH}$ 近似雅可比:

它的逆为:

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

这样，我们就可以谈论李群乘法与李代数加法的关系了。假定对某个旋转 R，对应的李代数为 ϕ。我们给它左乘一个微小旋转，记作 ∆R，对应的李代数为 ∆ϕ。那么，在李群上，得到的结果就是 ∆R · R，而在李代数上，根据 BCH近似，为：Jl−1(ϕ)∆ϕ + ϕ。合并起来，可以简单地写成：

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

这将为之后李代数上的做微积分提供了理论基础。同样的, 对于 $S E(3)$, 亦有类似的 $\mathrm{BCH}$ 近似公式:

这里 $\mathcal{J}_{l}$ 形式比较复杂, 它是一个 $6 \times 6$ 的矩阵, 读者可以参考 [6] 中式 $(7.82)$ 和 (7.83) 内容。由于我们在计算中不用到该雅可比, 故这里略去它的实际形式。

李代数求导

在SLAM中，我们经常会构建与位姿有关的函数，然后讨论该函数关于 位姿的导数，以调整当前的估计值。所以讨论李代数的求导非常的重要，但是SO(3),SE(3)上没有良好定义的加法，他们只是群。如果我们把T当成一个普通矩阵来优化处理，那就必须对它加以约束，那就必须对它加以约束。而李代数是由向量组成的，具有良好的加法运算。因此使用李代数解决求导问题的思路分为两种：

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

首先，考虑SO(3)的情况，假设对空间点p进行额旋转，得到Rp。宣布在要就算旋转后的点的坐标相对于旋转的导数，我们不严谨的记为：

由于 $S O(3)$ 没有加法, 所以该导数无法按照导数的定义进行计算。设 $R$ 对应的李代数为 $\phi$, 我们转而计算:

这里同样省略推到过程给出化简后的结果：

不过，由于这里仍然含有形式比较复杂的 $J_l$，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

扰动模型

另一种求导方式, 是对 $\boldsymbol{R}$ 进行一次扰动 $\Delta \boldsymbol{R}$ 。这个扰动可以乘在左边也可以乘在右 边, 最后结果会有一点儿微小的差异, 我们以左扰动为例。设左扰动 $\Delta \boldsymbol{R}$ 对应的李代数为 $\varphi$ 。然后, 对 $\varphi$ 求导, 即:

该式的求导比上面更为简单:

可见, 扰动模型相比于直接对李代数求导, 省去了一个雅可比 $J_{l}$ 的计算。这使得扰动 模型更为实用。请读者务必理解这里的求导运算, 这在位姿估计当中具有重要的意义。 最后, 我们给出 $S E(3)$ 上的扰动模型, 而直接李代数上的求导就不再介绍了。假设 某空间点 $p$ 经过一次变换 $T$ (对应李代数为 $\xi$ ), 得到 $T p$。现在, 给 $T$ 左乘一个扰动 $\Delta \boldsymbol{T}=\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right)$, 我们设扰动项的李代数为 $\delta \xi=[\delta \rho, \delta \phi]^{T}$, 那么: $\frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) p-\exp \left(\boldsymbol{\xi}^{\wedge}\right) p}{\delta \boldsymbol{\xi}}$ $\approx \lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) p-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}$ $=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \xi^{\wedge} \exp \left(\xi^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}$ $=\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \phi^{\wedge} & \delta \rho \\ \mathbf{0}^{T} & 0\end{array}\right]\left[\begin{array}{c}\boldsymbol{R p}+\boldsymbol{t} \\ 1\end{array}\right]}{\delta \xi}$ $=\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{c}\delta \phi^{\wedge}(\boldsymbol{R} p+\boldsymbol{t})+\delta \boldsymbol{\rho} \\ 0\end{array}\right]}{\delta \xi}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} p+\boldsymbol{t})^{\wedge} \\ 0^{T} & 0^{T}\end{array}\right] \triangleq(\boldsymbol{T} \boldsymbol{p})^{\odot} .$ 我们把最后的结果定义成一个算符 $\odot$ 2), 它把一个齐次坐标的空间点变换成一个 $4 \times 6$ 的矩阵。