系列文章目录

SLAM基础篇01
SLAM基础篇02

系列文章目录
前言
SLAM是干什么的？
- SLAM的数学建模
机器人学基础
- 齐次矩阵
- 关于旋转的表示：旋转向量、欧拉角、四元数
李群和李代数基础
- 李群
- 李代数
- 李代数 so(3)\mathfrak{s o}(3)so(3)和 se(3)\mathfrak{s e}(3)se(3)
- 指数和对数映射
- BCH公式
- 李代数求导
- 扰动模型

前言

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

SLAM是干什么的？

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

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

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

SLAM的数学建模

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

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

机器人学基础

齐次矩阵

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

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

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

对于上面的这个式子，我们用矩阵的形式表达出来就是：
[a′1]=[Rt0T1][a1]≜T[a1]\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][a′1]=[R0Tt1][a1]≜T[a1]
这里把我们向量的末尾加了一个“1”，变成了四维齐次坐标。这个T就是所谓的齐次变换矩阵，它有比较特殊的结构：左上为旋转矩阵，右侧为平移向量。左下为向量，右下为1。这种矩阵又称为特殊欧式群(Special Euclidean Group)

SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}.S E(3)=\left\{T=\left[\begin{array}{cc} R & t \\ 0^{T} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right\} . SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.
与 SO(3)S O(3)SO(3) 一样, 求解该矩阵的逆表示一个反向的变换:
T−1=[RT−RTt0T1].T^{-1}=\left[\begin{array}{cc} R^{T} & -R^{T} t \\ 0^{T} & 1 \end{array}\right] . T−1=[RT0T−RTt1].

这里额外补充一点向量外积的知识：对于向量a,b∈R3\boldsymbol a, \boldsymbol b \in \mathbb{R}^{3}a,b∈R3,外积可以表达为：
a×b=[ijka1a2a3b1b2b3]=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10]b≜a∧b.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 .a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b.
a×b=∥a∥∥b∥sin⁡(θ)n\boldsymbol {a} \times \boldsymbol {b}=\|\boldsymbol {a}\|\|\boldsymbol {b}\| \sin (\theta) \boldsymbol {n}a×b=∥a∥∥b∥sin(θ)n
外积的方向垂直于这两个向量, 大小为 ∣a∣∣b∣sin⁡⟨a,b⟩|a||b| \sin \langle a, b\rangle∣a∣∣b∣sin⟨a,b⟩, 是两个向量张成的四边形的有向面积。对于外积, 我们引入了 ∧\wedge∧ 符号, 把 aaa 㝍成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric), 你可以将 ∧{ }^{\wedge}∧ 记成一个反对称符号。这样就把外积 a×ba \times ba×b, 写成了矩阵与向量的乘法 a∧ba^{\wedge} ba∧b, 把它变成了线件运算。这个符号将在后文经常用到, 请记住它。
外积只对三维向量存在定义, 我们还能用外积表示向量的旋转。根据右手法则，从向量a转到向量b，转动轴的方向其实就是a和b外积的方向，大小由a和b的夹角决定。

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

关于刚体旋转的表示，除了上面介绍的旋转矩阵，但是旋转矩阵用9个量来表示一个三自由度的旋转，显然有些冗余，其次旋转矩阵自身带有约束，必须是行列式为1的旋转矩阵，这对我们以后的估计和优化会带来非常大的麻烦。因此有必要介绍其他的表示方法。
旋转向量
之前在介绍外积的时候提到说外积可以表示向量间的旋转。其实任意的旋转都可以用一个旋转轴和一个旋转角表示。因此我们定义一个旋转向量，其方向与旋转轴一致，长度等于旋转角。这里的旋转向量其实就是下面要接杀的李代数。对于一个旋转轴为 n\boldsymbol nn, 角度为 θ\thetaθ 的旋转, 显然, 它对应的旋转向量为 θn\theta \boldsymbol nθn 。由旋转向量到旋转矩阵的过程由罗德里格斯公式 (Rodrigues’s Formula) 表明, 由于推导过程比较复杂, 我们不作描述, 只给出转换的结果
R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧.\boldsymbol R=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol n \boldsymbol n^{T}+\sin \theta \boldsymbol{n}^{\wedge} . R=cosθI+(1−cosθ)nnT+sinθn∧.
这里，符号 ∧^{\wedge}∧ 是向量到反对称的转换符，前面介绍向量外积的时候有提到。
当然我们也可以从旋转矩阵计算旋转向量和转角：
θ=arccos⁡(tr⁡(R)−12).\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right) . θ=arccos(2tr(R)−1).
Rn=n\boldsymbol R \boldsymbol n = \boldsymbol n Rn=n
转轴n\boldsymbol nn是矩阵R特征值1对应的特征向量。求解此方程，再归一化就可得到旋转轴。
欧拉角
欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。由于它等价于 ZY X 轴的旋转，我们就以 ZY X 为例。
假设一个刚体的前方（朝向我们的方向）为X 轴，右侧为 Y 轴，上方为 Z 轴，见下图。那么，ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：

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

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

四元数
事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于，当我们想用两个坐标表示地球表面时（如经度和纬度），必定存在奇异性（纬度为 ±90◦ 时经度无意义）。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。
回忆我们以前学习过的复数。我们用复数集 C\mathbb CC 表示复平面上的向量，而复数的乘法则能表示复平面上的旋转：例如，乘上复数 i 相当于逆时针把一个复向量旋转 90 度。类似的，在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数是 Hamilton 找到的一种扩展的复数. 它既是紧凑的，也没有奇异性。
一个四元数 qqq 拥有一个实部和三个虚部, 像这样:
q=q0+q1i+q2j+q3k,q=q_{0}+q_{1} i+q_{2} j+q_{3} k, q=q0+q1i+q2j+q3k,
其中 i,j,ki, j, ki,j,k 为四元数的三个虚部。这三个虚部满足关系式:
{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j\left\{\begin{array}{l} i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j \end{array}\right. ⎩⎨⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数:
q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3,q=[s, v], \quad s=q_{0} \in \mathbb{R}, v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}, q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3,
这里, sss 称为四元数的实部, 而 vvv 称为它的虚部。如果一个四元数虚部为 0 , 称之为实四元数。反之, 若它的实部为 0 , 称之为虚四元数。
我们可以用四元数表达对一个点的旋转。假设一个空间三维点 p=[x,y,z]∈R3\boldsymbol p=[x, y, z] \in \mathbb{R}^{3}p=[x,y,z]∈R3, 以及一个由轴角 n,θ\boldsymbol n, \thetan,θ 指定的旋转。三维点 p\boldsymbol pp 经过旋转之后变成为 p′\boldsymbol p^{\prime}p′ 。如果使用矩阵描述, 则为p′=Rp\boldsymbol p^{\prime} =\boldsymbol R \boldsymbol pp′=Rp
如果用四元数来描述,首先把三维空间点用一个虚四元数来描述:
p=[0,x,y,z]=[0,v].\boldsymbol p=[0, x, y, z]=[0, \boldsymbol v] . p=[0,x,y,z]=[0,v].
这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,用四元数 q\boldsymbol qq 表示这个旋转:
q=[cos⁡θ2,nsin⁡θ2].\boldsymbol q=\left[\cos \frac{\theta}{2}, \boldsymbol n \sin \frac{\theta}{2}\right] .q=[cos2θ,nsin2θ].
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
{θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]T/sin⁡θ2\left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right. {θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
对上式的 θ 加上 2π，我们得到一个相同的旋转，但此时对应的四元数变成了 −q。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取 θ 为 0，则得到一个没有任何旋转的实四元数。

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

设四元数 q=q0+q1i+q2j+q3k\boldsymbol q=q_{0}+q_{1} i+q_{2} j+q_{3} kq=q0+q1i+q2j+q3k, 对应的旋转矩阵 R\boldsymbol RR 为:
R=[1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22]\boldsymbol{R}=\left[\begin{array}{lll} 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right] R=⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤
反之, 由旋转矩阵到四元数的转换如下。假设矩阵为 R={mij},i,j∈[1,2,3]\boldsymbol R=\left\{m_{i j}\right\}, i, j \in[1,2,3]R={mij},i,j∈[1,2,3], 其对应的四元数 qqq 由下式给出:
q0=tr⁡(R)+12,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0.q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}} . q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21.
值得一提的是, 由于 q\boldsymbol qq 和 −q-\boldsymbol q−q 表示同一个旋转, 事实上一个 R\boldsymbol RR 对应的四元数表示并不是惟一的。同时, 除了上面给出的转换方式之外, 还存在其他几种计算方法, 而本书都省略了。实际編程中| 当 q0q_{0}q0 接近 0 时, 其余三个分量会非常大, 导致解不稳定, 此时我们] 再考虑使用其他的方式进行转换。

李群和李代数基础

李群

前面介绍过，三位旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧式群SE(3)。我们可以观察到这两个"群"对于加法不封闭，二对于乘法封闭，换句话说就是：
R1+R2∉SO(3)，R1R2∈SO(3),R_{1}+R_{2} \notin S O(3)，R_{1} R_{2} \in S O(3), R1+R2∈/SO(3)，R1R2∈SO(3),
我们首先介绍群（Group）的概念：是一种集合加上一种运算的代数结构。我们把集合记作 AAA, 运算记作 . , 那么群可以记作 G=(A,⋅)G=(A, \cdot)G=(A,⋅) 。群要求这个运算满足以下几个条件:

封闭性: ∀a1,a2∈A,a1⋅a2∈A\forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A∀a1,a2∈A,a1⋅a2∈A.
结合律： ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)\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)∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3).
么元: ∃a0∈A\exists a_{0} \in A∃a0∈A, s.t. ∀a∈A,a0⋅a=α⋅a0=α\forall a \in A, \quad a_{0} \cdot a=\alpha \cdot a_{0}=\alpha∀a∈A,a0⋅a=α⋅a0=α.
逆: ∀a∈A,∃a−1∈A\forall a \in A, \quad \exists a^{-1} \in A∀a∈A,∃a−1∈A, s.t. a⋅a−1=a0a \cdot a^{-1}=a_{0}a⋅a−1=a0.

矩阵中常见的群有：

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

李代数

考虑任意旋转矩阵R，有

RRT=I.R R^{T}=I . RRT=I.
在等式两边对时间求导, 得到:
R˙(t)R(t)T+R(t)R˙(t)T=0.\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0 . R˙(t)R(t)T+R(t)R˙(t)T=0.
整理得:
R˙(t)R(t)T=−(R˙(t)R(t)T)T.\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}\right)^{T} . R˙(t)R(t)T=−(R˙(t)R(t)T)T.
这里我们引入符号∨^{\vee}∨，它与符号∧^{\wedge}∧正相反，可以将反对称矩阵变成与之对应的向量。即
a∧=A=[0−a3a2a30−a1−a2a10],A∨=a.\boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} . a∧=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,A∨=a.
于是, 由于 R˙(t)R(t)T\dot{R}(t) \boldsymbol{R}(t)^{T}R˙(t)R(t)T 是一个反对称矩阵, 我们可以找到一个三维向量 ϕ(t)∈R3\phi(t) \in \mathbb{R}^{3}ϕ(t)∈R3 与之对应。于是有：
R˙(t)R(t)T=ϕ(t)∧.\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\phi(t)^{\wedge} . R˙(t)R(t)T=ϕ(t)∧.
等式两边右乘 R(t)R(t)R(t), 由于 RRR 为正交阵, 有:
R˙(t)=ϕ(t)∧R(t)=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]R(t).\dot{\boldsymbol{R}}(t)=\phi(t)^{\wedge} \boldsymbol{R}(t)=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \boldsymbol{R}(t) . R˙(t)=ϕ(t)∧R(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤R(t).
可以看到, 每对旋转矩阵求一次导数, 只需左乘一个 ϕ∧(t)\phi^{\wedge}(t)ϕ∧(t) 矩阵即可。为方便讨论, 我们设 t0=0t_{0}=0t0=0, 并设此时旋转矩阵为 R(0)=I\boldsymbol{R}(0)=\boldsymbol{I}R(0)=I 。按照导数定义, 可以把 R(t)\boldsymbol{R}(t)R(t) 在 0 附近进行一阶泰勒展开:
R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t).\begin{aligned} \boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{R}\left(t_{0}\right)\left(t-t_{0}\right) \\ &=\boldsymbol{I}+\phi\left(t_{0}\right)^{\wedge}(t) . \end{aligned} R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t).
我们看到 ϕ\phiϕ 反应了 R\boldsymbol{R}R 的导数性质, 故称它在 SO(3)S O(3)SO(3) 原点附近的正切空间 (Tangent Space) 上。同时在 t0t_{0}t0 附近, 设 ϕ\phiϕ 保持为常数 ϕ(t0)=ϕ0\phi\left(t_{0}\right)=\phi_{0}ϕ(t0)=ϕ0 。则有
R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t).\dot{R}(t)=\phi\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\phi_{0}^{\wedge} \boldsymbol{R}(t) . R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t).
上式是一个关于 RRR 的微分方程, 而且我们知道初始值 R(0)=IR(0)=IR(0)=I, 解之, 得:
R(t)=exp⁡(ϕ0∧t).R(t)=\exp \left(\phi_{0}^{\wedge} t\right) . R(t)=exp(ϕ0∧t).
每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定义如下:
李代数由一个集合 V\mathbb{V}V, 一个数域 F\mathbb{F}F 和一个二元运算 [,] 组成。如果它们满足以下几条性质, 称 (V,F,[,](\mathbb{V}, \mathbb{F},[,](V,F,[,], 为一个李代数, 记作 g\mathfrak{g}g 。

尌闭性 ∀X,Y∈V,[X,Y]∈V\forall X, Y \in \mathbb{V},[X, Y] \in \mathbb{V}∀X,Y∈V,[X,Y]∈V.
双线性 ∀X,Y,Z∈V,a,b∈F\forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}∀X,Y,Z∈V,a,b∈F, 有:
[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y][a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]
自反性 ∀X∈V,[X,X]=0\quad \forall X \in \mathbb{V},[X, X]=0∀X∈V,[X,X]=0.
雅可比等价 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0\forall X, Y, Z \in \mathbb{V},[X,[Y, Z]]+[Z,[Y, X]]+[Y,[Z, X]]=0∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0.

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

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

之前提到的 ϕ\phiϕ，事实上是一种李代数。SO(3) 对应的李代数是定义在 R3上的向量，我们记作 ϕ\phiϕ。根据前面的推导，每个 ϕ\phiϕ都可以生成一个反对称矩阵
Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3\Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \in \mathbb{R}^{3 \times 3} Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3
在此定义下, 两个向量 ϕ1,ϕ2\phi_{1}, \phi_{2}ϕ1,ϕ2 的李括号为:
[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨.\left[\phi_{1}, \phi_{2}\right]=\left(\Phi_{1} \Phi_{2}-\Phi_{2} \Phi_{1}\right)^{\vee} . [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨.
读者可以去验证该定义下的李括号满足上面的几条性质。由于 ϕ\phiϕ 与反对称矩阵关系很紧密, 在不引起歧义的情况下, 就说 so(3)\mathfrak{s o}(3)so(3)的元素是 3 维向量或者 3 维反对称矩阵, 不加区别:
s0(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}.\mathfrak{s 0}(3)=\left\{\phi \in \mathbb{R}^{3}, \Phi=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\} . s0(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}.
至此, 我们已消楚了 so(3)\mathfrak{s o}(3)so(3)的内容。它们是一个由三维向量组成的集合, 每个向量对应到一个反对称矩阵, 可以表达旋转矩阵的导数。它与 SO(3)S O(3)SO(3) 的关系由指数映射给定:
R=exp⁡(ϕ∧).\boldsymbol{R}=\exp \left(\phi^{\wedge}\right) . R=exp(ϕ∧).
对于SE(3),他也有对应的李代数 se(3)\mathfrak{s e}(3)se(3)：
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}\mathfrak{s e}(3)=\left\{\xi=\left[\begin{array}{c} \rho \\ \phi \end{array}\right] \in \mathbb{R}^{6}, \rho \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \xi^{\wedge}=\left[\begin{array}{cc} \phi^{\wedge} & \rho \\ 0^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
我们把每个 se(3)\mathfrak{s e}(3)se(3) 元素记作 ξ\xiξ, 它是一个六维向量。前三维为平移, 记作 ρ\rhoρ : 后三维为旋转, 记作 ϕ\phiϕ, 实质上是 so(3)\mathfrak{s o}(3)so(3) 元素。同时, 我们拓展了 ∧{ }^{\wedge}∧ 符号的含义。在 se(3)\mathfrak{s e}(3)se(3) 中, 同样使用 ∧\wedge∧ 符号, 将一个六维向量转换成四维矩阵, 但这里不再表示反对称。

指数和对数映射

上文提到了指数矩阵exp(ϕ∧)exp(\phi^{\wedge} )exp(ϕ∧)，在李群和李代数中又称为指数映射。那它到底该如何计算呢？这里我们直接给出结果：
exp⁡(θa∧)=cos⁡θI+(1−cos⁡θ)aaT+sin⁡θa∧.\exp \left(\theta a^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a a ^ { T }}+\sin \theta \boldsymbol{a}^{\wedge} . exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧.
回忆前一讲内容, 它和罗德里格斯公式, 如出一辑。这表明, so(3)\mathfrak{s o}(3)so(3)实际上就是由所谓的旋转向量组成的空间, 而指数映射即罗德里格斯公式。通过它们, 我们把 se(3)\mathfrak{s e}(3)se(3)中任意一个向量对应到了一个位于 SO(3)S O(3)SO(3) 中的旋转矩阵。反之, 如果定义对数映射, 我们也能把 SO(3)S O(3)SO(3) 中的元素对应到so(3)\mathfrak{s o}(3)so(3) 中:
ϕ=ln⁡(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨.\phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee} . ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨.
不过我们通常不按照泰勒展开去计算对数映射。在介绍旋转的表示中，我们已经介绍过如何根据旋转矩阵计算对应的李代数，即利用迹的性质分别求解转角和转轴，采用那种方式更加省事一些。
同理，se(3)\mathfrak{s e}(3)se(3) 的指数映射形式如下：

BCH公式

虽然我们已经清楚了 SO(3) 和 SE(3)上的李群与李代数关系，但是，当我们在 SO(3) 中完成两个矩阵乘法时，李代数中 s0(3)\mathfrak{s 0}(3)s0(3) 上发生了什么改变呢？反过来说，当s0(3)\mathfrak{s 0}(3)s0(3) 上做两个李代数的加法时，SO(3) 上是否对应着两个矩阵的乘积？如果成立的话，相当于：
ln⁡(exp⁡(A)exp⁡(B))=A+B?\ln (\exp (A) \exp (B))=A+B ? ln(exp(A)exp(B))=A+B?
遗憾的是，这个式子对于A,B为标量时成立，但A,B若是矩阵，则不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式（BCH 公式）给出。由于它完整的形式较复杂，我们给出它展开式的前几项：
ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯\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 ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+⋯
其中[]为李括号。 BCH\mathrm{BCH}BCH 公式告诉我们, 当处理两个矩阵指数之积时, 它们会产生一些由李括号组成的余项。特别地, 考虑 SO(3)S O(3)SO(3) 上的李代数 ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee}ln(exp(ϕ1∧)exp(ϕ2∧))∨, 当 ϕ1\phi_{1}ϕ1 或 ϕ2\phi_{2}ϕ2 为小量时, 小量二次以上的项都可以被忽略掉。此时, BCH\mathrm{BCH}BCH 拥有线性近似表达:
ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{J1(ϕ2)−1ϕ1+ϕ2if ϕ1is small, Jr(ϕ1)−1ϕ2+ϕ1if ϕ2is small. \ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx \begin{cases}J_{1}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { if } \phi_{1} \text { is small, } \\ J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} & \text { if } \phi_{2} \text { is small. }\end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{J1(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1 if ϕ1 is small, if ϕ2 is small.
以第一个近似为例。该式告诉我们，当对一个旋转矩阵 R2R_{2}R2 （李代数为 ϕ2\phi_{2}ϕ2 ）左乘一个微小旋转矩脌 R1R_{1}R1 (李代数为 ϕ1\phi_{1}ϕ1 ）时，可以近似地看作，在原在的李代数 ϕ2\phi_{2}ϕ2 上, 加上了一项 Jl(ϕ2)−1ϕ1J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}Jl(ϕ2)−1ϕ1 。同理, 第二个近似描述了右乘一个微小位移的情况。于是, 李代数在 BCH\mathrm{BCH}BCH 近似下，分成了左乘近似和右乘近似两种, 在使用时我们须加注意, 使用的是左乘模型还是右韭模型。
本书以左乘为例。左乘 BCH\mathrm{BCH}BCH 近似雅可比:
Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧.J_{l}=J=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) a a^{T}+\frac{1-\cos \theta}{\theta} a^{\wedge} . Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧.
它的逆为:
Jl−1=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT−θ2a∧.J_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} I+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) a a^{T}-\frac{\theta}{2} a^{\wedge} . Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧.
而右乘雅可比仅需要对自变量取负号即可:
Jr(ϕ)=JI(−ϕ).J_{r}(\phi)=J_{I}(-\phi) . Jr(ϕ)=JI(−ϕ).
这样，我们就可以谈论李群乘法与李代数加法的关系了。假定对某个旋转 R，对应的李代数为 ϕ。我们给它左乘一个微小旋转，记作 ∆R，对应的李代数为 ∆ϕ。那么，在李群上，得到的结果就是 ∆R · R，而在李代数上，根据 BCH近似，为：Jl−1(ϕ)∆ϕ + ϕ。合并起来，可以简单地写成：
exp⁡(Δϕ∧)exp⁡(ϕ∧)=exp⁡((ϕ+Jl−1(ϕ)Δϕ)∧)\exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
反之, 如果我们在李代数上进行加法, 让一个 ϕ\phiϕ 加上 Δϕ\Delta \phiΔϕ, 那么可以近似为李群上带左右雅可比的乘法:
exp⁡((ϕ+Δϕ)∧)=exp⁡((JlΔϕ)∧)exp⁡(ϕ∧)=exp⁡(ϕ∧)exp⁡((JrΔϕ)∧)\exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(\boldsymbol{J}_{r} \Delta \phi\right)^{\wedge}\right) exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
这将为之后李代数上的做微积分提供了理论基础。同样的, 对于 SE(3)S E(3)SE(3), 亦有类似的 BCH\mathrm{BCH}BCH 近似公式:
exp⁡(Δξ∧)exp⁡(ξ∧)≈exp⁡((Jl−1Δξ+ξ)∧)exp⁡(ξ∧)exp⁡(Δξ∧)≈exp⁡((Jr−1Δξ+ξ)∧).\begin{aligned} &\exp \left(\Delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{l}^{-1} \Delta \xi+\xi\right)^{\wedge}\right) \\ &\exp \left(\xi^{\wedge}\right) \exp \left(\Delta \xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{r}^{-1} \Delta \xi+\xi\right)^{\wedge}\right) . \end{aligned} exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧).
这里 Jl\mathcal{J}_{l}Jl 形式比较复杂, 它是一个 6×66 \times 66×6 的矩阵, 读者可以参考 [6] 中式 (7.82)(7.82)(7.82) 和 (7.83) 内容。由于我们在计算中不用到该雅可比, 故这里略去它的实际形式。

李代数求导

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

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

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

∂(Rp)∂R\frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \boldsymbol{R}} ∂R∂(Rp)
由于 SO(3)S O(3)SO(3) 没有加法, 所以该导数无法按照导数的定义进行计算。设 RRR 对应的李代数为 ϕ\phiϕ, 我们转而计算:
∂(exp⁡(ϕ∧)p)∂ϕ\frac{\partial\left(\exp \left(\phi^{\wedge}\right) p\right)}{\partial \phi} ∂ϕ∂(exp(ϕ∧)p)
这里同样省略推到过程给出化简后的结果：
∂(Rp)∂ϕ=(−Rp)∧Jl.\frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \phi}=(-\boldsymbol{R} \boldsymbol p)^{\wedge} \boldsymbol{J}_{l} . ∂ϕ∂(Rp)=(−Rp)∧Jl.
不过，由于这里仍然含有形式比较复杂的 JlJ_lJl，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

扰动模型

另一种求导方式, 是对 R\boldsymbol{R}R 进行一次扰动 ΔR\Delta \boldsymbol{R}ΔR 。这个扰动可以乘在左边也可以乘在右边, 最后结果会有一点儿微小的差异, 我们以左扰动为例。设左扰动 ΔR\Delta \boldsymbol{R}ΔR 对应的李代数为 φ\varphiφ 。然后, 对 φ\varphiφ 求导, 即:
∂(Rp)∂φ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ.\frac{\partial(\boldsymbol{R p})}{\partial \varphi}=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} . ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p.
该式的求导比上面更为简单:
∂(Rp)∂φ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ≈lim⁡φ→0(1+φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0φ∧Rpφ=lim⁡φ→0−(Rp)∧φφ=−(Rp)∧.\begin{aligned} \frac{\partial(R p)}{\partial \varphi} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ & \approx \lim _{\varphi \rightarrow 0} \frac{\left(1+\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} R p}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(\boldsymbol{R} p)^{\wedge} \varphi}{\varphi}=-(\boldsymbol{R} p)^{\wedge} . \end{aligned} ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧.
可见, 扰动模型相比于直接对李代数求导, 省去了一个雅可比 JlJ_{l}Jl 的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算, 这在位姿估计当中具有重要的意义。
最后, 我们给出 SE(3)S E(3)SE(3) 上的扰动模型, 而直接李代数上的求导就不再介绍了。假设某空间点 ppp 经过一次变换 TTT (对应李代数为 ξ\xiξ ), 得到 TpT pTp。现在, 给 TTT 左乘一个扰动 ΔT=exp⁡(δξ∧)\Delta \boldsymbol{T}=\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right)ΔT=exp(δξ∧), 我们设扰动项的李代数为 δξ=[δρ,δϕ]T\delta \xi=[\delta \rho, \delta \phi]^{T}δξ=[δρ,δϕ]T, 那么:
∂(Tp)∂δξ=lim⁡δξ→0exp⁡(δξ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pδξ\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}}∂δξ∂(Tp)=limδξ→0δξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p
≈lim⁡δξ→0(I+δξ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pδξ\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δξ→0δξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p
=lim⁡δξ→0δξ∧exp⁡(ξ∧)pδξ=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \xi^{\wedge} \exp \left(\xi^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}=limδξ→0δξδξ∧exp(ξ∧)p
=lim⁡δξ→0[δϕ∧δρ0T0][Rp+t1]δξ=\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δξ→0δξ[δϕ∧0Tδρ0][Rp+t1]
=lim⁡δξ→0[δϕ∧(Rp+t)+δρ0]δξ=[I−(Rp+t)∧0T0T]≜(Tp)⊙.=\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} .=limδξ→0δξ[δϕ∧(Rp+t)+δρ0]=[I0T−(Rp+t)∧0T]≜(Tp)⊙.
我们把最后的结果定义成一个算符 ⊙\odot⊙ 2), 它把一个齐次坐标的空间点变换成一个 4×64 \times 64×6 的矩阵。

视觉slam学习|基础篇01相关推荐

《锋利的jQuery》学习---基础篇01（持续更新）
书是公司买的,借过来看看,感谢公司,其实我电脑里有这本书的电子书,但是发现自己越来越不喜欢用电脑看书了,严重费眼睛. 嘿嘿,看来这是要逼着我买kindle呀.好了,现在有书,废话不多说,从最基础的开始 ...
视觉slam学习笔记以及课后习题《第五讲特征点法视觉里程计》
这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题,因为是为了统计知识点来方便自己以后查阅,所以有部分知识可能不太严谨,如果给大家造成了困扰请见谅,大家发现了问题也可以私信或者评论给我及时改正 ...
视觉slam学习笔记以及课后习题《第三讲李群李代数》
前言这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题,因为是为了统计知识点来方便自己以后查阅,所以有部分知识可能不太严谨,如果给大家造成了困扰请见谅,大家发现了问题也可以私信或者评论给我及 ...
LINUX学习基础篇（十二）痕迹命令
LINUX学习基础篇(十二)痕迹命令系统痕迹命令 w命令 who命令 last命令 lastlog命令 lastb命令系统痕迹命令系统中有一些重要的痕迹日志文件,如/var/log/wtmp./ ...
深度学习基础篇【5】从0开始搭建YOLOV5 并进行测试
深度学习基础篇[5] 从0开始搭建 YOLOV5 并进行测试如何评价YOLO V5,那就必须拿"上一代"YOLO V4来做对照了.先说结论,YOLO V5 在性能上稍弱于YOL ...
深度学习基础篇（一）
深度学习基础篇(一) Part I 深度学习基础本书1-4章将带你了解一些基本概念:什么是深度学习,它可以用来做什么以及它如何工作.此外,熟悉使用深度学习解决数据问题的典型工作流程.如果还没怎么了解 ...
mysql经典总结文章_MySQL基础篇(01)：经典实用查询案例，总结整理
MySQL基础篇(01):经典实用查询案例,总结整理发布时间:2020-02-26 22:25:21 来源:51CTO 阅读:244 作者:知了一笑本文源码:GitHub·点这里 || GitEE ...
python传递参数格式_Python语言学习基础篇之Python发送Post请求之根据参数位置传参、数据类型、不同方式传参...
本文主要介绍了Python语言学习基础篇之Python发送Post请求之根据参数位置传参.数据类型.不同方式传参,通过具体的内容向大家展现,希望对大家Python语言的学习有所帮助. 目录一.验证 ...
回溯法采用的搜索策略_强化学习基础篇（三十四）基于模拟的搜索算法
强化学习基础篇(三十四)基于模拟的搜索算法上一篇Dyna算法是基于真实经验数据和模拟经验数据来解决马尔科夫决策过程的问题.本篇将结合前向搜索和采样法,构建更加高效的搜索规划算法,即基于模拟的搜索算法 ...

视觉slam学习|基础篇01