第十讲：后端 1
本节目标

理解后端的概念。
理解以 EKF 为代表的滤波器后端工作原理。
理解非线性优化的后端，明白稀疏性是如何被利用的。
使用 g2o 和 Ceres 实际操作后端优化。

知识点整理

本讲主要是针对长时间内最优轨迹和地图的优化，减小累积误差对长时间运动带来的影响。
1、批量的处理方式：考虑在很长一段时间内的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己。

2、渐进的处理方式：当前状态只由过去的时刻决定，甚至只由前一时刻决定。

3、当没有运动方程时的处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。此时非常类似于SfM（即通过一组图像来恢复运动和结构，但是SfM中允许使用完全无关的图像），SLAM中的图像有时间上的先后顺序。

4、由于每个方程都受噪声的影响，所以把位姿x和路标y看成服从某种概率分布的随机变量，会随着运动和观测数据的到来进行更新。

5、当将状态量和噪声项以高斯分布来模拟时，均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。

6、对状态进行估计，可以得到当前状态的估计=似然*先验，其中似然由观测方程给定，先验表示当前状态是基于过去所有的状态估计得到的。当前状态估计至少会受到前一时刻状态的影响，所以，对此引出两种不同的方法。

1）马尔科夫性：即认为当前时刻状态仅与前一时刻状态有关，与之前时刻的状态无关。此时就可以得到以扩展卡尔曼滤波为代表的滤波器方法

2）当前时刻与之前所有状态都有关：非线性优化方法

7、线性系统的最优无偏估计——卡尔曼滤波：做的工作是“如何把k-1时刻的状态分布推导至k时刻”，即只需要维护一个状态量，对它不断的进行迭代和更新即可。根据贝叶斯定律可以得到下述等式，前一项表示似然函数，后一项表示根据k-1时刻的状态构成的先验


将步骤分为预测和更新两个阶段。其中，预测阶段显示了如何从上一时刻的状态，根据输入信息来推断当前时刻的状态分布


推导过程中使用到的一个小技巧是根据高斯分布的特点，针对指数部分比较其一次项和二次项的系数。

然后注意到，此时的卡尔曼增益，使用方差的先验分布计算得到，然后再使用该增益即可得到方差的后验估计。

8、非线性系统和扩展卡尔曼滤波器：在某点附近考虑运动方程及观测方程的一阶展开，只保留一阶项，即可将非线性系统转化为线性系统。此时给出的是单次线性近似下的最大后验估计。

9、当想在某段时间内估计某个不确定量时，可以尝试用EKF。

10、EKF的局限：

假设了马尔科夫性，当出现回环时，滤波器将会难以处理

在推导过程中，只在固定点上做一次Taylor展开，当具有强烈非线性时就会存在加大的非线性误差。而非线性优化会在迭代中，状态估计发生改变之后，会重新对新的估计点进行Taylor展开

由于EKF将非线性系统转化为线性系统，所以需要存储状态量的均值和方差，并对它们进行维护和更新，当路标数也放进去时存储量会很大。

11、BA：从视觉重建中提炼出最优的3D模型和相机参数。从每一个特征点反射出来的几束光线，在对相机姿态和特征点空间位置做出最优调整之后，最后收束到相机光心的过程。它类似于求解只有观测方程的SLAM问题。目前，BA不仅具有很高的精度，也开始具有良好的实时性，这一点借助于BA的稀疏性

从世界坐标系中的一个点P出发，把相机的内外参数和畸变都考虑进来，最后投影成像坐标。在这里，路标即为这里的三维点P，观测数据则像是像素坐标。如下图所示


当时用非线性优化思想时，需要不断求解增量方程的增量△x


注意，此时的E和F均为整体目标函数对整体变量的导数，将是由很多个小分块构成的大块矩阵，可以使用高斯牛顿法或列文伯格-马夸尔特方法求解，其中使用具体哪一种方法的差别为H是取JTJ还是JTJ+λI。F表示整个代价函数在当前状态下对相机姿态的偏导数，E表示该函数对路标点位置的偏导数



13、矩阵H的稀疏性可以使用图优化方法来实现运算的加速。误差项eij仅描述在位姿i状态下看到路标j这一件事，对其余部分的变量的导数为0.这体现了该误差项与其他路标和轨迹无关的特性。下图表示了图优化结构与增量方程的稀疏性之间的关系


14、Schur消元：将H△x=g变成下述形式，其中B和C都是对角块矩阵，而E是非对角。


所以，需要使用高斯消元，去掉右上角的矩阵E。即可得


如此一来，根据第一行的值，只需要求解C的逆矩阵，可以得到△xc，然后再带入△xc，即可得△xp。这一步也称为边缘化，即做条件概率展开

15、物理意义：S矩阵的非对角线上的非零矩阵快，表示了该出对应的两个相机变量之间存在着共同观测的路标点，称为共视（co-visibility）

16、鲁棒核函数：核函数保证每条边的误差不会大的没边而掩盖掉其他的边，即将原先的二范数度量变成一个光滑的、增长没有那么快的函数，这可以使得整个优化结果更为稳健

视觉SLAM十四讲第二讲笔记
本书主要围绕下面的框架逐个部分讲解。
一、视觉SLAM的整体框架
经典视觉SLAM框架:


整个视觉SLAM流程包括:

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

1. 视觉里程计
视觉里程计关心相邻图像之间的相机运动， 最简单的情况当然是两张图像之间的运动关系。所以，为了定量地估计相机运动，必须在了解相机与空间点的几何关系之后进行。Visual Odometry能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。假定我们已有了一个视觉里程计，估计了两张图像间的相机运动。那么，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一 方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。然后只通过视觉里程计来估计轨迹，將不可避免出现累计漂移(Accumulating Drift)，这是由于视觉里程计（在最简单的情况下）只估计两个图像间运动造成的。


为了解决漂移问题，我们还需要两种技术： 后端优化和回环检测。 回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
2. 后端优化
后端优化主要指处理SLAM过程中噪声的问题。 后端优化要考虑的问题，就是如何从这些带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计(Maximum-a-Posteriori,MAP)。 在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
3. 回环检测
回环检测，又称闭环检测(Loop Closure Detection)，主要解决位置估计随时间漂移的问题。 为了实现回环检测，我们需要让机器人具有识别曾到达过的场景的能力。我们可以判断图像间的相似性，来完成回环检测。
4. 建图
建图(Mapping)是指构建地图的过程。 大体上讲，它们可以分为度量地图与拓扑地图两种。

度量地图(Metric Map)

度量地图强调精确地表示地图中物体的位置关系，通常我们用稀疏(Sparse)与稠密(Dense)对它们进行分类。我们选择一部分具有代表意义的东西，称之为路标(Landmark)，那么一张稀疏地图 就是由路标组成的地图，而不是路标的部分就可以忽略掉。相对的，稠密地图着重于建模所有看到的东西。对于定位来说，稀疏路标地图就足够了。而用于导航时，我们往往需要稠密的地图(否则撞上两个路标之间的墙怎么办？)。二维度量地图是许多个小格子(Grid)， 三维则是许多小方块(Voxel)。一般地，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。 当我们查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如 A*,D* 等等。
拓扑地图(Topological Map)

相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。 拓扑地图是一个图(Graph)，由节点和边组成，只考虑节点间的连通性，例如A，B 点是连通的，而不考虑如何从A点到达B点的过程。



二、SLAM问题的数学表述
首先，由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。这就把一段连续时间的运动变成了离散时刻$t = 1,...,K$当中发生的事情。在这些时刻，用$x$表示机器人自身的位置。于是各时刻的位置就记为$x_1,...,x_K$，它们构成了机器人的轨迹。地图方面，我们设地图是由许多个 路标(Landmark)组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有$N$个，用$y_1,...,y_N$表示它们。
在这样设定中，“机器人携带着传感器在环境中运动”，由如下两件事情描述:

什么是运动?

我们要考虑从$k − 1$时刻到$k$时刻，机器人的位置$x$是如何变化的。
什么是观测?

假设机器人在$k$时刻，于$x_k$处探测到了某一个路标$y_j$，我们要考虑这件事情是如何用数学语言来描述的。

所以，先来看运动。我们使用一个通用的、抽象的数学模型来表示:

$x_k=f(x_{k-1}, u_k, w_k)$

这里的$u_k$是运动传感器的读数(有时候也叫输入)，$w_k$是噪声。$f$是运动方程。
再来看一下观测。观测方程描述的是当机器人在$x_k$位置上 看到某个路标点$y_j$，产生了一个观测数据$z_{k,j}$。

$z_{k,j}=h(y_j,x_k,v_{k,j})$

这里的$v_{k,j}$是这次观测里的噪声。
针对不同的传感器，这两个方程有不同的参数化形式。 如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程:
$\begin{matrix}
        \left\{
         \begin{array}{c}
         x_k=f(x_{k-1}, u_k, w_k)  \\
         z_{k,j}=h(y_j,x_k,v_{k,j})  \\
         \end{array}
        \right.
        \end{matrix}$
这两个方程描述了最基本的SLAM问题：当我们知道运动测量的读数$u$，以及传感器的读数$z$时，如何求解定位问题(估计x)和建图问题(估计y)？这时，我们把SLAM问题建模成了一个状态估计问题:如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？
状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关。我们按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。 其中线性高斯系统(Linear Gaussian, LG系统)是最简单的，它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter, KF)给出。而在复杂的非线性非高斯系统(Non-Linear Non-Gaussian，NLNG系统)中我们会使用以扩展卡尔曼滤波器(Extended Kalman Filter,EKF)和非线性优化两大类方法去求解它。

Slam十四讲第1讲
重点总结
首先第1讲介绍了slam的基本概念、主要结构、所需掌握的基础知识。


SLAM 是 Simultaneous Localization and Mapping 的缩写，中文译作“同时定位与地图构建”，目的是解决“定位”和“地图构建”两个问题。


《视觉Slam十四讲》主要结构


需要掌握的基础知识 1）高等数学、线性代数、概率论  2）C++语言基础  3）Linux基础


课后习题


有线性方程 Ax = b，当我们知道 A,b，想要求解 x 时，如何求解？这对 A 和 b 需 要哪些条件？提示：从 A 的维度和秩角度来分析。

1）b等于0；n元齐次线性放程序A_(m×n) x=0有非零解的充分必要条件是习数矩阵的秩R(A)<n；

2）b不等于0；非齐次线性方程组Ax=b有解的充分必要条件是系数矩阵的秩等于增广矩阵的秩，即R(A)=R(B).

R(A)=R(B)=n <=> Ax=b有唯一解；

R(A)=R(B)<n <=> Ax=b有无穷多解；


高斯分布是什么？它的一维形式是什么样子？它的高维形式是什么样子？

高斯分布又叫正态分布，与高斯分布相关的一个重要定理是中心极限定理，它的内容为：任何分布的抽样分布当样本足够大时，其渐进分布都是高斯分布。

1） 一维高斯分布形式：  2）高维高斯分布：




你知道 C++ 的类吗？你知道 STL 吗？你使用过它们吗？

类定义是用关键字class开头，后面跟类的名臣，类的主体包含在一对花括号中。类定义后必须跟着一个分号或一个声明列表。关键字public确定了类成员的访问属性。


class car
{
   public :
      double length; //定义汽车长度
      double height; //定义汽车高度
 };

STL是C++标准库的一部分，不用单独安装。



参考：http://c.biancheng.net/stl/

4. 略

5. 你知道 C++11 标准吗？其中哪些新特性你之前听说过或使用过？有没有其他的标准？

1）新类型

2）统一的初始化

3）声明

4）智能指针

5）异常规范方面的修改，等等。

参考：

https://blog.csdn.net/qq_40421919/article/details/91049264

https://blog.csdn.net/feiyu2046/article/details/63256095

6. 略

7. Linux 的目录结构是什么样的？你知道哪些基本命令，比如 ls, cat 等等？


ls [-alrtAFR] [name...]

参数 :
-a 显示所有文件及目录 (ls内定将文件名或目录名称开头为"."的视为隐藏档，不会列出)

-l 除文件名称外，亦将文件型态、权限、拥有者、文件大小等资讯详细列出

-r 将文件以相反次序显示(原定依英文字母次序)

-t 将文件依建立时间之先后次序列出

-A 同 -a ，但不列出 “.” (目前目录) 及 “…” (父目录)

-F 在列出的文件名称后加一符号；例如可执行档则加 “*”, 目录则加 “/”

-R 若目录下有文件，则以下之文件亦皆依序列出
cat [-AbeEnstTuv] [--help] [--version] fileName

参数：
-n 或 --number 由 1 开始对所有输出的行数编号

　　-b 或 --number-nonblank 和 -n 相似，只不过对于空白行不编号

　　-s 或 --squeeze-blank 当遇到有连续两行以上的空白行，就代换为一行的空白行

　　-v 或 --show-nonprinting
参考：

https://www.cnblogs.com/weigege2015/p/9278828.html

https://www.cnblogs.com/weigege2015/p/9278828.html

8. 如何在 Ubuntu 中安装软件（不打开软件中心的情况下）？这些软件被安装在什么地方？当我只知道模糊的软件名称（比如我想要装一个 eigen 名称的库），我应该如何安装它？

sudo apt-get inatall 软件名

一般安装在/usr/local目录下。部分软件放在/opt下，则是一个软件统一在一个文件夹下。/opt目录专门是用来给第三方软件放置文件的，比如一些压缩包解压的软件都放在这里。

9. * 花一个小时学习一下 Vim，因为你迟早会用它。你可以在终端中输入 vimtutor 阅读一遍所有内容。我们不需要你非常熟练地操作它，只要在学习本书的过程中使用它 键入代码即可。不要在它的插件上浪费时间，不要想着把 vim 用成 IDE，我们只用它做文本编辑的工作。

参考：https://www.jianshu.com/p/4e59c605183a

@《视觉SLAM十四讲》知识点与习题
《视觉SLAM十四讲》第二讲知识点整理+习题
正在学习SLAM相关知识，将一些关键点及时记录下来。
知识点整理
本讲主要是介绍了SLAM，主要是视觉SLAM方面的基础知识，对一些术语进行了大体的解释，并且将整本书的内容的逻辑梳理了一遍，手把手上手视觉SLAM。

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

定位 ，“我在什么地方”，即自身状态
建图 ，“周围环境是怎么样”
传感器，分为携带于机器人本体和安装于环境中两种情况。但是后者约束了外部环境，前者只能通过测量到的间接物理量来计算位置数据。后者更适用于未知环境
相机，单目相机、双目相机、深度相机三大类。以一定速率拍摄周围的环境，形成一个连续的视频流。

单目相机：结构特别简单，成本特别低，单目SLAM非常受研究者关注。

但是在投影的过程中，丢失了深度信息，然而这个距离（物体与相机）很关键。因此，从单目SLAM中恢复三维结构，需要改变相机的视角，即移动相机来估计相机的运动，才能计算深度。

在相机移动过程中，不同远近的物体在图像上的运动会形成视差，有助于定量的判断

单目SLAM估记得轨迹和地图将与实际的轨迹和地图相差一个因子，即所谓的尺度scale。由于单目SLAM无法仅凭图像得到这个真实尺度，所以又称为尺度不确定性
双目相机和深度相机：测量物体与我们之间的距离，消除尺度不确定性。

双目相机：通过基线（两个相机之间的距离）来估计每个像素的空间位置。双目相机需要大量的计算才能估计每一个像素点的深度。测量的深度范围与基线相关。后者越大，测量到的就越远。

但是，双目相机的配置与标定均较为复杂，深度量程和精度受双目的基线与分辨率所限，且视差的计算非常消耗计算资源。室内室外均可

深度相机：通过物理测量手段，通过红外或TOF来获得距离，相较于双目相机节省了大量的计算。

但是，深度相机存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题，主要用于室内。


经典视觉SLAM框架，传感器信息读取、视觉里程计（VO）、后端优化（Optimization）、回环检测（Loop Closing）、建图（Mapping）。



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

传感器信息读取：视觉SLAM：相机图像信息的读取和预处理。在机器人中，还有码盘、惯性传感器等信息的读取和同步
VO： 估计相邻图像间相机的运动，以及局部地图的样子。VO只计算相邻时刻的运动，而和再往前的过去的信息没有关联。将相邻时刻的运动”串“起来，就构成了运动轨迹，就进行了定位。且根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。

但是，会出现累计漂移。即误差会逐帧累加。
Optimization： 接受不同时刻VO测量的相机位姿，以及Loop Closing的信息，对他们进行优化，得到全局一致的轨迹和地图，对整个轨迹的形状进行校正。

主要处理噪声的问题。需从带噪声的数据中估计整个系统的状态，以及这个状态（轨迹、地图）估计的不确定性。将不确定性表达出来以后，采用filtering或非线性优化，估计状态的均值和不确定性（方差）
Loop Closing: 判断机器人是否到达过先前的为止。若检测到回环，将信息提供给Optimization进行处理。实质是一种计算图像数据相似性的算法
Mapping： 根据估记的轨迹，建立于任务要求对应的地图。根据需求，对环境进行描述。

度量地图：精确地表示地图中物体的位置关系，用稀疏与稠密对齐分类。稀疏：选择具有代表意义的东西——路标，适用于定位。	稠密：建模所有看到的东西，适用于导航。但是稠密地图存储空间大，且大多数时候很多细节部分是无用的。

拓扑地图：强调地图元素之间的关系。由节点和边组成，只考虑节点之间的连通性。但是不擅长表达具有复杂结构的地图。如何对地图进行分割形成节点与边，又如何使用拓扑地图进行导航进行路径规划，仍是有待研究的问题


观测：在k时刻于xk处探测到了某一个路标yj，产生了一个观测数据zk,j。

运动：从k-1时刻到k时刻，位置x的变化。需要测量自身运动的传感器。

当知道运动测量的读数u，以及传感器的读数z，如何求解定位问题（估计x）和建图问题（估计y）。因此，SLAM问题转换成为了一个状态估计问题
状态估计问题：根据噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

线性高斯系统的无偏的最优估计可以又卡尔曼滤波器KF给出
复杂的非线性非高斯系统，使用以扩展卡尔曼滤波器和非线性优化两大类方法求解
最早的实时视觉SLAM系统是基于EKF开发的，但是EKF的缺陷是线性化误差和噪声高斯分布假设。
目前，主流视觉SLAM使用以图优化为代表的优化技术进行状态估计



习题

阅读文献1, 2。心得会在另一个博客中记录
阅读综述3,4,5,6,7
g++命令参数

g++ Test.cpp：功能：生成默认为a.exe的文件，这个过程包含了编译和链接

g++ -o Test.exe Test.cpp。-o命令是输出的意思，这样就输出了Test.exe。使用-o可以指定目标名称	8
注意需要首先使用+按钮添加添加二进制文件，修改名字，且将环境选择为default。Debug界面里保持默认配置即可。接下来设置断点，点击Default按钮。会看到界面有些许的变化，上方出现Stop All和Stop按钮。在左侧打开Variables一栏，即可以查看变量的值。



如果将CMakeLists.txt中的target_link_libraries(useHello hello_shared)注释掉，在Build时会报错，提示”Linking CXX executable useHello … recipe for target ‘useHello’ failed … undefined reference to ‘printHello()’"这句话就表示，此时虽然将库文件包含进去了，但是没有给头文件，无法知道该如何去调用该库文件中的函数，从而无法知道printHello()函数应该如何使用
看《cmake实践》。把一些用到的点，书上没有解释的整理如下

cmake . ：命令后面的点号，代表本目录

中间文件有：CMakefiles, CMakeCache.txt, cmake_install.cmake等文件，以及Makefile

CMakeLists.txt: cmake的构建定义文件，文件名大小写相关

若工程中有多个目录，需每个目录均存在CMakeLists.txt

PROJECT指令定义工程名称，同时也隐式的定义了两个cmake变量，projectName_BINARY_DIR和projectName_SOURCE_DIR

SET指令可以显式的定义变量