Ubuntu20.04 ORB-SLAM2（含ROS）配置并在Gazebo下测试-Part2

前言

Ubuntu20.04系统安装ROS noetic(含Gazebo11)、安装ORB-SLAM2，并在此基础上实现基于ROS与Gazebo的ORB-SLAM2测试。

Part2：ORB-SLAM2在数据集与Gazebo11下的测试

1.ORB-SLAM2数据集下的测试

Part1提到需要将ORB-SLAM2与ROS工程放在一个文件文件夹下，如下图所示：

下面讲解在离线数据集下测试ORB-SLAM2，包括单目、双目和RGBD，分别测试一个数据集：

a. 单目数据集测试

（1）下载测试数据集
TUM Dataset下载地址：TUM Dataset，选择下载fr1/xyz，并放入/home/cx/cxapp/datasets目录下。进入ORB-SLAM2文件，执行命令：

cd /home/cx/cxapp/catkin_ws/src/ORB_SLAM2

（2）进行数据集测试
执行命令：

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml /home/cx/cxapp/datasets/rgbd_dataset_freiburg1_xyz

注意：数据集rgbd_dataset_freiburg1_xyz的文件地址修改为自己文件所在位置。
（3）测试示例

b. 双目数据集测试

（1）下载测试数据集
下载网址：http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/
为便于管理，将下载的数据放在/home/cx/cxapp/datasets文件夹下，如下图：

（2）进行数据集测试
在ORB-SLAM2目录内打开一个终端，输入：

./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml  /home/cx/cxapp/datasets/MH_01_easy/mav0/cam0/data /home/cx/cxapp/datasets/MH_01_easy/mav0/cam1/data Examples/Stereo/EuRoC_TimeStamps/MH01.txt

注意：需正确输入数据集所在位置，请输入MH_01_easy在自己电脑下存放位置。
（3）测试示例

c. RGB-D数据集测试

（1）下载测试数据集
为便于测试，可使用单目测试中下载的数据集，即fr1/xyz，本测试数据集测放置位置为：/home/cx/cxapp/datasets/rgbd_dataset_freiburg1_xyz。其他TUM Dataset见：
http://vision.in.tum.de/data/datasets/rgbd-dataset/download
（2）进行数据集预处理
因为Kinect相机使用非同步方式采集RGB和depth图像，导致两种图像的时间戳集不相交，所以需要对数据集进行预处理实现RGB与depth图像的关联。为此，使用“associate.py”脚本从rgb.txt文件和depth.txt文件中读取时间戳，通过找到最佳匹配实现RGB与depth图像的关联。
预处理方法：
在associate.py下载python脚本文件associate.py，将其放置在/cxapp/catkin_ws/src/ORB_SLAM2/Examples/RGB-D文件夹下，并在该文件夹下打开终端执行命令：

python associate.py /home/cx/cxapp/datasets/rgbd_dataset_freiburg1_xyz/rgb.txt /home/cx/cxapp/datasets/rgbd_dataset_freiburg1_xyz/depth.txt > associations.txt

但是出现下面的错误：

这是由Python2和python3语法的差别引起的，需要将associate.py中第86行87行的

first_keys = first_list.keys()

second_keys = second_list.keys()

修改为：

first_keys = list(first_list.keys())

second_keys = list(second_list.keys())

最后在/cxapp/catkin_ws/src/ORB_SLAM2/Examples/RGB-D文件夹下得到RGB与depth图像的关联文件associations.txt。

（3）进行数据集测试
在ORB-SLAM2目录下打开终端，输入命令：

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/cx/cxapp/datasets/rgbd_dataset_freiburg1_xyz /home/cx/cxapp/catkin_ws/src/ORB_SLAM2/Examples/RGB-D/associations.txt 

注意：RGB-D数据集位置、associations.txt文件位置需更改为自己的文件位置。
（4）测试示例
得到地图与当前帧特征点测试结果如下：

小结

单目与双目的数据集测试比较简单，RGB-D数据集测试需要对齐RGB与depth图像的时间戳。在数据集的测试中，需要正确配置数据集的文件地址。

2.ORB-SLAM2在Gazebo11下的测试

a. 仿真环境搭建

（1）移动机器人平台-Turtlebot3
为实现ORB-SLAM2在Gazebo仿真软件下的测试，需要搭建机器人仿真平台，这里搭建Turtlebot3的移动机器人仿真平台，参考：
Turtlebot3

注意：将Turtlebot3的pack统一放在/home/cx/cxapp/catkin_ws/src目录下。

（2）视觉传感器搭建
在具有移动机器人平台的基础上，需要添加视觉传感器以实现外部RGB、depth图像的采集，这里搭建Kinect V1相机，参考：
Kinect相机构建

（3）Gazebo World创建
I. 移动机器人需要在某一场景下进行定位、建图以及导航等操作，根据测试需求可以创建自己的World，参考：

创建World文件参考1
创建World文件参考2
创建World文件参考3

这里创建名为cx.world的Gazebo场景文件，并将其放置在/home/cx/cxapp/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds目录下：

II. 为了启动创建的cx.world场景文件，需要修改launch文件。对/home/cx/cxapp/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch目录下的turtlebot3_world.launch文件进行修改，添加cx.world在launch文件中：

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="-2.0"/>
  <arg name="y_pos" default="-0.5"/>
  <arg name="z_pos" default="0.0"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/cx.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
</launch>

III. 编译刷新环境
返回/home/cx/cxapp/catkin_ws目录，打开终端，执行下面命令进行编译并激活工作空间：

catkin_make
source devel/setup.bash

b. 仿真测试

（1）在/home/cx/cxapp/catkin_ws/src目录下，打开终端，输入下面命令启动仿真场景：

roslaunch turtlebot3_gazebo turtlebot3_world.launch

启动Gazebo后的界面为：

（2）在/home/cx/cxapp/catkin_ws/src目录下，打开第二个终端，输入下面命令启动turtlebot3键盘控制功能：

 roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

启动turtlebot3的控制节点，如下图：
（3）在/home/cx/cxapp/catkin_ws/src目录下，打开第三个终端，输入命令：

rosrun ORB_SLAM2 RGBD ./ORB_SLAM2/Vocabulary/ORBvoc.txt ./ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml

启动ORB-SLAM2 RGBD节点，此时发现图像没有显示，报错：WAITING FOR IMAGES。这是因为ros_rgbd.cc文件接受的图像话题与Kinect V1发布的话题没有对应。首先查看当前发布的话题，打开第四个终端，执行命令：

rostopic list 

得到话题信息如下，可以看到Kinect V1发布的话题：

根据话题名将/home/cx/cxapp/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src目录下的ros_rgbd.cc文件进行修改，修改后内容为：

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/image_raw", 1);

然后在catkin_ws_orb/src目录下重新运行命令：

rosrun ORB_SLAM2 RGBD ./ORB_SLAM2/Vocabulary/ORBvoc.txt ./ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml

（4）测试示例
实现特征点检测、地图构建，如图：

ORB-SLAM2地图：地图点（黑色）、局部地图点（红色）与关键帧（蓝色）、当前关键帧（绿色）

总结

开展SLAM研究与应用，需要熟悉Linux系统和ROS操作、cmake、shell、OpenCV等，为促进后续开发，需要更深入学习。
从开源项目入手，学习其理论和工程实现，是入门的捷径，深入之后在其基础上做一定的改进。
然而工程实现只是科研的一部分，更重要的是抓住SLAM中的一个小问题，看看能否对现有的算法进行改进或者比较。所以还是要从论文出发，寻找灵感，去实现并验证效果。