《视觉SLAM十四讲》读书笔记

终于阅读了一遍这本听说过很多很多次的书，笔记做的比较粗糙，基本上是当作扫盲书籍在看，对于比较难的内容都没有细细推敲，等到要用的时候再看吧。

第二讲 初识SLAM

按相机的工作方式，可以把相机分成三大类：

• 单目（Monocular）：只有一个摄像头
• 双目（Stereo）：有两个摄像头
• 深度相机（（RGB-D）：除了能 够采集到彩色图片之外，还能读出每个像素离相机的距离，通常携带多个摄像头
• 此外还有全景相机、Event相机等特殊或新兴的种类

单目相机

只使用一个摄像头进行SLAM的做法称为单目SLAM。

单目相机的数据就是照片，，本质上是拍照时的场景（Scene），在相机的成像平面上留下的一个投影。它以 二维的形式反映了三维的世界。显然，这个过程丢掉了场景的一个维度：也就是所谓的深度（或距离）。

在单目相机中，我们无法通过单个图片来计算场景中物体离我们的距离（远近）。

必须移动相机之后，才能估计它的运动（Motion），同时估计场景中物体的远近和大小，不妨称之为结构（Structure）。

• 如果相机往右移动，那么图像里的东西就会往左边移动
• 近处的 物体移动快，远处的物体则运动缓慢。
• 当相机移动时，这些物体在图像上的运动，形成了视差。通过视差，我们就能定量地判断哪些物体离得远，哪些物体离的近。

如果把相机的运动和场景大小同时放大两倍，单目所看到的像是一样的。

• 单目SLAM估计的轨迹和地图，将与真实的轨迹、地图，相差一个因子，也就是所谓的尺度（Scale）
• 由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性。

平移之后才能计算深度，以及无法确定真实尺度问题的本质原因是通过单张图像无法确定深度。

双目相机 (Stereo) 和深度相机

双目相机和深度相机的目的，在于通过某种手段测量物体离我们的距离，克服单目无法知道距离的缺点。

知道了距离，就可以通过单个图像恢复场景的三维结构，也就消除了尺度不确定性。

尽管都是为测量距离，但双目相机与深度相机测量深度的原理是不一样的。

双目相机由两个单目相机组成，但这两个相机之 间的距离（称为基线（Baseline））是已知的。我们通过这个基线来估计每个像素的空间位 置——这和人眼非常相似。

• 对双目相机进行拓展，也可以搭建多目相机，但本质是一样的。双目相机测量到的深度范围与基线相关。基线距离越大，能够测量到的就越远。双目相机的距离估计是比较左右眼的图像获得的，并不依赖其他传感设备。

• 双目或多目相机的缺点是配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率限制，而且视差的计算非常消耗计算资源，需要使用GPU和FPGA设备加速后，才能实时输出整张图像的距离信息。

• 在现有的条件下，计算量是双目的主要问题之一。

深度相机（又称RGB-D 相机），以通过红外结构光或Time-of-Flight（ToF）原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体离相机的距离。

• 距离计算不像双目那样通过软件计算来解决，而是通过物理的测量手段，所以相比于双目可节省大量的计算量。
• 不过，现在多数 RGB-D 相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题，在SLAM方面，主要用于室内SLAM，室外则较难应用。

经典视觉SLAM框架：

整个视觉SLAM流程可以分为以下步骤：

1. 传感器信息读取。在视觉SLAM中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
2. 视觉里程计(Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端（FrontEnd）。
3. 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后， 又称为后端（Back End）。
4. 回环检测（Loop Closing）。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
5. 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。

如果把工作环境限定在静态、刚体，光照变化不明显、没有人为干扰的场景，那么，这个SLAM系统是相当成熟的了。

视觉里程计(Visual Odometry, VO)：

• 关心相邻图像之间的相机运动，最简单的情况是两张图像之间的运动关系。
• 定量估计相机运动必须了解相机与空间点的几何关系。
• VO能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。
• 叫里程计是是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的过去的信息没有关联。（短时记忆）。

仅通过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift）。这 是由于视觉里程计（在最简单的情况下）只估计两个图像间运动造成的。每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻， 导致经过一段时间之后，估计的轨迹将不再准确。（一旦有误差后面估计的再准也会有误差）

它将导致无法建立一致的地图。为了解决漂移问题，还需要两种技术：后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。

后端优化

后端优化主要指处理SLAM过程中噪声的问题。后端优化要考虑的问题，就是如何从带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。

相对的，视觉里程计部分，有时被称为“前端”。在SLAM框架中，前端给后端提供待优化的数据，以及这些数据的初始值。而后端负责整体的优化过程，它往往面对的只有 数据，不必关心这些数据到底来自什么传感器。在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

SLAM 问题的本质：对运动主体自身和周围环境空间不确定性的估计。

回环检测

回环检测，又称闭环检测（LoopClosure Detection），主要解决位置估计随时间漂移的问题。机器人经过一段时间运动后回到了原点，但是由 于漂移，它的位置估计值却没有回到原点。如果有某种手段，让机器人知道“回到了原点”这件事，或者把“原点”识别出来，再把位置估计值“拉”过去，就可以消除漂移了。这就是所谓的回环检测。

视觉回环检测常用方法：判断图像间的相似性，辨认它们来自同一个地方。

建图

建图（Mapping）是指构建地图的过程。地图是对环境的描述，但这个描述并不是固定的，需要视SLAM的应用而定。不同的应用对应不同的地图。大体上可以分为度量地图与拓扑地图两种。

度量地图（Metric Map）：度量地图强调精确地表示地图中物体的位置关系，通常我们用稀疏（Sparse）与稠密（Dense）对它们进行分类。

• 稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标（Landmark），那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略掉。
• 稠密地图着重于建模所有看到的东西。稠密地图通常按照某种分辨率，由许多个小块组成。二维度量地图是许多个小格子（Grid），三维则是许多小方块（Voxel）。一 般地，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。

拓扑地图（Topological Map）：相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。

拓扑地图是一个 图（Graph），由节点和边组成，只考虑节点间的连通性，例如A，B点是连通的，而不考虑如何从A点到达B点的过程。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。

SLAM问题的数学表述

运动方程：

xk是k时刻位置，uk是运动传感器的读数（有时也叫输入），wk为噪声。

观测方程：

yj为路标点，zkj为观测数据，vkj为观测里的噪声。

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

线性高斯系统：最简单，无偏的，最优估计可以由卡尔曼滤波器（KalmanFilter, KF）给出。

非线性非高斯系统：扩展卡尔曼滤波器（Extended Kalman Filter, EKF）和非线性优化两大类方法去求解。

第三讲 三维空间刚体运动

本章主要内容：一个刚体在三维空间中的运动是如何描述的

内积可以描述向量间的投影关系，而外积则是：

外积的方向垂直于这两个向量，大小为|a||b|sin⟨ a, b ⟩，是两个向量张成的四边形的有向面积。

外积只对三维向量存在定义，还能用外积表示向量的旋转。

描述旋转变化的矩阵叫旋转矩阵，旋转矩阵有一些特别的性质。事实上，它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以把旋转矩阵的集合定义如下：

SO(n)是特殊正交群（Special Orthogonal Group）的意思。

引入齐次坐标，将旋转矩阵和平移向量写在一个向量里，可以使整个关系变成一个线性关系，得到变换矩阵T。

关于变换矩阵，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为向量，右下角为1。这种矩阵又称为特殊欧氏群（Special Euclidean Group）：

其逆也表示一个反向的变换。

为了更紧凑地表达三维刚体运动，有了旋转向量，它是三维向量，其方向与旋转轴一致，而长度等于旋转角。

同样，对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，其维数为六维。

由旋转向量到旋转矩阵的过程由罗德里格斯公式（Rodrigues’s Formula）表明：

欧拉角：它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。由于旋转的不同，欧拉角也存在不同的定义方式。先绕X轴，再绕Y轴，最后绕Z轴，就得到了一个XYZ轴的旋转。同理还有ZYZ,ZYX等旋转方式。如果讨论更细一些，还需要区分每次旋转是绕固定轴旋转的，还是绕旋转之后的轴旋转的，这也会给出不一样的定义方式。

欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。（它等价于ZYX轴的旋转）

假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴，见图3-3。那么，转角相当于把任意旋转分解成以下三个轴上的转角：

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

欧拉角会碰到著名的万向锁问题，在俯仰角为±90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，在其它形式的欧拉角中也存在。由于这种原理，欧拉角不适于插值和迭代，往往只用于人机交互中。

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。

事实上，找不到不带奇异性的三维向量描述方式。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。但是在表达三维空间旋转时，有一种类似于复数的代数（乘法可表示旋转），四元数。四元数是Hamilton 找到的一种扩展的复数。它既是紧凑的，也没有奇异性。

一个四元数拥有一个实部和三个虚部：

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

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

这里，s称为四元数的实部，而v称为它的虚部。如果一个四元数虚部为0，称之为实四元数。反之，若它的实部为0，称之为虚四元数。

一个单位四元数可以表示三维空间中的任意一个旋转。

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

四元数和通常复数一样，常见的有四则运算、数乘、求逆、共轭、数乘和点乘等等。

3D空间变换还有其它的一些形式：相似、仿射、射影变换等。

• 相似变换：相似变换比欧氏变换多了一个自由度，它允许物体进行均匀的缩放

• 仿射变换：与欧氏变换不同的是，仿射变换只要求是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

• 射影变换：射影变换是最一般的变换。

实践内容：Eigen（向量计算）

第四讲 李群与李代数

这一章的主要问题是什么样的相机位姿最符合当前观测数据，一种典型的方法就是构造成优化问题，求解最优的R,T，使误差最小化。

李群与李代数的意义在于解决优化变量中带有约束的旋转矩阵（正交且行列式为1）带来的问题，把位姿估计变成无约束的优化问题，简化求解方式。

旋转矩阵和变换矩阵对加法是不封闭的，也就是说任意两个旋转矩阵/变换矩阵相加之后不再是一个旋转矩阵/变换矩阵。但是关于乘法是封闭的。

我们知道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合，我们把它叫做群。

群（Group）是一种集合加上一种运算的代数结构。

把集合记作A，运算记作·，那么群可以记作G = (A, ·)。群要求这个运算满足几个条件：

1. 封闭性：

2. 结合律：

3. 幺元：

4. 逆：

矩阵中常见的群有：

• 一般线性群GL(n)：指 n × n 的可逆矩阵，它们对矩阵乘法成群。
• 特殊正交群SO(n)：也就是所谓的旋转矩阵群，其中SO(2)和SO(3)最为常见。
• 特殊欧氏群SE(n)：也就是前面提到的n维欧氏变换，如SE(2)和SE(3)。

李群是指具有连续（光滑）性质的群。

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。

李群和李代数的相互转换关系：

使用李代数的一大动机是为了进行优化，而在优化过程中导数是非常必要的信息。在位姿估计时，常常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。

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

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

单目SLAM中由于尺度不确定性和尺度漂移，整个SLAM过程中的尺度会发生变化，所以需要进行一个相似变换。对应的有相似变换群Sim(3)。

实践部分：Sophus（李代数运算）

第五讲 相机与图像

相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程能够用一个几何模型进行描述。这个模型有很多种，其中最简单的称为针孔模型。

由于相机镜头上的透镜的存在，会使得光线投影到成像平面的过程中会产生畸变。因此，我们使用针孔和畸变两个模型来描述整个投影过程。这两个模型能够把外部的三维点投影到相机内部成像平面，构成了相机的内参数。

在相机中最终获得的是一个个像素，这需要在成像平面上对像进行采样和量化。

为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面 o-u-v。我们在像素平面得到了P′的像素坐标：[u, v]^T。

由于相机在运动，所以的相机坐标应该是它的世界坐标（记为Pw），根据相机的当前位姿，变换到相机坐标系下的结果。

它描述了的世界坐标到像素坐标的投影关系。相机的位姿R,t又称为相机的外参数。相比于不变的内参，外参会随着相机运动发生改变，同时也是SLAM 中待估计的目标，代表着机器人的轨迹。

相机在归一化平面上的投影：

畸变：为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响：一是透镜自身的形状对光线传播的影响，二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像面时的位置发生变化。

由透镜形状引起的畸变称之为径向畸变。（会使真实环境中的一条直线在图片中变成了曲线，越靠近图像边缘，这种现象更明显。叫径向畸变是因为这种不规则的畸变通常径向对称。）主要分为桶形畸变和枕形畸变。

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。 在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

除了透镜的形状会引入径向畸变外，在相机的组装过程中由于不能使得透镜和成像面 严格平行也会引入切向畸变。

存在两种去畸变处理（畸变校正）做法：

• 可以先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置；

• 或者先考虑图像中的某个点，然后按照去畸变方程，讨论它去畸变后的空间位置。

  

单目相机成像过程：

一共谈到了四种坐标：世界、相机、归一化相机和像素坐标

双目相机确定深度的原理是通过同步采集左右相机的图像，计算图像间视差，来估计每一个像素的深度。

通过几何关系可以计算视差。根据视差，我们可以估计一个像素离相机的距离。视差与距离成反比：视差越大，距离越近。视差最小为一个像素，于是双目的深度存在一个理论上的最大值。

双目相机估计视差的难度在于如何确切地知道左右眼中图像的对应关系。

RGB-D 相机模型能够主动测量每个像素的深度，目前主流原理有两大类：

• 通过红外结构光（Structured Light）来测量像素距离的
• 通过飞行时间法（Time-of-flight, ToF）原理测量像素距离的

无论是结构光还是ToF，RGB-D相机都需要向探测目标发射一束光线（通常是红外光）。在结构光原理中，相机根据返回的结构光图案，计算物体离自身的距离。而在ToF 中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体离自身的距离。

RGB-D相机可以在同一个图像位置， 读取到色彩信息和距离信息（相机会自动生成彩色图和深度图并完成配对），计算像素的 3D 相机坐标，生成点云（Point Cloud）。

计算机中图像的表示：

• 灰度图：每个像素位置（x, y）对应到一个灰度值I
• 深度图：记录各个像素离相机的距离
• 彩色图像：则需要通道（channel）的概念，记录RGB三个数值

实践内容：OpenCV（图像处理）

第六讲 非线性优化

这章的主要问题是如何优化处理噪声以及图优化

这一部分和图形学类似，不做笔记了。

常见的两种非线性优化方法：Gauss Newton 和 Levernberg-Marquardt。

实践部分：Ceres（最小二乘问题求解）和g2o（图优化）

第七 & 八讲 视觉里程计

视觉里程计的功能在于根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。

实现方法按是否需要提取特征，分为特征点法的前端以及不提特征的直接前端法。

特征点法：从图像中选取比较有代表性的点（路标/视觉特征）。这些点在相机视角发生少量变化后会保持不变，所以我们会在各个图像中找到相同的点。然后，在这些点的基础上，讨论相机位姿估计问题，以及这些点的定位问题。

特征是图像信息的另一种数字表达形式

还有许多更加稳定的局部图像特征，如著名的SIFT（需要GPU，CPU算很慢），SURF，ORB（FAST的改进版，加了方向性）等。相比于朴素的角点，这些人工设计的特征点能够拥有如下的性质：

1. 可重复性（Repeatability）：相同的“区域”可以在不同的图像中被找到。
2. 可区别性（Distinctiveness）：不同的“区域”有不同的表达。
3. 高效率（Efficiency）：同一图像中，特征点的数量应远小于像素的数量。
4. 本地性（Locality）：特征仅与一小片图像区域相关。

特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。

• 关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。
• 描述子通常是一个 向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。
• 描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量 空间上的距离相近，就可以认为它们是同样的特征点。

FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。ORB在FAST基础上改进了角点数量的指定和尺度及旋转的描述（尺度不变性用图像金字塔，不同层次降采样得到不同分辨率的图像；旋转则是灰度质心法，根据几何中心和质心的连线定义特征点方向）。ORB的描述子用的是改进的BRIEF 特征描述，用二进制描述关键点的特征。

特征匹配方法有暴力匹配法、快速相似近邻算法等。

求解匹配点对的不同方法：

• 当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组 2D 点估计运动。该问题用对极几何来解决。 由于单目视觉的尺度不变性，单目初始化不能只有纯旋转，必须要有一定程度的平移。
• 当相机为双目、RGB-D时，或者我们通过某种方法得到了距离信息，那问题就是根据两组3D点估计运动。该问题通常用ICP来解决。 ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化（迭代）方式的求解（类似于Bundle Adjustment）
• 如果我们有3D点和它们在相机的投影位置（3D-2D)，也能估计相机的运动。该问题通过PnP（Perspective-n-Point）求解。PnP问题有很多种求解方法，例如用三对点估计位姿的P3P，直接线性变换（DLT）， EPnP迭等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment。

直接法：根据图像的像素灰度信息来计算相机运动

在直接法中，我们并不需要知道点与点之间之间的对应关系，而是通过最小化光度误差（Photometric error）来求得它们。可以完全不用计算关键点和描述子，既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像梯度），直接法就能工作。

根据使用像素的数量，直接法分为稀疏、稠密和半稠密三种。相比于特征点法只能重构稀疏特征点（稀疏地图），直接法还具有恢复稠密或半稠密结构的能力。

1. 光流：描述了像素在图像中的运动

随着时间的经过，同一个像素会在图像中运动，而我们希望追踪它的运动过程。计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流法的代表有Lucas-Kanade光流（LK光流，OpenCV中有）

光流法基本假设：

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。（在实际过程中很可能不成立）

在LK光流中，还假设某一个窗口内的像素具有相同的运动。

在SLAM中，LK光流常被用来跟踪角点的运动。

总结：光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较慢（或采集频率较高）。

2. 直接法

   直接法的思路是根据当前相机的位姿估计值，来寻找p2的位置。优化问题是最小化光度误差（Photometric Error），也就是P的两个像的亮度误差。能够做这种优化的理由，仍是基于灰度不变假设。在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的。

   稀疏直接法：只看稀疏关键点

   半稀疏直接法：部分像素，如只使用带有梯度的像素点，舍弃像素梯度不明显的地方

   稠密直接法：所有像素

直接法的优缺点：

• 优点：
  • 可以省去计算特征点、描述子的时间
  • 只要求有像素梯度即可，无须特征点，可以在特征点确实的场合下使用
  • 可以构建半稠密乃至稠密的地图
• 缺点：
  • 非凸性，直接法完全依靠梯度搜索，降低目标函数来计算相机位姿，只有在运动很小时才能成功
  • 单个像素没有区分度，只能要么计算图像块， 要么计算复杂的相关性，每个像素意见不一致时只能少数服从多数
  • 灰度值不变是很强的假设

第九讲 实践章：设计前端

纯实践，由于不做SLAM，直接跳过

第十 & 十一讲 后端

后端优化：前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。所以，在视觉里程计的基础上，还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

后端优化有两种情况下的状态估计问题：

• 批量（Batch）：考虑一个更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更 新自己的状态，也会用未来的信息来更新自己
• 渐进（Incremental）：当前的状态只由过去的时刻决定，甚至只由前一个时刻决定

运动方程与观测方程的求解（假设噪声服从高斯分布）：

• 假设k时刻状态只与k-1时刻有关：马尔可夫性，使用卡尔曼滤波方法（EKF）
• 假设k时刻状态与之前所有状态的都有关：非线性优化为主体的优化框架

经典卡尔曼滤波器的步骤是预测-更新：

Bundle Adjustment（BA），是指从视觉重建中提炼出最优的 3D 模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of lightrays），在我们把相机姿态和特征点空间位置做出最优的调整(adjustment) 之后，最后收束到相机光心的这个过程，简称为BA。（类似与只有观测方程的SLAM问题）

BA能精确地优化每个相机位姿与特征点位置。不过在更大的场景中，大量特征点的存在会严重降低计算效率，导致计算量越来越大以至于无法实时化。在更大场景下后端优化方法使用的是位姿图。

位姿图：只保留关键帧的轨迹，一旦初始值给定就不优化路标点的位置，而只关心所有的相机位姿之间的联系。

SLAM后端优化还有种方法是因子图（贝叶斯网络/概率图）：

第十二讲 回环检测

主流视觉SLAM中检测回环的方式：词袋模型

回环检测的目的：消除SLAM过程中的累积误差，构建全局一致的轨迹和地图。

回环检测的关键就是如何有效地检测出相机经过同一个地方。

回环检测的基本思路：

• 基于里程计的几何关系（Odometry based）：当我们发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系
• 基于外观（Appearance based）：仅根据两张图像的相似性确定回环检测关系

直接用两张图像相减的结果很差：因为像素灰度严重受环境光照和相机曝光的影响，并且相机视角发生少量变化时，即使每个物体的光度不变，它们的像素也会在图像中发生位移，造成一个很大的差异值。所以引出了另一种检测图像相似度的方式：词袋模型。

词袋，也就是Bag-of-Words（BoW），目的是用“图像上有哪几种特征”来描述一个图像。

词袋使用例子：对于一张包含一个人、一辆车的图片和一张包含两个人、一只狗的图片，度量相似性的过程包括

1. 确定“人、车、狗”等概念——对应于BoW中的“单词”（Word），许多单词放在一起，组成了“字典”（Dictionary）。
2. 确定一张图像中，出现了哪些在字典中定义的概念——我们用单词出现的情况（或直方图）描述整张图像。这就把一个图像转换成了一个向量的描述。
3. 比较上一步中的描述的相似程度。

也就是说，通过字典和单词，只需一个向量就可以描述整张图像了。该向量描述的是“图像是否含有某类特征”的信息，比单纯的灰度值更加稳定。。又因为描述向量说的是“是否出现”，而不管它们“在哪儿出现”，所以与物体的空间位置和排列顺序无关，因此在相机发生少量运动时，只要物体仍在视野中出现，就仍然保证描述向量不发生变化。

字典生成问题类似于一个聚类问题，经典算法有K-Means。进一步的优化有k叉树，类似K-Means。

相似度计算的做法： TF-IDF（搜索引擎里用的那个）

在回环检测中的一些优化：

• 对于不同的环境避免绝对的相似性阈值：如果当前帧与之前某关键帧的相似度，超过当前帧与上一个关键帧相似度的 3 倍，就认为可能存在回环。
• 关键帧最好稀疏一些：会把“相近”的回环聚成一类，使算法不要反复地检测同一类的回环。
• 词袋检测之后的验证：（实现方式见仁见智）
  • 设立回环的缓存机制，认为单次检测到的回环并不足以构成良好的约束，而在一段时间中一直检测到的回环，才认为是正确的回环。
  • 是对回环检测到的两个帧进行特征匹配，估计相机的运动。然后，再把运动放到之前的PoseGraph 中，检查与之前的估计是否有很大的出入。

第十三讲 建图

对于建图，不同的应用有着不同的需求。但是对于地图的用处，可以归纳成：

1. 定位：确定机器人位置
2. 导航：机器人能在地图中进行路径规划，知道地图哪些地方不可通过，哪些地方可以通过（需要稠密的地图）
3. 避障：局部的、动态的障碍物的处理（需要稠密的地图）
4. 重建：地图展示（需要稠密的地图）
5. 交互：命令“取桌子上的报纸”（需要语义地图）

稠密地图是相对于稀疏地图而言的——稀疏地图只建模感兴趣的部分，也就是前面说了很久的特征点（路标点）。而稠密地图是指建模所有看到过的部分。

稠密地图的重建需要知道每一个像素点（或大部分像素点）的距离，那么大致上有以下几种解决方案：

1. 使用单目相机，利用移动相机之后进行三角化，测量像素的距离。
2. 使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。
3. 使用 RGB-D 相机直接获得像素距离。

前两种方式称为立体视觉（StereoVision），其中移动单目的又称为移动视角的立体视觉（Moving View Stereo）。

如何确定第一张图的某像素出现在其他图里的位置：极线搜索和块匹配技术

像素p1的深度会在某条线段上，从另一个视角看来，这条线段也是图像平面上的一条线（极线），当们知道两个相机间的运动时，这条极线也是能够确定的，需要确定的是极线上的那个点是刚才看到的p1点。

由于比较单个像素的亮度值不一定稳定可靠，所以会在p1周围取一个小块，然后在极线上也取很多同样大小的小块进行比较，就可以一定程度上提高区分性。这就是所谓的块匹配。（需要假设在不同图像间整个小块的灰度值不变，这种比较才有意义）

计算小块和小块之间的差异有许多不同的计算方法：

1. SAD(Sum of Absolute Difference)。顾名思义，即取两个小块的差的绝对值之和：

2. SSD（Sum of Squared Distance， 平方和）：

3. NCC(Normalized Cross Correlation)（归一化互相关），相关性接近0表示两个图像不相似，而接近1才表示相似：

在搜索距离较长的情况下，通常会得到一个非凸函数：这个分布存在着许多峰值，然而真实的对应点必定只有一个。这种情况下通常使用概率分布来描述深度值，对深度的分布假设存在着若干种不同的做法，如假设为高斯混合分布。

导航中比较常用的，本身有较好的压缩性能的地图形式：八叉树地图

八叉树某一格都占满了就没有必要展开，非常节省空间。

因为位姿估计通常带有误差，这种直接拼接往往不够准确，同一个物体的点云无法完美叠加，这时地图上会出现这个物体的两个重影，这种现象有时被称为”鬼影“。

实时的三维重建方法有很多，书中重点介绍了经典的TSDF地图。

TSDF（Truncated Signed Distance Function，截断符号距离函数）：

与八叉树相似，TSDF地图也是一种网格式（或直观地说，方块式）的地图。

• 先选定要建模的三维空间，比如3×3×3m那么大，按照一定分辨率，将这个空间分成许多小块，存储每个小块内部的信息。不同的是，TSDF 地图整个儿存储在 GPU 显存当中而不是内存中。利用GPU的并行特性可以并行地对对每个体素进行计算和更新，而不像CPU遍历内存区域那样，不得不串行地进行。
• 每个TSDF体素内，存储了该小块与最近的物体表面的距离。如果小块在最近物体表面的前方，它就有一个正的值；反之，如果该小块位于表面之后，那么这个值就为负。由 于物体表面通常是很薄的一层，所以就把值太大的和太小的都取成1和-1，这就得到了截断之后距离，也就是所谓的TSDF。那么按照定义，TSDF为0的地方就是表面本身—— 或者，由于数值误差的存在，TSDF由负号变成正号的地方就是表面本身。

第十四讲 现在与未来

未来SLAM发展方向：

• 往轻量级、小型化方向发展，让 SLAM 能够在嵌入式或手机等小型设备上良好的运行，然后考虑以它为底层功能的应用。
• 利用高性能计算设备，实现精密的三维重建、场景理解等功能。目的是完美地重建场景，而对于计算资源和设备的便携性则没有多大限制。
• 视觉+惯导SLAM
• 语义SLAM