一、以小萝卜机器人为例，引入SLAM概念
1、机器人功能：实现内（自身定位）外（探索环境并建模）两项功能
（1）执行器：手、脚
（2）传感器：天线、摄像机
（3）大脑：主机
2、定位传感器：
（1）本体传感器：安装在本机，如轮式编码器、相机。通常不能直接得到定位信息，需要将得到的间接物理量，通过计算转换为定位信息。但它对环境没有任何要求，适用于未知环境
（2）环境传感器：安装与环境中，如导轨、二维码标识。简单直接，但对应用环境有要求，通常只能用于已知环境
SLAM中，主要讨论利用相机解决定位和建图问题。
3、相机，安工作方式可以分为三类
（1）单目相机（Monocular）：成本低，但图片丢掉了一个维度的信息。我们可以通过视差（相机移动时，近处物体的相对相机移动块，远处物体则慢）来获取物体的相对远近，但无法得知物体的绝对距离，这就是单目相机的尺度不确定性。
（2）双目相机（Stereo）：能够消除尺度不确定性。根据两个眼睛看到的图像视差，以及双眼间的距离（基线），就可以计算出物体的距离。显然，能探测到的深度范围，与基线大小及分辨率相关。基线越大，探测范围越大，所以无人车上的双目通常比较大个。缺点是标定复杂，探测范围及探测精度受限于基线和分辨率。同时，由于计算比较耗资源，需要配备GPU和FPGA设备加速后，才能实时输出距离信息，所以配置也比较麻烦，计算量是一个主要瓶颈。
（3）深度相机（RGB-D）：不能能采集到颜色信息，还能直接读取像素与相机的距离，通常携带多个摄像头，工作原理相对复杂。通过红外结构光（Tof或Time-of-Flight）原理，主动向物体发射光，接受反射光，直接的获得像素的测量距离，因此解决了单目相机尺度不确定性和二维相机计算量的问题。但目前大多数RGB-D相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透明材料等诸多问题。在SLAM方面，仅用于室内。
由于单目相机能力有限，深度相机成本太高且发展不成熟，SLAM中的主流相机就是双目相机。
4、获取到图片之后，就要用来定位和地图构建。SLAM框架，就是要利用这一系列的图片，来实现定位和建图的目的。目前，SLAM框架基本定型，细节方面则还在不断的发展。
二、SLAM框架简介
1、整体：
（1）Simultaneous Localization And Mapping：译为同时定位与地图构建
（2）模块构成：
（3）前端：估算相邻图像间相机的运动，以及构建局部地图
（4）回环检测：检测是否到达先前经过的位置，如果是则把信息传递给后端
（5）后端：接受相机位姿信息和回环信息，优化统一后得到全局一致的轨迹和地图
（6）传感器数据：除了图像信息读取，还可能包括机器人的码盘、惯性传感器等信息的读取和同步
（7）建图：建立与任务要求对应的地图，比如扫地机器人可能只关注地面，而不管天花板和墙。
2、前端：通过相邻帧的图像来估算相机的运动，并恢复场景的空间结构，因此我们也称其为‘视觉里程计’。
（1）估计相邻两帧的相机运动（即机器人运动），以及空间结构
（2）讲所有相机运动串联起来，就是机器人的运动轨迹，从而解决了定位问题
（3）讲所有空间结构信息串联起来，就有了地图。
（4）由于每次估计都会有误差，所以前端得出的结果会有累积误差／漂移，它将导致我们无法建立一致的地图。因此我们需要回环检测和后端优化技术，回环检测负责把‘机器人重新经过某处’的事件监测出来，后端根据前端和回环检测给的信息校正整个轨迹的形状。
3、后端：说到底，就是尽量解决‘噪声’造成问题，并给出最终结果的不确定性（避免不了的误差影响）。
（1）尽量减少噪声带来误差
（2）根据噪声的大小，估算不确定性
（3）后端只关心数据本身，而不关心数据来自于什么传感器，所以在视觉SLAM中，前端与计算机视觉研究领域（如图像特征提取与匹配）更相关，后端泽主要是滤波和非线性优化算法。在历史上，SLAM技术曾不包括前端部分。这反映了SLAM的本质------对运动主体自身和周围环境空间的不确定性估计。
4、回环检测：
（1）原理：假设机器人经过一段时间的运动后回到了原点，但由于漂移，它的位置估计却没有到达原点。于是我们可以获取到二者的差值，并传递给后端。
（2）这里的关键就是，让机器人能够知道自己回到了原点。视觉SLAM中，通过计算图像数据的相似性，来实现回环检测功能。由于图像数据非常的丰富多变，这给回环的实现降低了不少难度。
5、建图
（1）后端给出一个结果后，根据应用需求的不同，我们可以建立不同的地图。简单的就直接一条线，复杂的就是三维地图。而现目前的汽车，地图都直接使用第三方的，我们只需要定位。总之，建图的灵活性很强，大体分为度量地图和拓扑地图。
（2）度量地图就是我们常见的地图，当然我们可以控制其疏密性（地图数据的详细程度）
（3）拓扑地图：只关注能否到达，而不管如何到达，如车站里看到的站点地图。
三、用数学语言描述SLAM过程
1、首先，我们取数据的时间是离散的（不同时间点，而不是连续时间段），因此我们得到的定位、图像等信息也都是离散的。
2、运动方程：Xk = f(Xk-1,Uk,Wk)。
（1）Xk：这一帧的估计位置
（2）f：函数表示，形式多样
（3）Xk-1：上一帧的估计位置
（4）Uk：传感器（如码盘）数据
（5）Wk：噪声
3、观测方程：Zk,j = h(Yj,Xk,Vk,j)
（1）Zk,j：机器人在Xk位置，观察到路标Yj，产生一个观测数据Zk,j
（2）Vk,j：噪声
4、解方程组，解决问题：
（1）问题描述：上面两个方程，组成一个方程组，描述了这样一个工况：当知道运动传感器读数U，和环境传感器读数Z时，如何求解定位问题（估计X）和建图问题（估计Y）。
（2）解方程：求解过程与两个方程的具体形式，以及噪声服从的概率分布有关。根据方程形式和噪声的概率分布，系统一般分为线性／非线性系统，高斯／非高斯系统。当然，线性高斯系统是最简单的系统，它的无偏最有估计可由卡尔曼滤波器给出。而对于非线性非高斯系统，我们一般有两种解决思路（扩展卡尔曼滤波器，非线性优化）。
（3）这本书的数学知识，主要就在建立这个方程组，以及求解他。SLAM的数学原理，也都集中于这个方程组。
四、熟悉开发环境
1、本想直接用OSX系统，但又想省去移植的麻烦，所以装了双系统。我装了16.04，不过不想每次都启动ubuntu，所以设置了默认启动系统：设置默认启动系统
2、HelloSlam：
（1）linux系统中，程序是否能执行，与其后缀名无关，而是看其是否具有可执行权限（有执行权限的文件，其颜色会有不同）。只要有了可执行权限，我们在终端输入程序名并回车，程序就会开始执行
（2）编辑HelloSlam.cpp文件
（3）执行g++ -o helloSlam HelloSlam.cpp ，得到可执行程序helloSlam
（4）执行./helloSlam，终端就能看到打印
3、使用cmake工具
（1）文件多起来的时候，使用g++的话，编译命令会越来越长，不能这样来。用cmake来编译，就好多了
（2）cmake的工作方式就是：在工程中编辑CMakeLists.txt文件，使用cmake命令，它会根据CMakeLists.txt文件生成一个makefile文件。然后执行make命令，它会根据makefile文件的内容编译整个工程。实际上，cmake过程处理了工程中文件间的关系，而make的原理还是调用g++命令来编译程序。所以，我们的工作从‘重复调用g++命令’转变到‘维护若干比较直观的CMakeLists.txt文件’。
4、使用cmake的标准步骤
（1）在根目录中创建中间目录build，用于存放编译过程中产生的中间文件
（2）在根目录创建CMakeLists.txt文件
（3）进入build目录，执行cmake ..，前面十多行信息是在检测编译器，后面会出现configuring done和Generating done，表面编译完成，最后还会告诉我们编译文件放在哪里（build files have been written to: .......），这里当然就是放在build目录中。里面当然是多了好多文件，需要自己慢慢熟悉，这里就先放着了。
（4）接着在build中执行make命令，就会得到可执行文件，也放在build中。
（5）前面步骤都执行成功，说明代码没问题，可以提交。当然，我们只想提交代码，而不想提交可执行文件和编译生成的中间文件，因此可以忽略掉build，这也是添加build目录的目的。
5、使用库：在c++中，只有含有main函数的文件，才可编译生成可执行文件，其它文件我们希望将其打包成一个整体，供其它程序作为工具调用，这个整体就是库。这里的重点是，使用cmake工具生成库，并调用它的功能。
（1）在根目录创建、编辑libHelloSlam.cpp文件
（2）更改CMakeLists.txt文件，在其中添加add_libraey(hello libHelloSlam.cpp)，它告诉cmake把libHelloSlam.cpp编译生成一个叫做hello的库
（3）像前面一样，执行cmake和make命令，然后就会在build目录中生成一个libhello.a文件，就是我们的库
（4）关于静态库与共享库的概念，自己去了解下
（5）为了让别人知道自己的库里面有哪些函数可调用，我们一般要创建一个头文件，里面声明方法，并添加注释。然后把头文件和库文件交给使用者。这里，我们只需要在根目录再添加并编辑一个libHelloSlam.h文件就行了。
（6）使用库：在根目录中添加并编辑一个useHello.cpp文件，在main函数中调用库中的方法。然后像编译HelloSlam.cpp文件步骤那样（这里多出的一步就是，要链接需要使用的库，添加代码：target_link_libraries(useHello hello)），将其编译为可执行文件。然后就走完流程了。
6、使用IDE---Kdevelop：使用IDE的好处就是，编译、运行等过程都省略成点按钮了，简单快捷。另外IDE一般还提供了跳转、补全、断点调试等方便的功能，极大方便了我们开发。
（1）安装配置
（2）导入工程：将build目录删除，然后用kdevelop导入CMakeLists.txt文件，最后点击build按钮，会发现又生成了跟之前一样的build目录
（3）运行、断点调试。总体来讲，使用IDE还是要方便得多。
