将rgbd数据集制作成rosbag，并发布图片和camera_info消息

因为最近做的项目需要和别的开源项目做一些对比，比如rgbdslamV2，但是rdgbslamV2使用的输入是rosbag，并且他必须要订阅四个话题才能运行，这四个话题分别是：

/camera/rgb/image_color

/camera/depth/image

/camera/rgb/camera_info

/camera/depth/camera_info

分别表示深度和彩色图片消息，以及对应的相机参数消息。这个camera_info的话题上发布的消息的类型是：

sensor_msgs/CameraInfo

她的格式如下（主要包含了时间戳，以及相机内参K，矫正系数D等等）：

下面的程序基于格式类似于TUM数据集格式（比如ICL-NUIM数据集）的数据集来实现的，也就是说，你需要有一个associations文件，这个文件里面记录了对应的一对对rgb和depth图像，他的格式如下：

1341846226.817920 rgb/1341846226.817920.png 1341846226.817948 depth/1341846226.817948.png
1341846226.850323 rgb/1341846226.850323.png 1341846226.850341 depth/1341846226.850341.png
1341846226.881894 rgb/1341846226.881894.png 1341846226.881917 depth/1341846226.881917.png
1341846226.917980 rgb/1341846226.917980.png 1341846226.918015 depth/1341846226.918015.png

每一行有四列，第一列对应着rgb图像的时间戳，第二列是rgb图像的相对路径，第三列是depth图像的时间戳，第四列是depth图像的相对路径。我在程序里面是通过这个文件来确定rgb和depth图像的对应关系，来一轮一轮地发送消息到话题上。

ros里面提供了一个包可以实现相关功能，我们首先安装一下，执行下面指令：

$ mkdir -p ~/image_transport_ws/src
$ cd ~/image_transport_ws/src
$ source /opt/ros/kinetic/setup.bash  #notice 看你安装的是什么版本的ros,我的是kinetic
$ catkin_create_pkg learning_image_transport image_transport cv_bridge #生成一个包
$ cd ~/image_transport_ws  #或者 cd ..
$ rosdep install --from-paths src -i -y --rosdistro kinetic #这里也需要改成自己的安装
$ catkin_make
$ source devel/setup.bash
$ git clone https://github.com/ros-perception/image_common.git
$ ln -s `pwd`/image_common/image_transport/tutorial/ ./src/image_transport_tutorial
$catkin_make #编译源文件

然后在/image_transport_ws 这个路径下的,运行:roscore
然后发布一个照片：
source devel/setup.bash
rosrun image_transport_tutorial my_publisher path/to/some/image.jpg
订阅这个
节点并显示照片：rosrun image_transport_tutorial my_subscriber

上面的指令会创建一个工作空间，然后这个工作空间里面的代码是通过软链接来链接到外面的clone的文件。而我们接下来要实现的功能就根据刚才运行的这个my_publisher的节点，修改my_publisher.cpp里面程序来实现的，具体是：发布rgbd图像的数据，以及对应的rgb和depth对应的camera_info消息。我们将my_publisher.cpp修改为如下：

#include <camera_info_manager/camera_info_manager.h>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <vector>
#include <glob.h>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
using namespace std;

int parseInfoFile(
        std::string &strAssociation_Path,
        vector<pair<int,pair<string,string> > > &vec_info)//vec_info保存rgb和depth的路径信息
{
    char * line = NULL;
    size_t len = 0;
    ssize_t read;
    FILE *pFile = fopen(strAssociation_Path.c_str(), "r");
    if(!pFile) {
        return -1;
    }
    while ((read = getline(&line, &len, pFile)) != -1) {

        std::istringstream is(line);//istringstream对象可以绑定一行字符串，然后以空格为分隔符把该行分隔开来。
        std::string part;//保存以空格分开的每一部分
        int iIdxToken = 0;
        while (getline(is, part))
        {
            if('#' == part[0])/// Skip file comment '#"
            {
                continue;
            }

            int timeSeq = 0;
            std::string strDepthPath;
            std::string strRgbPath;

            std::istringstream iss(part);
            std::string token;

            while (getline(iss, token, ' '))
            {
                if(2 == iIdxToken) //Time rgb
                {//first token which is time
                    //什么都不干
                }
                else if(1 == iIdxToken)//rgb path，fr1，fr2和fr3的第2列和第四列相反，可能需要修改这里iIdxToken为3
                {
                    strRgbPath = token;
                    // std::cout<<strRgbPath<<endl;
                }
                else if(0 == iIdxToken)//Time depth
                {
                    timeSeq=atoi(token.c_str());
                }
                else if(3 == iIdxToken)//depth path fr1，fr2和fr3的第2列和第四列相反，可能需要修改这里iIdxToken为1
                {
                    strDepthPath = token;
                    // std::cout<<strDepthPath<<endl;
                }
                iIdxToken++;
            }

            vec_info.push_back(make_pair(timeSeq,make_pair(strRgbPath,strDepthPath)));
        }
    }
    fclose(pFile);
    return 0;
}

int main(int argc, char** argv)
{

    if(argc !=2)
    {
        cerr << endl << "path_to_depth_error"<< endl;
        return 1;
    }
    char new_name[20];
    string basedir=string(argv[1])+'/';
    string input_associatiations=basedir+"/associations.txt";
    vector<pair<int,pair<string,string> > > picInfo;
    if(parseInfoFile(input_associatiations,picInfo))
        cerr<<" Can‘t read file！ "<<endl;
    cout<<"There is "<<picInfo.size()<<" picture pair in all!!"<<endl;

    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;

    image_transport::ImageTransport it(nh);
    //分别将深度图和彩色图发送到两个不同的话题上，准备被录制
    image_transport::Publisher pubrgb = it.advertise("/camera/rgb/image_color", 30);
    image_transport::Publisher pubdepth = it.advertise("/camera/depth/image", 30);
    //分别将rgb和深度图对应的info信息发送到不同的话题上
    ros::Publisher pub_rgb_info = nh.advertise<sensor_msgs::CameraInfo>("/camera/rgb/camera_info", 30);
    ros::Publisher pub_depth_info = nh.advertise<sensor_msgs::CameraInfo>("/camera/depth/camera_info", 30);
    const string camurl = "file:///home/wei/Documents/project/tools/image_transport/calib.yaml";//保存图像参数的yaml文件的位置
    camera_info_manager::CameraInfoManager caminfo(nh, "rgb", camurl);

    sensor_msgs::CameraInfo ci;


    //the argv is the url of the image,may we can use that for all images
    ros::Rate loop_rate(16);
    std::cout<<"Progress: "<<endl;
    for(unsigned int i = 0; i < picInfo.size(); ++i)//开始处理每个图片
    {
        if(!nh.ok())
            break;

        cv::Mat rgb,depth;
       // int timestamp=picInfo[i].first;
        string rgbpath=picInfo[i].second.second;
        rgbpath=basedir+rgbpath;
        string depthpath=picInfo[i].second.first;
        depthpath=basedir+depthpath;


        rgb = cv::imread(rgbpath,-1);
        depth=cv::imread(depthpath,-1);
        //cv::imshow("have a look",depth);
        //cv::waitKey(1);
        if(rgb.empty()||depth.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << rgbpath << "   or   "<< depthpath<< endl;
            return 1;
        }

        std::cout << '\r'
                  << std::setw(4) << std::setfill('0') << i << " / "
                  << std::setw(4) << std::setfill('0') << picInfo.size()
                  << std::flush;


        sensor_msgs::ImagePtr rgbmsg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", rgb).toImageMsg();
        ros::Time begin = ros::Time::now();
        rgbmsg->header.stamp = begin;//这里的时间戳换成你自己的,需要自己再写一个读取时间戳的函数,注意和image进行对应.一般时间戳文件也包含在图片的文件夹中.
        pubrgb.publish(rgbmsg);//发布rgb图像

        sensor_msgs::ImagePtr depthmsg = cv_bridge::CvImage(std_msgs::Header(), "mono16", depth).toImageMsg();
        depthmsg->header.stamp=begin;
        pubdepth.publish(depthmsg);//发布深度图像

        ci=caminfo.getCameraInfo();
        ci.header.frame_id = "/openni_rgb_optical_frame";
        ci.header.stamp=begin;
        pub_rgb_info.publish(ci);//发布rgbinfo

        ci.header.frame_id = "/openni_depth_optical_frame";
        pub_depth_info.publish(ci);//发布深度depth

        ros::spinOnce();
        loop_rate.sleep();
    }
}

简单来说上面的代码分为以下几步：

1. 指定需要发布的信息的话题，并创建一个ros::Publisher用于发布相机info消息
2. 创建一个camera_info_manager::CameraInfoManager类型的caminfo对象，并用他从指定的url里面（其实就是保存这上面的相机数据的url）读取相机的info
3. 再创建一个消息：sensor_msgs::CameraInfo，这个消息在一个循环里面不断更新，首先读取caminfo.getCameraInfo();，然后更新info消息的时间戳等消息
4. 最后发布即可

 

另外，修改CMakeLists.txt，将第4行和第12行修改为：

find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport camera_info_manager message_generation sensor_msgs)
catkin_package(CATKIN_DEPENDS cv_bridge image_transport camera_info_manager message_runtime sensor_msgs)

实际上就是添加了一个camera_info_manager包。然后把整个工作空间重新catkin_make一下，然后执行：

rosrun image_transport_tutorial my_publisher  /path/to/your/dataset

在执行rostopic list,就可以查看到很多的话题正在发布，包括我开头提到的需要的四个话题，然后我们对需要的话题开始进行录制：

rosbag record -O /path/to/save/ros.bag /camera/rgb/image_color /camera/depth/image /camera/rgb/camera_info /camera/depth/camera_info

遇到的问题

• 第一个问题是自己如何创建这个camera_info话题上的sensor_msgs/CameraInfo消息，网上的教程非常少，找了好久才找到一个：

  https://answers.ros.org/question/211240/publishing-camera_info-messages/

这里并没有在代码里面直接设置相机的相关参数，比如之前图片里面展示的K，P，D，R（没找到直接设置的代码案例），这里采用的方法是写一个yaml文件，从yaml文件里面读取相关参数，但是这个yaml文件如何写？找了好久网上根本没人给出这个yaml文件的内容，感觉应该就是通过ros的标定程序获得的参数文件，这里直接给出这个yaml文件的内容，大家把里面的参数修改一下换成呢个自己的就可以了，不用重新标定了。（你看，帮你省了多大力，还不快给我点个赞！！！）

image_width: 640
image_height: 480
camera_name: rgb
camera_matrix:
  rows: 3
  cols: 3
  data: [481.2, 0.0, 319.5, 0.0, -480.0,239.5, 0.0, 0.0, 1.0]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.0, 0.0, 0.0, 0.0, 0.0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
projection_matrix:
  rows: 3
  cols: 4
  data: [481.2, 0.0,  319.5, 0.0, 0.0,-480.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0]

• 程序写完之后，在rviz里面读取指定的话题上的图片数据之后，rviz读取不出来，显示下面的错误：

[ WARN] [1607503757.824458468]: OGRE EXCEPTION(2:InvalidParametersException): Stream size does not match calculated image size in Image::loadRawData at /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreImage.cpp (line 283)
[ERROR] [1607503757.824565432]: Error loading image: OGRE EXCEPTION(2:InvalidParametersException): Stream size does not match calculated image size in Image::loadRawData at /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreImage.cpp (line 283)

原因是因为将opencv图像转化为rosmsg的时候，设定的编码方式出了问题，如下：

sensor_msgs::ImagePtr rgbmsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", rgb).toImageMsg();

比如你rgb里面放的如果不是rgb图像，而是灰度图，那么就会出现上面的 问题。这个编码方式有以下几种：

mono8: CV_8UC1, grayscale image

mono16:CV_16UC1, 16-bit grayscale image

bgr8: CV_8UC3, color image with blue-green-red color order

rgb8: CV_8UC3, color image with red-green-blue color order

bgra8: CV_8UC4, BGR color image with an alpha channel

rgba8: CV_8UC4, RGB color image with an alpha channel

如果这个不行就换个试试，比如同样是灰度图，我使用mono8就会报错，使用mono16就可以。这可能因为原本的灰度图就是CV_16UC1格式的。

• 遇到错误

ERROR：Tried to advertise a service that is already advertised in this node

原因就是发送消息的话题名字重复了，或者是在while(ros::ok())将advertise包含进去了，这样就会导致重复地向主节点注册主题，故报错。据wiki上解释，有时候自己编写的节点可能与系统本身所有的节点发送的主题起冲突，此时只需修改你自己的主题名即可。

以上就是这个小小的脚本编写过程中遇到的问题，虽然是个很小的程序，但是还是费了不少力气搜索资料的，我想这个程序应该有不少人也需要，因此在这里和大家分享一下。别忘了点个赞哟！！！！！