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是正确的。

这里我们在定位模式下，调整了数据集不同的播放速度，打印结果如下：