ORB_SLAM2采用ROS Bag获取数据
前言
之前写了采用相机实时获取数据进行SLAM导航的相关内容，此处将采用ROS Bag获取数据进行SLAM建图的学习。
一、ROS bag数据
1.ROS bag简介
ROSBag 既可以指命令行中数据包相关命令，也可以指 c++/python 的 rosbag 库。它主要用于记录、回放、分析 rostopic 中的数据，可以将指定 rostopic 中的数据记录到 .bag 后缀的数据包中，便于对其中的数据进行离线分析和处理。
对于订阅某个 topic 的节点来说，它无法区分这个 topic 中的数据到底是实时获取的数据还是从 rosbag 中回放的数据。这有助于我们基于离线数据快速重现曾经的实际场景，进行可重复、低成本的分析和调试。
2.获取数据集
在之前的博文中，曾经介绍过ORB_SLAM2框架提供了基于真实道路的KITTI数据集采用双目、单目摄像头进行SLAM；提供了基于室内的 TUM数据集采用用RGB-D、单目摄像头实现SLAM；也提供了基于四轴飞行器采集的EuRoC数据集采用双目、单目摄像头实现SLAM。
此处以TUM数据集进行演示如何获取rosbag数据。打开TUM数据集的深度相机数据集下载网址（https://vision.in.tum.de/data/datasets/rgbd-dataset/download）
此处，可以在页面的正中间查看到数据集的相关信息，包括长度、大小等。点击Download一栏中蓝色字体tgz可以下载对应的数据集。然而此处下载的文件格式为压缩包格式tgz而非我们所需的ROS bag文件类型。点击右侧more info可以查看该数据集的详细信息。
在此处，即可选择下载ROSbag类型的数据集文件
3.查看数据集信息
下载完成数据集后，我们可以在命令行查看其详细的信息。
# 查看数据集信息
rosbag info xxxx
其中xxxx修改为所需查看的ROS bag名称。
此处可以看出，该数据集发布的彩色信息和深度信息分别为 **/camera/depth/image ** 和 /camera/rgb/image_color 。
二、配置文件
1.配置ros文件
由于ORB_SLAM2默认规定使用深度相机进行建图时，相机或数据集端发布的彩色信息和深度信息必须为 /camera/rgb/image_raw和 /camera/depth_registered/image_raw ，与ROS bag数据集上发布的地址不同，故而需要进行相关文件的修改。
# 至ORB_SLAM2功能包目录下
roscd ORB_SLAM2
gedit src/ros_rgbd.cc
在该文件内找到如下图第66行至69行内容，将其注释修改。
修改为如下内容
//修改前：
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth_registered/image_raw", 1);
//修改后：
ros::NodeHandle nh("~");
// 添加param参数
std::string rgb_topic = nh.param<std::string>("rgb", "/camera/rgb/image_raw");
std::string depth_topic = nh.param<std::string>("depth", "/camera/depth_registered/image_raw");
//输出param参数内容
cout << "rgb: " << rgb_topic << endl;
cout << "depth: " << depth_topic << endl;
//订阅话题
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic, 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic, 1);
该段修改将官方写死的订阅话题修改为两个参数，从而使得可以在调用时定义参数内容。主要使用方式如下：
# 功能包 节点 磁带文件 内参文件 _rgb参数内容 _depth参数内容
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Astra.yaml _rgb:=/camera/rgb/image_raw _depth:=/camera/depth_registered/image_raw
其中 _rgb 参数默认订阅为 /camera/rgb/image_raw ，而 _depth参数则默认为 /camera/depth_registered/image_raw。
修改完成后，需要进行编译达到启动效果。
roscd ORB_SLAM2/
roscd ORB_SLAM2/
./build_ros.sh
编译成功截图如下：
2.配置内参文件
配置完成ros文件后，需要对内参文件进行修改。此处打开的相机内参文件为官方提供的TUM1.yaml。在下载ORB_SLAM2源码时，官方自带了如下三个深度相机的内参文件。
关于如何选择内参文件，需要根据你下载的ROS bag数据集进行判断，如我下载的文件名为rgbd_dataset_freiburg1_xyz.bag，文件名字所含数字为 1 ，故而使用的内参文件对应于TUM1。内参文件主要需要修改参数DepthMapFactor。
# 进入ORB_SLAM2源码目录下
roscd ORB_SLAM2
cd ../../..
# 修改文件
cd Examples/RGB-D
cp TUM1.yaml TUM1_rosbag.yaml
gedit TUM!_rosbag.yaml
打开文件后，修改第35行参数值为1.0。
修改前：
修改后：
三、启动节点
1.回放ROS bag数据集
和使用相机实时获取数据一样，采用ROS bag读取数据需要先回放数据。
# Master
roscore
# 回放数据
rosbag play --pause -l rgbd_dataset_freiburg1_xyz.bag
其中参数**–pause表示使用空格键进行控制回放，-l表示循环回放。此处我下载的ROS bag文件为rgbd_dataset_freiburg1_xyz.bag**，你在回放时应当修改为你对应需要回放的文件。 
此处可以点击键盘上“S”键，使数据向前一步（step）。点击空格键可以控制开始或暂停。
2.深度相机节点
回放ROS bag数据集后，即可打开深度相机节点进行SLAM建图了。此处依旧以rgbd_dataset_freiburg1_xyz.bag 文件为例进行说明。启动代码如下
# 进入ORB_SLAM2源码目录下
roscd ORB_SLAM2
cd ../../..
# 启动节点
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image
此处说明启动节点的格式如下：
rosrun 功能包名 节点名 磁带文件 内参文件 _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image
其中由于启动的相机为深度相机，故而节点为RGBD。此处通过命令rosbag info发现我所下载的数据集所发布的彩色信息和深度信息话题分别在 /camera/rgb/image_color 和 /camera/depth/image ，故而指定参数 _rgb 和 _depth内容如上。
启动相机节点后，在回放数据集出通过空格键控制回放开始。最终实现如下效果：
