转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12634631.html
写在前面
最近在搞本科毕设，关于基于深度学习的 SLAM 回环检测方法。期间，为了锻炼自己的工程实现能力，(也为了增添毕设的工作量，显得不那么水），我自己写了一个简单的双目 SLAM 系统，其中嵌入了一种基于深度学习的轻量级回环检测模块 (https://github.com/rpng/calc)，目前这种方法是我找到的最轻量级且效果不错的回环检测方法，当然目前只是暂时使用这种方法，如果我能在毕设期间优化出更好的方法的话，我就把它换掉（大概是不可能的了）。
选择双目是因为双目比较简单（不像单目需要很多工作去初始化、估计深度、减小尺度误差等），整个系统结构比较清晰和简单，没有做很多细节上的优化，所以最终效果没有特别好。但我个人认为，这样的结构比较适合 SLAM 的初学者去熟悉一个完整的 SLAM 系统。毕竟如果直接研究 ORB-SLAM2 这种一万多行代码、其中嵌入了各种各样 trick 的复杂系统，对于 SLAM 初学者是很不友好的（心酸泪）。这也是我厚着脸皮开源这个弱鸡 SLAM 系统的原因。
本文会对这个系统的架构以及配置安装方法进行简单的介绍。本项目的 GitHub 地址：(https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing)。之后若有更新还请参考 GitHub 中的 README。
相关依赖
参考
高翔《视觉 SLAM 十四讲：第二版》第十三章双目视觉里程计
(https://github.com/gaoxiang12/slambook2/tree/master/ch13)。本系统借鉴了该视觉里程计的基础架构，前端和后端基本使用了相同的方法。
ORB-SLAM2
(https://github.com/raulmur/ORB_SLAM2)。本系统使用了修改后的 ORB-SLAM2 中提取 ORB 特征部分 (ORBextractor) 的部分代码。
Lightweight Unsupervised Deep Loop Closure
(https://github.com/rpng/calc)。本系统使用了该方法作为回环检测的方法，并将修改后的代码嵌入了本系统。
库
Caffe
使用 CPU 版本即可。其下载和安装可参考我总结的教程：(https://github.com/Mingrui-Yu/Tutorials/blob/master/Ubuntu/caffe.md)。其用于回环检测。
DeepLCD & ORB-SLAM2
我们使用了魔改后的 DeepLCD 库和魔改后的 ORB-SLAM2 中的一点点代码。这些部分已经包括在项目中，无需另外安装。
其他
这里是一些 SLAM 常规使用内容，具体内容课参照 GitHub 项目中的 README。
- C++11
- Boost filesystem
- Google Logging Library (glog)
- OpenCV
- Eigen
- Sophus
- g2o
- Pangolin
可能还有漏掉的（emmm），有问题的话欢迎大家 issue。
配置安装
本项目的 GitHub 地址：(https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing)。
完成上述的配置安装。
clone 本项目：
git clone https://github.com/Mingrui-Yu/A-Simple-Stereo-SLAM-System-with-Deep-Loop-Closing.git
build 本项目：
mkdir build
cd build
cmake ..
make
之后，/bin 文件夹中会有运行程序的可执行文件；/lib 文件夹中会有 libmyslam.so。
例程运行
目前因为时间原因（或者仅仅是懒），只写了双目 KITTI 的主程序，其位于 /app/run_kitti_stereo。
首先下载 KITTI 数据集, 国内可通过泡泡机器人汇总的百度网盘链接下载。
运行方式:
./bin/run_kitti_stereo config/stereo/gray/KITTI00-02.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/00
其中 KITTI00-02.yaml 是相应参数的配置文件（包括相机参数等），其风格参考 ORB-SLAM2 所提供的配置文件。
另外，在 yaml 配置文件中，有几个参数可以控制系统运行时的显示效果：
- Camera.fps: 控制系统运行的帧率（大体，不会很准）
- LoopClosing.bShowResult: 是否显示回环检测的匹配结果和重投影结果
- Viewer.bShow: 系统运行时，是否实时显示视频帧及地图
轨迹结果展示（挑了效果最好的一次hhh）：
系统架构 & 原理简介
下面简单介绍一下整个系统的架构和原理。
参考 ORB-SLAM2，整个系统分三个线程：前端，后端，回环闭合。
前端
前端通过特征点 + 光流法进行追踪。初始化时，对第一帧左图提取 ORB feature，使用的是 ORB-SLAM2 中的分格及八叉树提取方法，因为 OpenCV 中的 ORB 特征提取有严重的分布不均情况。注意，与普通特征点法（例如 ORB-SLAM2）不同的是，这里的 ORB feature 提取没有使用尺度金字塔，仅仅是在原图像上进行提取。同时，也不需要计算 feature 的描述子。这二者是因为，本系统中 feature 的跟踪不通过特征匹配，而是基于光流法。
在左图中提取 ORB feature 后，通过光流法，找到这些 feature 在同帧右图中的位置。之后，根据一对 feature 在左右目中的位置关系，进行三角测量，得到其对应的 MapPoint 的深度，并在地图中创建 MapPoint。同是，根据当前帧创建 KeyFrame，送入到后端中。
之后，每一帧的跟踪中，会先根据恒速模型，得到一个当前帧位姿的估计初值。之后，通过光流法，找到上一帧的特 feature 征点在当前帧的位置，得到当前帧的 feature 。再根据当前帧的 feature 及 feature 对应的 MapPoints 的位置，通过 g2o 优化得到当前帧的位姿。此处的优化仅优化当前帧位姿，当前帧观测到的 MapPoints 仅以约束形式参与优化。如果某个 MapPoints 在刚创建不久（两帧以内）就成为 Outlier 了，那么该 MapPoint 会被从地图中删除。
因此，并不需要在每一帧的跟踪时都提取 feature ，每次仅需通过光流法将上一帧的 feature 关联到当前帧。但每一次光流跟踪都会有一定 feature 丢失，只有当前帧的 feature 少于一个阈值时，会再进行 ORB feature 点的提取，此时，会将之前剩余的 feature 作为 mask，附近不提取新的 feature 。再通过三角测量得到新的 MapPoint 并插入地图。同时，此时会创建 KeyFrame，送入后端。
如果某次追踪到的特征点特别特别少，则判定为 LOST。目前，系统 LOST 后的 Relocalzation 还没有完成，等我苟过毕设（毕竟写这玩意跟我毕设主题没太大关系）。
后端
后端会对一个 active map 进行维护，同时在 active map 中进行优化。前端送入的 KeyFrame，会在后端进行一定的处理，并插入地图。而 active map 其实是一个滑动窗口，其中含有一定数量的近期的 KFs 及 它们观测到的 MapPoints，作为 active KFs 和 active MapPoints。当一个新的 KF 送入后端时，后端会将它同时插入 map 和 active KFs，并将其观测到的 MapPoints 插入 active MapPoints。如果此时 active map 中的 KF 数目超过了限制，会从中删除一个根据条件选择的 KF。
之后，会在 active map 中进行一次优化，优化包括 active KFs 和 active MapPoints。其中，如果某个 active MapPoints 第一次被观测到时的 KF 不在 active map 中，那么它会被固定，仅作为约束参与优化。优化后被视为 Outlier 的 MapPoints 会被从地图中删除。
另外，新的 KF 会被送入回环闭合线程。
回环闭合
这一部分相对比较复杂。回环闭合线程会维护一个 KeyFrame Database，用于回环检测。
对于送入回环闭合线程的新的 KF，首先，会对它进行预处理。第一步，对其 feature 进行扩充和处理。上文说过，前端提取的 feature 不是在尺度金字塔上提取的，而在回环闭合过程中，因为需要特征匹配，所以需要尺度金字塔来克服尺度变化带来的影响。在这一步中，会基于尺度金字塔，将每个 feature 扩充成 8（金字塔层数）个 keypoints，即每层的同一位置都视为一个 keypoint。之后，会对 keyponts 进行筛选：去除其中不能视为 FAST 角点的（响应低于阈值），以及超出边界的（因为需要计算描述子、角度等，keypoints 的位置需要离图像边界有一定距离）。根据筛选后的 keypoints，计算该 KF 的 ORB 描述子。以上内容是用于特征匹配的。另外，为了之后的回环检测，预处理中 DeepLCD 中的网络会对整幅 KF 提取一个描述向量。新的 KF 经过上述处理后，会带着计算好的描述子和描述向量，被存储进 KeyFrame Database 中。
回环检测：对于每一个要查询是否存在回环帧（Loop KF）的 Current KF，系统会将 Current KF 与 KeyFrame Database 中所有 KF 的描述向量进行比较，求余弦相似度作为相似分数。因为查询速度相当快，所以此处简单的使用了线性查询。 找到相似分数最高的 Candidate KF，如果该相似分数高于一个阈值，则认为该 Candidate KF 可以送入下一环节：特征匹配。
特征匹配：根据 Candidate KF 和 Current KF 的 ORB 描述子进行特征匹配。因为存在 keypoints 归属于相同的 feature，所以根据 keypoints 匹配可能出现重复的 feature 匹配。所以这里会从 keypoints 之间的匹配再上升至 feature 之间的匹配，从而去除重复匹配。同时，根据匹配的距离，对匹配对进行筛选，从中选出距离小的有效的匹配对。如果有效匹配对的数量达到一定阈值，则可以送入下一步。
计算当前帧正确位姿：根据 Candidate KF 的 feature 对应的 MapPoints 与 Current KF 的 feature 之间的匹配，首先进行 PnP 求解，这里使用了 OpenCV 的 solvePnPRansac()。之后，会再进行一次 g2o 优化，同样，只优化 Current KF 位姿，MapPoints 仅作为约束参与优化。如果优化的 inlier 数目超过一定阈值，则最终正式将 Candidate KF 确认为 Loop KF，并送入接下来的回环校正模块。
回环校正：回环矫正分两个部分。首先是回环融合，有了 Current KF 的正确位姿，就可以对其位姿进行调整。同时，active map 中的 active KFs 会根据它们之前与 Current KF 的相对位姿，同步进行调整（固定相对位姿），同理，active MapPoints 也会根据它们与 active KF 的相对位置，进行相应的调整（固定相对位置）。active map 调整完毕后，会进行一次位姿图优化，来对之前 KFs 的位姿进行全局调整。位姿图优化的边，一种是前端跟踪时，KF 与相邻 KF 之间的相对位姿，另一种就是回环边。对 KF 的位姿进行优化后，相应的会将所有的 MapPoints 根据与观测 KF 之间的相对位置进行位置校正。
作为一个 SLAM 的初学者的初级工作，这个项目中可能会存在很多问题或错误。如果大家发现了任何问题，欢迎来 issue 一下。非常感谢大家的指正和建议！