ORB-SLAM2学习笔记5之EuRoc、TUM和KITTI开源数据运行ROS版ORB-SLAM2生成轨迹

0 引言

ORB-SLAM2学习笔记1已成功编译安装ROS版本ORB-SLAM2到本地，本篇目的是用EuRoc、TUM、KITTI开源数据来运行ROS版ORB-SLAM2，并生成轨迹。

1 数据预处理

1.1 EuRoc数据

EuRoc开源数据已经支持rosbag的数据包下载，如下图，直接点击对应的link下载即可，本文以Machine Hall 01为例：

下载的Machine Hall 01数据包，用rosbag info MH_01_easy.bag 命令查看：

path:        MH_01_easy.bag
version:     2.0
duration:    3:06s (186s)
start:       Jun 25 2014 03:02:59.81 (1403636579.81)
end:         Jun 25 2014 03:06:06.70 (1403636766.70)
size:        2.5 GB
messages:    47283
compression: none [2456/2456 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    3682 msgs    : sensor_msgs/Image         
             /cam1/image_raw    3682 msgs    : sensor_msgs/Image         
             /imu0             36820 msgs    : sensor_msgs/Imu           
             /leica/position    3099 msgs    : geometry_msgs/PointStamped

1.2 TUM数据

TUM开源数据已经支持rosbag的数据包下载，如下图，直接点击对应的ROS bag下载即可，本文以freiburg1_desk为例：

下载的freiburg1_desk数据包，用rosbag info rgbd_dataset_freiburg1_desk.bag 命令查看：

path:         rgbd_dataset_freiburg1_desk.bag
version:      2.0
duration:     23.8s
start:        May 10 2011 20:44:09.56 (1305031449.56)
end:          May 10 2011 20:44:33.32 (1305031473.32)
size:         371.7 MB
messages:     19893
compression:  bz2 [1210/1210 chunks; 29.85%]
uncompressed:   1.2 GB @ 52.3 MB/s
compressed:   370.9 MB @ 15.6 MB/s (29.85%)
types:        sensor_msgs/CameraInfo         [c9a58c1b0b154e0e6da7578cb991d214]
              sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
              sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
              tf/tfMessage                   [94810edda583a504dfda3829e70d7eec]
              visualization_msgs/MarkerArray [f10fe193d6fac1bf68fad5d31da421a7]
topics:       /camera/depth/camera_info     595 msgs    : sensor_msgs/CameraInfo        
              /camera/depth/image           595 msgs    : sensor_msgs/Image             
              /camera/rgb/camera_info       613 msgs    : sensor_msgs/CameraInfo        
              /camera/rgb/image_color       613 msgs    : sensor_msgs/Image             
              /cortex_marker_array         2360 msgs    : visualization_msgs/MarkerArray
              /imu                        11815 msgs    : sensor_msgs/Imu               
              /tf                          3302 msgs    : tf/tfMessage

1.3 KITTI数据

KITTI开源数据是不支持rosbag的数据包下载，ORB-SLAM2学习笔记4 中已经下载非ROS版的KITTI数据data_odometry_poses，但本文又需要用rosbag的数据包，所以需要转换图片，时间戳等数据信息为rosbag数据包。

推荐一个转换脚本：https://gitee.com/zengtaiping/image2rosbag_KITTIodometry
下载转换的脚本：

git clone https://gitee.com/zengtaiping/image2rosbag_KITTIodometry.git

下载后，进入到文件夹中，可发现如下的文件tree:

.
├── img2bag_kitti_MonoBag.py 
├── img2bag_kitti_odo.py      
├── img2bag_kitti_StereoBag.py # kitti转换成双目rosbag
├── kitti_republish.launch
└── README.md

0 directories, 5 files

然后执行如下命令（PATH为data_odometry_gray存放的路径，KITTI_StereoBag_seq00.bag可自行命名）来转换KITTI数据为rosbag数据，以下为 KITTI data_odometry_poses 中的 00 组数据，转换成双目rosbag为例：(也可用其他脚本转换成单目rosbag等)

python img2bag_kitti_StereoBag.py PATH/data_odometry_gray/dataset/sequences/00 KITTI_StereoBag_seq00.bag PATH/data_odometry_gray/dataset/sequences/00/times.txt

等待片刻，用命令rosbag info KITTI_StereoBag_seq00.bag 查看生成的rosbag包

path:        KITTI_StereoBag_seq00.bag
version:     2.0
duration:    7:50s (470s)
start:       Jan 01 1970 08:00:00.00 (0.00)
end:         Jan 01 1970 08:07:50.58 (470.58)
size:        3.9 GB
messages:    9082
compression: none [4541/4541 chunks]
types:       sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
topics:      camera/left/image_raw    4541 msgs    : sensor_msgs/Image
             camera/right/image_raw   4541 msgs    : sensor_msgs/Image

2 代码修改

由于ROS版本ORB-SLAM2中的图像topic都写死了，所以主要修改对应的图像topic名字为对应的开源数据图像topic的名字。

切记每次修改或统一修改后，执行./build_ros.sh重新编译。

2.1 单目

打开ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_mono.cc文件，以使用EuRoc数据中的cam0图像topic名字 /cam0/image_raw为例，如下所示，大概在第64行代码处修改即可。

ros::NodeHandle nodeHandler;
// ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
ros::Subscriber sub = nodeHandler.subscribe("/cam0/image_raw", 1, &ImageGrabber::GrabImage,&igb);

2.2 双目

打开ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc文件，以使用EuRoc数据中的cam0图像topic名字 /cam0/image_raw和cam1图像topic名字 /cam1/image_raw为例，如下所示，大概在第112、113行代码处修改即可。

ros::NodeHandle nh;

// message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1);
// message_filters::Subscriber right_sub(nh, "camera/right/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/cam0/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/cam1/image_raw", 1);

2.3 RGB-D

打开ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc文件，以使用TUM数据中的rgb彩色图像topic名字 /camera/rgb/image_color和depth深度图像topic名字 /camera/depth/image为例，如下所示，大概在第68、69行代码处修改即可。

// message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1);
// message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 1);

3 运行ROS版ORB-SLAM2

3.1 单目

以使用EuRoc数据中的cam0图像topic名字 /cam0/image_raw为例，修改代码后，重新执行./build_ros.sh，然后执行以下ROS版本命令：

# 新开终端1
roscore

# 新开终端2   ORB-SLAM2工程目录下执行
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml

# 新开终端3    MH_01_easy.bag 存放目录下执行
rosbag play MH_01_easy.bag

执行后，两个可视化页面ORB-SLAM2 Current Frame 和 ORB-SLAM2 Map Viewer开始有数据输入，如下图：


数据回放完毕后，还在该终端目录下保存了轨迹文件KeyFrameTrajectory.txt

3.2 双目

以使用1.3中转换的KITTI rosbag数据为例，左目图像topic名字 camera/left/image_raw和右目图像topic名字 camera/right/image_raw为例，修改代码后，重新执行./build_ros.sh，然后执行以下ROS版本命令：

# 新开终端1
roscore

# 新开终端2   ORB-SLAM2工程目录下执行
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/KITTI00-02.yaml false

# 新开终端3    MH_01_easy.bag 存放目录下执行
rosbag play KITTI_StereoBag_seq00.bag

执行后，两个可视化页面ORB-SLAM2 Current Frame 和 ORB-SLAM2 Map Viewer开始有数据输入，如下图：


数据回放完毕后，还在该终端目录下保存了三个文件：

Saving keyframe trajectory to KeyFrameTrajectory_TUM_Format.txt ...

trajectory saved!

Saving camera trajectory to FrameTrajectory_TUM_Format.txt ...

trajectory saved!

Saving camera trajectory to FrameTrajectory_KITTI_Format.txt ...

trajectory saved!

3.3 RGB-D

以使用TUM数据中的rgb彩色图像topic名字 /camera/rgb/image_color和depth深度图像topic名字 /camera/depth/image为例，修改代码后，重新执行./build_ros.sh，然后执行以下ROS版本命令：

# 新开终端1
roscore

# 新开终端2   ORB-SLAM2工程目录下执行
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

# 新开终端3    MH_01_easy.bag 存放目录下执行
rosbag play rgbd_dataset_freiburg1_desk.bag

执行后，两个可视化页面ORB-SLAM2 Current Frame 和 ORB-SLAM2 Map Viewer开始有数据输入，如下图，Map Viewer 可视化有问题，轨迹在中心处聚集：

仔细排查后，TUM 官网已经说明，如果用的是16-bit PNG图片数据，factor=5000，但如果用的是 32-bit ROS bag数据，factor=1：

factor = 5000 # for the 16-bit PNG files
# OR: factor = 1 # for the 32-bit float images in the ROS bag files

所以打开对应的Examples/RGB-D/TUM1.yaml文件，修改DepthMapFactor参数为 1.0

# Deptmap values factor
# DepthMapFactor: 5000.0
DepthMapFactor: 1.0

然后重新执行rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml 和 rosbag play rgbd_dataset_freiburg1_desk.bag ，如下图，Map Viewer可视化问题解决：


数据回放完毕后，还在该终端目录下保存了轨迹文件KeyFrameTrajectory.txt

至此，成功用EuRoc、TUM、KITTI开源数据来运行ROS版ORB-SLAM2，并生成轨迹。

此外，如果需要用evo评估工具来评估ROS版ORB-SLAM2生成的轨迹和真值轨迹，可参考之前的ORB-SLAM2学习笔记。

ORB-SLAM2学习笔记系列：

• ORB-SLAM2学习笔记1之Ubuntu20.04+ROS-noetic安装ORB-SLAM2
• ORB-SLAM2学习笔记2之TUM开源数据运行ORB-SLAM2生成轨迹并用evo工具评估轨迹
• ORB-SLAM2学习笔记3之EuRoc开源数据集运行ORB-SLAM2生成轨迹并用evo工具评估轨迹
• ORB-SLAM2学习笔记4之KITTI开源数据集运行ORB-SLAM2生成轨迹并用evo工具评估轨迹

Reference:

• https://gitee.com/zengtaiping/image2rosbag_KITTIodometry
• https://cvg.cit.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect

须知少时凌云志，曾许人间第一流。

⭐️