ORB-SLAM2新拓展，应用于自动驾驶车辆视觉定位的连续地图保存
前记：本项目为自动驾驶汽车或电动汽车的定位提供一个ROS拓展包，该定位功能基于原始ORB-SLAM2框架。在这次拓展中，ORB特征的二进制地图可以被保存在磁盘上，作为下一步定位模式下输出的参考轨迹。在汽车高速行驶时，基于地图的定位方法比纯视觉SLAM方法定位效果更好，提高了系统定位精度的同时也节约了计算资源。整个系统的模式是先建图后定位，系统工作流程图如图1所示。
图1. 系统工作流程图，SLAM建图（右边），定位（左边）
初始化，调用地图，判断是否成功调用，如果没有成功就启动SLAM建图模式（右边）先建图，然后将地图保存；如果地图调用成功，则启动定位模式（左边），运行左边定位模式的时候右边的SLAM建图模式是关闭。同理右边在建图过程中，左边定位模式也是关闭。此处附上源代码链接。
一、安装依赖项
系统在ubuntu16.04(原始版本用的ubuntu16.04, 这里采用的是ubuntu18.04)环境下被测试，和ORB-SLAM2依赖的库一样，系统同时依赖于如下安装库：
1.1 Opencv
至少需要2.4.3版本的Opencv，测试过程中采用的是Opencv2.4.11或者Opencv 3.2版本。
1.2 Eigen
至少需要3.1.0版本以上
1.3 ROS
这里采用 ROS melodic版本。
1.4 Boost
地图保存/加载扩展需要用到的序列化库 libboost_serialization 通过如下命令下载：
sudo apt install libboost-serialization-dev
1.5 安装 octomap ROS 包
sudo apt-get install ros-melodic-octomap ros-melodic-octomap-mapping ros-melodic-octomap-msgs ros-melodic-octomap-ros ros-melodic-octomap-rviz-plugins ros-melodic-octomap-server
二、编译
2.1 从仓库中将源码克隆到ROS工作空间中
cd catkin_ws/src
git clone https://gitlab.lrz.de/perception/orbslam-map-saving-extension
如果之前已经新建了工作空间，这里第一步可以省略。第二行命令是下载原代码，如果已经下载了.zip压缩文件，直接解压即可。
2.2 编译安装包
cd …
catkin_make
2.3 source工作空间
source devel/setup.bash
2.4 提取在orb_slam2_lib/Vocabulary 文件夹中提取ORBVoc.txt 文件
cd src/orbslam-map-saving-extension/orb_slam2_lib/Vocabulary
tar -xf ORBvoc.txt.tar.gz
2.5 为了快速的加载，将辞典文件ORBVoc从.txt转化为.bin二进制文件
./bin_vocabulary
三、运行示例
我们选用KITTI数据集
步骤1. 下载和转换KITTI数据集格式
步骤2. 保存KITTI数据集的ORB特征地图
步骤3. 加载ORB地图，并以更高的数据集播放速度执行定位模式
步骤4.可视化见图和定位结果
3.1 准备KITTI数据集
pip install kitti2bag
下载KITTI RAW sequence 中的synced+rectified and calibration 数据。我们选择2011_09_30_ drive_0027序列，下载命令如下：
$ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_drive_0027/2011_09_30_drive_0027_sync.zip
$ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_calib.zip
$ unzip 2011_09_30_drive_0027_sync.zip
$ unzip 2011_09_30_calib.zip
(文件比较大，4G大小，这里我们事先下载好并解压)
将原始数据转化为ROS 的.bag格式文件
kitti2bag -t 2011_09_30 -r 0027 raw_synced
注意这里解压好的.bag格式数据集放在$(env HOME)/datasets/KITTI/kitti_2011_09_30_drive_0027_ synced.bag路径下。
3.2 新建和保存ORB特征地图
在启动SLAM系统之前，确保获得相应的ROS工作空间。执行如下命令：
source devel/setup.bash
SLAM系统然后通过如下命令打开：
roslaunch orb_slam2_ros orb_slam_kitti_from_bag.launch
打开启动文件后，加载ORB词汇表。按空格键开始播放数据集包和建图。当.bag格式的数据包播放完毕之后，按Ctrl-C关闭系统，执行地图保存功能。其中ORB特征地图文件map.bin被保存到当前的工作空间中，catkin_ws/devel/lib/orb_slam2_ros/. 而SLAM轨迹数据文件FrameTrajectory_TUM_Format.txt, 被保存到同样的文件夹中。
将轨迹文件重命名为slam_FrameTrajectory_TUM_Format.txt, 因此在连续的执行定位模式时它不会被覆盖。
可操作步骤：更改启动文件中数据集路径以匹配当前系统上的.bag数据集文件位置。启动文件中的默认数据集路径为：$(env HOME)/datasets/KITTI/kitti_2011_09_30_drive_0027_ synced.bag，其中默认的ORB-SLAM设置文件被配置与KITTI示例文件包文件一起工作。从设置文件编号到进一步的KITTI数据集序列号都可以在Readme文件中找到。
3.3 在ORB特征地图中定位
在ORB设置文件的底部可以找到地图加载和保存设置，在orbslam-map-saving-extension/ orb_slam2_ros/settings/KITTI04-12.yaml文件中设置ORB相关参数。通过设置Map.load_ map: 1 来使系统初始化时加载先前创建的地图。另外，禁用地图保存：Map.save_map: 0 .
在启动ROS启动文件时，我设置数据集.bag文件的播放速度为原来的两倍2x, t通过如下命令来实现：-r 1 换成-r 2 来加倍数据集的播放速度。
现在，重新运行启动文件来开启定位模式：
roslaunch orb_slam2_ros orb_slam_kitti_from_bag.launch
命令行输出确认成功加载了地图文件map.bin. 按下空格键开始播放.bag数据集文件。
当.bag数据集文件播放完毕，按Ctrl-C键来关闭系统，现在仅仅是轨迹文件被保存。
重新命名轨迹文件为：
Localization_FrameTrajectory_TUM_Format.txt, 因此在连续的定位运行中它不会被覆盖
四、可视化
为了可视化定位结果，我们增加了一些必须的安装包：
(evo评价工具)
pip install matplotlib
pip install evo
4.1 轨迹可视化
为了可视化SLAM以及定位模块的轨迹，主文件夹中提供了一个基本的绘图脚本，例如，只需简单运行如下命令即可：
python example_trajectory_plots.py
输出的结果如图2所示：
可以看到，即使在更高的速度下（2x），定位在视觉上也与轨迹也是相匹配的，注：如果以两倍的速度运行SLAM模块，由于特征匹配失败，建图过程将会相应的失败。
4.2 特征模型可视化
在SLAM或者定位模式下，可以看见可视化ORB特征的匹配，因此开始rqt_image_view以及倾听 /orb_slam2/frame 话题，定位模式的示例框架输出显示如图3所示：
图3 定位模式可视化图
五、 附加的信息
5.1 KITTI里程计序列建图设置文件
下表将设置文件号映射到用于提取视觉里程计/ SLAM数据集的每个序列的名称、开始帧和结束帧:
Nr. Sequence name Start End
00: 2011_10_03_drive_0027 000000 004540
01: 2011_10_03_drive_0042 000000 001100
02: 2011_10_03_drive_0034 000000 004660
03: 2011_09_26_drive_0067 000000 000800
04: 2011_09_30_drive_0016 000000 000270
05: 2011_09_30_drive_0018 000000 002760
06: 2011_09_30_drive_0020 000000 001100
07: 2011_09_30_drive_0027 000000 001100
08: 2011_09_30_drive_0028 001100 005170
09: 2011_09_30_drive_0033 000000 001590
10: 2011_09_30_drive_0034 000000 001200
基于以上这个表，例如序列2011_09_30_drive_0027, 设置文件KITTI04_12.yaml是正确的。
这里我们在定位模式下，调整了数据集不同的播放速度，打印结果如下：