SLAM：

十四讲：

---

传感器约束了外部环境

测到的通常都是一些间接的物理量而不是直接的位置数据

只能通过一些间接的手段，从这些数据推算自己的位置

好处是没有对环境提出任何要求

camera：单目、双目、深度

Monocular、Stereo、RGB-D （红外光结构可 Time-of-flight TOF通过主动向物体发射光并接收返回的光，测出物体与相机之间的距离——这部分并不像双目相机那样通过软件计算来解决，而是通过物理的测量手段，所以相比于双目相机可节省大量的计算）——  主要用于室内

运动：Motion

结构：Structure 同时估计场景中物体的远近和大小

视差：disparity 近处的物体移动快、远处慢

尺度：scale 单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子——由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称不尺度不确定性（scale ambiguity）

在视觉 SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配，后端则主要是滤波与非线性优化算法；

度量地图 v.s. 拓扑地图

---

最基本的SLAM问题：

当知道运动测量的读数 u，以及传感器的读数 z 时，如何求解定位问题（估计 x）和建图问题（估计 y）？

状态估计：通过带有噪声的测量数据，估计内部隐藏状态变量

EKF：扩展卡尔曼滤波

非线性优化

---

SO(3)群

SE(3)群

欧拉角

四元数（用四元数表示旋转）

从真实世界到相机照片的变换是一个射影变换；如果相机的焦躁为无穷远，那么这个变换为仿射变换；

欧氏变换：保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变自身的样子；

相似变换：旋转部分多了一个缩放因子——相似变换群 Sim(3)

仿射变换：只要求A可逆，而不必是正交矩阵；

射影变换：最为一般；

---

第4讲：

李群和李代数

在 SLAM 中，除了表示之外，我们还要对它 们进行估计和优化。因为在 SLAM 中位姿是未知的，而我们需要解决形如“什么样的相机位姿最符 合当前观测数据”这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的 R, t，使 得误差最小化。

如前所言，旋转矩阵自身是带有约束的(正交且行列式为 1)。它们作为优化变量时，会引入额 外的约束，使优化变得困难。通过李群—李代数间的转换关系，我们希望把位姿估计变成无约束的 优化问题，简化求解方式。

--------------------------------------

李群就是连续（光滑）的群

李代数对应李群的正切空间，描述了李群局部的导数；

罗德里格斯公式是表示从旋转向量至旋转矩阵的转换过程

小so(3)的李代数空间就是由旋转向量组成的空间，其物理意义就是旋转向量

小so(3)的李代数：三维向量的集合，每个向量的反对称矩阵都可以表达大SO(3)的李群上旋转矩阵的导数；

也就是说，李群空间的做任意一个旋转矩阵 R 都可以用李代数空间的一个向量的反对称矩阵指数来近似；

某个时刻小萝卜的位姿为 T 。它观察到了一个世界坐标位于 p 的点，产生了 一个观测数据 z。那么，由坐标变换关系知:

z = T p + w.

==> 最小二乘

求解此问题，需要计算目标函数 J 关于变换矩阵 T 的导数。我们把具体的算法留到后面再讲。 这里的重点是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前 的估计值。然而，SO(3), SE(3) 上并没有良好定义的加法，它们只是群。如果我们把 T 当成一个普 通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有 良好的加法运算。因此，使用李代数解决求导问题的思路分为两种:

1. 用李代数表示姿态，然后根据李代数加法来对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。 第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。

相比于直接对李代数求导，省去了一个雅可比 Jl 的计算。这使得扰动模型更为实用

Sophus 早期版本只提供了双精 度的李群/李代数类。后续版本改写成了模板类。模板类的 Sophus 中可以使用不同精度的李群/李代 数，但同时增加了使用难度

---

第五章 相机

相 机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内 参，而有时需要你自己确定相机的内参，也就是所谓的标定。鉴于标定算法业已成熟(如著名的单 目棋盘格张正友标定法[25]

相机的位姿 R, t 又称为相机的外参数(Camera Extrinsics)。 相比于不变的内参，外参会随着相机运动发生改变，同时也是 SLAM 中待估计的目标，代表着机器 人的轨迹。

投影过程还可以从另一个角度来看。式(5.8)表明，我们可以把一个世界坐标点先转换到相机坐 标系，再除掉它最后一维的数值(即该点距离相机成像平面的深度)，这相当于把最后一维进行归 一化处理，得到点 P 在相机归一化平面上的投影:

如果对相机坐标同时乘以任意非零常数，归一化坐 标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

径向畸变（透镜本身）

切向畸变（摄像头传感器不垂直）

在这一节中，我们对相机的成像过程使用针孔模型进行了建模，也对透镜引起的径向畸变和切 向畸变进行了描述。实际的图像系统中，学者们提出了很多其他的模型，比如相机的仿射模型和透 视模型等，同时也存在很多其他类型的畸变。考虑到视觉 SLAM 中一般都使用普通的摄像头，针孔 模型及径向畸变和切向畸变模型已经足够，因此，我们不再对其他模型进行描述。

值得一提的是，存在两种去畸变处理(Undistort，或称畸变校正)做法。我们可以选择先对整 张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。或者，也可以从畸 变图像上的某个点出发，按照畸变方程，讨论其畸变前的空间位置。二者都是可行的，不过前者在 视觉 SLAM 中似乎更加常见一些。所以，当一个图像去畸变之后，我们就可以直接用针孔模型建立 投影关系，而不用考虑畸变了。因此，在后文的讨论中，我们可以直接假设图像已经进行了去畸变 处理。

虽然由视差计算深度的公式很简洁，但视差 d 本身的计算却比较困难。我们需要确切地知道左

眼图像某个像素出现在右眼图像的哪一个位置(即对应关系)，这件事亦属于“人类觉得容易而计 算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只 有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用 GPU 或FPGA 来实时计算

---

第七章 视觉里程计

比较重要核心的一章了， TBD

---

第8章：SLAM未来

常用开源 SLAM 方案

2007 年，Klein 等人提出了 PTAM(Parallel Tracking and Mapping)[66]，这也是视觉 SLAM 发展 过程中的重要事件。PTAM 的重要意义在于以下两点:

PTAM 提出并实现了跟踪与建图过程的并行化。我们现在已然清楚，跟踪部分需要实时响应 图像数据，而对地图的优化则没必要实时地计算。后端优化可以在后台慢慢进行，然后在必 要的时候进行线程同步即可。这是视觉 SLAM 中首次区分出前后端的概念，引领了后来许 多视觉 SLAM 系统的设计(我们现在看到的 SLAM 多半都分前后端)。 PTAM 是第一个使用非线性优化，而不是使用传统的滤波器作为后端的方案。它引入了关键 帧机制:我们不必精细地处理每一幅图像，而是把几个关键图像串起来，然后优化其轨迹和 地图。早期的 SLAM 大多数使用 EKF 滤波器或其变种，以及粒子滤波器等;在 PTAM 之后， 视觉 SLAM 研究逐渐转向了以非线性优化为主导的后端。由于之前人们未认识到后端优化 的稀疏性，所以觉得优化后端无法实时处理那样大规模的数据，而 PTAM 则是一个显著的 反例。 PTAM 同时是一个增强现实软件，演示了酷炫的 AR 效果(如图 8-2所示)。根据 PTAM估计的相机位姿，我们可以在一个虚拟的平面上放置虚拟物体，看起来就像在真实的场景中 一样。

IMU 为快速运动提供了较好的解决方式，而相机又能在慢速运动下解决IMU 的漂移问题——在这个意义下，它们二者是互补的。

语义SLAM：

SLAM 和语义的结合点主要有两个方面[9]:

1. 语义帮助 SLAM。传统的物体识别、分割算法往往只考虑一幅图，而在 SLAM 中我们拥有 一台移动的相机。如果我们把运动过程中的图片都带上物体标签，就能得到一个带有标签的 地图。另外，物体信息亦可为回环检测、BA 优化带来更多的条件。

2. SLAM 帮助语义。物体识别和分割都需要大量的训练数据。要让分类器识别各个角度的物 体，需要从不同视角采集该物体的数据，然后进行人工标定，非常辛苦。而 SLAM 中，由 于我们可以估计相机的运动，可以自动地计算物体在图像中的位置，节省人工标定的成本。 如果有自动生成的带高质量标注的样本数据，能够很大程度上加速分类器的训练过程。