ROS学习：制作自己的TUM数据集

Mr Qin

分类：ROS

个人专栏：图像处理

发布时间 2021.12.02阅读数 5029 评论数 1

1. 问题描述：

在ROS系统中，可以使用bag文件来保存和恢复系统的运行状态，比如录制雷达和相机话题的bag包，然后通过回放用来SLAM定位或者进行联合外参标定。但是在一些情况下bag包并不是那么方便，例如：

所运行的程序不能时时运行，需要每张图片按照顺序进行处理。此时虽然rosbag可以设置回放速度以及队列缓存，但是有时仍然不能很好的满足顺序处理的要求。
有些平台没有安装ROS，不能接收图像的topic数据，即不能很好地将程序进行移植。

此时就需要我们将bag数据转存为TUM通用数据集的格式，然后通过读取TUM数据的方式读取我们的数据。具体的，本博文实现的功能为：提取并保存bag包中的RGB图像和depth图像，使其可以以单目或者RGB-D的数据集的形式供使用。

2. TUM数据集格式介绍：

TUM是一个RGB-D的数据集，主页为https://vision.in.tum.de/data/datasets/rgbd-dataset/download 。每一个数据集包含以下文件：depth ,rgb ,depth.txt ,rgb.txt ,accelerometer.txt ,groundtruth.txt。要运行此数据集，只需要恢复前四个文件即可。
rgb和depth文件中保存了rgb和深度图片，如：

其中rgb.txt和depth.txt记录的每张图片的采集时间和图片名称（图片名称也是以时间来命名的）。

所以我们只需要编写一个ROS节点，获取并保存相应的相应的图片及时间戳就行了。

3. 程序编写：

首先在catkin_ws工作空间下创建功能包(ROS的基本知识在这里不再赘述)

cd ~/catkin_ws/src
catkin_create_pkg rosbag2tum roscpp std_msgs sensor_msgs  cv_bridge

在rosbag2tum/src文件夹在创建新的cpp文件bagToTUM.cpp，编写节点程序。

首先，在程序开始前定义了一些全局变量，用来存储数据保存路径以及图像topic名称，这些也是需要根据自己实际情况进行修改的。
然后，定义保存路径的函数savePath()，输入参数为包存路径以及double格式的图像时间戳，用时间戳命名图像。
然后，定义了回调函数GrabRGB()和GrabDepth(),在接受到相关话题数据时运行，用来保存、显示图像。
最后，主函数main()中订阅了相关话题。

#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <string> 
using namespace std;

//设置一些保存路径
string Path = "/home/qin/桌面/fr1/";
string pathRGB = Path + "rgb.txt";
string pathDepth = Path + "depth.txt";
string dataRGB = Path + "rgb/";
string dataDepth = Path + "depth/";
//设置图像topic名称
string depth_topic = "/camera/depth/image";
string rgb_topic = "/camera/rgb/image_color";

//保存图像路径至txt文件下
void savePath(string &path, string &type,double time){
   ofstream of;
   of.open(path, std::ios_base::app);
   if(of.fail()){
       ROS_INFO("Fail to opencv file!!");
   }else{
       of<<endl;
       of<<std::fixed<< time <<" "<< type <<time<<".png"; 
       of.close();
   }
}
//RGB图像回调函数
void GrabRGB(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
       //保存图像及路径
        cv_ptr = cv_bridge::toCvShare(msg);
        //颜色通道转换
        cv::Mat bgrImage;
        cvtColor(cv_ptr->image, bgrImage, CV_BGR2RGB);
        //cv::imshow("RGB", bgrImage);

        double time = cv_ptr->header.stamp.toSec();
        string type = "rgb/";
        savePath(pathRGB, type, time);
        std::ostringstream osf;
        osf<< dataRGB <<std::fixed <<time << ".png";//图像以时间戳命名
        cv::imwrite(osf.str(), bgrImage);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
     //cv::waitKey(2);    
}

//深度图像回调函数
void GrabDepth(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvShare(msg);
        //imshow("Depth", cv_ptr->image);
        double time = cv_ptr->header.stamp.toSec();
        string type = "depth/";
        savePath(pathDepth, type, time);
        std::ostringstream osf;
        osf<< dataDepth <<std::fixed  <<time << ".png";
        cv::imwrite(osf.str(), cv_ptr->image);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
     //cv::waitKey(2);
}

//节点主函数
int main(int argc,char **argv)
{
    ros::init(argc,argv,"bagToTUM");
    ros::start();
    ofstream of;
    of.open(pathRGB, std::ios_base::app);
    if(of.fail()){
       ROS_INFO("Fail to opencv file!!");
    }else{
       of<<"#------------start a new dataset-----------------"; 
       of.close();
    }
    of.open(pathDepth, std::ios_base::app);
    if(of.fail()){
       ROS_INFO("Fail to opencv file!!");
    }else{
       of<<"#------------start a new dataset-----------------"; 
       of.close();
    }

    //订阅图像话题
    ROS_INFO("bagToTUM is ready.");
    ros::NodeHandle nodeHandler;
    ros::Subscriber subRGB = nodeHandler.subscribe(rgb_topic, 5, &GrabRGB);
    ros::Subscriber subDepth = nodeHandler.subscribe(depth_topic, 5, &GrabDepth);

    ros::spin();
    return 0;
}

接下来配置CMakeLists.txt。所依赖的第三方只有opencv库（这里为opencv 3.x），opencv库容易和ros的cv_bridge版本冲突，解决方案见我的csdn博文https://blog.csdn.net/qinqinxiansheng/article/details/120219388 。

cmake_minimum_required(VERSION 3.0.2)
project(rosbag2tum)

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  roscpp
  sensor_msgs
  std_msgs
)

find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
   find_package(OpenCV 2.4.3 QUIET)
   if(NOT OpenCV_FOUND)
      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
   endif()
endif()

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES rosbag2tum
  CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${OpenCV_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)

# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

 add_executable(bag2tum src/bagToTUM.cpp)
 target_link_libraries(bag2tum ${catkin_LIBRARIES} ${OpenCV_LIBS})

最后用catkin_make命令编译工作空间即可。

4. 使用说明及结果展示：

4.1 注意事项

运行时需要知道的是：

此程序可以用于单目。
运行前相关文件夹要有效，程序不会自己新建文件夹。
图像的路径保存在rgb.txt和depth.txt中，若文件路径下不含该txt文件会重新生成一个。
rosbag数据不会完全还原原始数据，且用topic收发数据时可能会丢失前几帧图像。
不要重复执行程序，图像的路径保存在rgb.txt时是直接在文件尾部添加的，多次时不会覆盖之前的结果，导致越来越长。
如果检测不出来节点，直接运行运行catkin_ws/devel/lib/rosrgb2tum/bag2tum也可以。

4.2 使用方法

这里我们用TUM的fr1_xyz.bag制作图像格式的数据集，并用ORB-SLAM2验证我们制作的数据集可以正常使用。

a. 在src/bagToTUM.cpp中修改相关路径以及topic 名称，在要保存的路径下创建rgb，depth文件夹

b. 重新编译工作空间，运行roscore命令以及rgb2tum节点，此时回自动创建rgb.txt和depth.txt文件

c. rosbag play命令运行bag数据集，比如我的

rosbag play  ./rgbd_dataset_freiburg1_xyz.bag  -r 0.5

其中-r 0.5表示以0.5倍速播放，这是因为如果数据量过大，程序可能处理不过来，导致丢失数据。

d. 程序运行效果过程如图所示（程序中已经把显示imshow注释掉了，否则会很卡）

e. 呐，798张图片一张不少

f. 然后下载associate.py（https://download.csdn.net/download/qinqinxiansheng/38156469 ）文件，对图像的时间戳进行对准

python  associate.py rgb.txt  depth.txt > associate.txt

生成的associate.txt文件如图所示：

g. 最后用ORB-SLAM2测试一下我们的数据集，完美。以我的为例，测试我们刚才新制作的数据集fr1_xyz

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/robot/MrQin/MyTUM/fr1_xyz  /home/robot/MrQin/MyTUM/fr1_xyz/associate.txt

恩，可以跑。如果你还不知道我为什么用TUM的rosbag的数据格式格式转成普通的图片格式，这样做的意义在哪里，那么再见。

TUM数据集

原创文章作者：Mr Qin。如若转载，请注明出处：古月居 http://www.guyuehome.com/35920

打赏 0

上一篇：【动态SLAM】TUM动态数据集介绍及讨论

下一篇：手机视频制作TUM格式数据集（ORB-SLAM2测试）

ROS学习：制作自己的TUM数据集

Mr Qin

1. 问题描述：

2. TUM数据集格式介绍：

3. 程序编写：

4. 使用说明及结果展示：

4.1 注意事项

4.2 使用方法

a. 在src/bagToTUM.cpp中修改相关路径以及topic 名称，在要保存的路径下创建rgb，depth文件夹

b. 重新编译工作空间，运行roscore命令以及rgb2tum节点，此时回自动创建rgb.txt和depth.txt文件

c. rosbag play命令运行bag数据集，比如我的

d. 程序运行效果过程如图所示（程序中已经把显示imshow注释掉了，否则会很卡）

e. 呐，798张图片一张不少

f. 然后下载associate.py（https://download.csdn.net/download/qinqinxiansheng/38156469 ）文件，对图像的时间戳进行对准

g. 最后用ORB-SLAM2测试一下我们的数据集，完美。以我的为例，测试我们刚才新制作的数据集fr1_xyz

为你推荐

Unity-ROS2与URDF导入（三）

ROS1结合自动驾驶数据集Kitti开发教程(八)画出2D检测框

ROS Gazebo仿真 控制Turtlebots实现扫地机器人算法

小车yolo机械臂（一）ros下gazebo搭建小车（可键盘控制）安装摄像头仿真 加载yolo检测识别标记物体

Ubuntu18.04 melodic 安装与下载ROS（超详细教程）

机器人抓取（七）—— Azure Kinect 相机的 ros 服务器（Service）

评论（1）

关于作者

Mr Qin

22

2

7

9

ROS学习：制作自己的TUM数据集

gazebo+ORB-SLAM仿真教程

一文讲解极线约束、本质矩阵、单应矩阵

相关推荐

ROS 常用GUI测试工具

机器人抓取（八）—— 基于 gqcnn 模型的抓取位姿估计 ros 服务器

ROS中Moveit生成轨迹如何作用于实际的机械臂(一)

turtlebot3 Slam建图和导航仿真

【Gazebo】UR3 添加 robotiq ft300 传感器和 f2 140 手爪

使用Colab对wget下载加速

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 ros中启动gazebo时报错

5积分 想买能用ROS2的开发套件。或者开发板

TA的专栏

Lab508

图像处理

ROS

动态SLAM

专栏导航

给作者打赏

ROS Gazebo仿真控制Turtlebots实现扫地机器人算法

小车yolo机械臂（一）ros下gazebo搭建小车（可键盘控制）安装摄像头仿真加载yolo检测识别标记物体

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板