在ubuntu16.04+ROS下使用Kinect V2构建自己的数据集

在ubuntu16.04+ROS的框架下使用kinect V2构建数据集

继上一章完成了kinectV2在ubuntu上驱动以及与ROS接口的安装后，在平时测试的时候，为了不想每次的去把机器人移动起来进行测试，可以尝试制作自己的数据集，方便测试。本博客是参考使用KinectV2制作自己的数据集这篇博客所完成的。

由于kinect v2 可以采集深度与RGB图像，因此可以构建一个TUM形式的数据集。TUM数据集主要包括彩图文件夹，深度图文件夹，rgb.txt，depth.txt，associate.txt。在运行slam时，先读取associate.txt文件，获得一对彩图和深度图的路径，然后通过对应的路径读取相应的彩图和深度图。

下载功能包

本次用的源代码是在github上下载的：

git clone https://github.com/MRwangmaomao/KinectV2_dataset_make.git

深度图像以及RGB图像的保存 get_image.cpp

代码分析

主函数

int main(int argc, char **argv)
{
    ros::init(argc, argv, "get_image");
    ros::start();    
    ros::NodeHandle nh;
    message_filters::Subscriber<sensor_msgs::Image>rgb_sub(nh, "/kinect2/qhd/image_color", 1);
    message_filters::Subscriber<sensor_msgs::Image>depth_sub(nh,"/kinect2/qhd/image_depth_rect",1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageSave,_1,_2));
    ros::spin();
    ros::shutdown();
    
    return 0;
}

该部分主要订阅了由roslaunch kinect2_bridge kinect2_bridge.launch发布的如下两个节点：

/kinect2/qhd/image_color
/kinect2/qhd/image_depth_rect

然后通过 message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub) 实现同步采集彩图与深度图。
然后运行图像的保存函数sync.registerCallback(boost::bind(&ImageSave,_1,_2))

图像保存

注解直接在代码中

void ImageSave(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    string imagergb = "image";
    string imaged = "depth";
    string lie;
    cv_bridge::CvImageConstPtr cv_ptrRGB;   //将ros格式的图像转为opencv的Mat格式
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
     
    
    counters++;
 

   ofstream frgb(save_path + "/rgb.txt",ios::app);  //用来打开文件流/home/wyc/Dataset/rgb.txt，以ios：app格式打开，如果没有该文件就生成该文件
   ofstream fdepth(save_path + "/depth.txt",ios::app);

   //显示图像
    cv::namedWindow(imagergb, cv::WINDOW_AUTOSIZE); 
    cv::imshow(imagergb,cv_ptrRGB->image);
    cv::waitKey(1);
    cv::Mat image_rgb = cv_ptrRGB->image;
    string picname_rgb =to_string( cv_ptrRGB->header.stamp.toSec());
    picname_rgb =save_path + "/rgb/" + picname_rgb + ".png"; 
    cout << picname_rgb << endl;
    imwrite(picname_rgb, image_rgb);//保存rgb图片
    lie = to_string( cv_ptrRGB->header.stamp.toSec()); 
    frgb << lie << " " << "rgb/" << lie << ".png" << endl;  //把信息输入txt文件中去

    //显示图像
    cv::namedWindow(imaged, cv::WINDOW_AUTOSIZE);
    cv::imshow(imaged,cv_ptrD->image);
    cv::waitKey(1);
    
    cv::Mat image_depth = cv_ptrD->image; 
    string picname_depth =to_string( cv_ptrD->header.stamp.toSec()); 
    picname_depth =save_path + "/depth/" + picname_depth + ".png";
    imwrite(picname_depth,  image_depth);//保存rgb图片
    lie = to_string( cv_ptrD->header.stamp.toSec()); 
    fdepth << lie << " " << "depth/" << lie << ".png" << endl;  

    // string picname = to_string(counters);   
    // string cutname = picname.substr(picname.length()-4,4); 
    
    std::cout << "输出第  " << counters << "  幅图像" << std::endl;
    if(counters == 100) // 假设只保存100幅图像 ，需要多少张的图片自己设置
    {
        frgb.close();   //关闭打开的文件流
        fdepth.close();
        cout << "保存图片成功。\n\n";
        ros::shutdown();
        return;
    } 
}

使用

在该cpp文件中需要修改保存文件的地址：

string save_path = "/home/wyc/Dataset";  //根据自己需要修改，这是存储数据集的文件夹的路径

然后保存，并编译：catkin_make
注意：在编译的时候：该包需要放在一个完整的工作空间下的src文件夹下，因为这并不是一个完整的工作空间。
编译完成后会生成两个节点：get_image_node，realsense_get_image_node
在本部分中只需要用到第一个节点
分别运行下面roslaunch文件 以及node节点,然后就可以得到深度图文件夹、RGB文件夹、rgb.txt，depth.txt文件

roslaunch kinect2_bridge kinect2_bridge.launch 
rosrun dataset_make get_image_node

***注意：***在建立存储数据集的文件夹的时候需要在文件夹下建立两文件名为depth、rgb的文件夹，这样才会保存图像文件在这两个文件夹下面。不然就只会生成两个txt文件，而不会保存图像文件。

associate.txt文件的生成

执行下列代码就可以生成associate.txt文件了：

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

注意：在该步骤中，首先需要去下载的源代码中将associate.py文件复制到存放数据集的文件中去，与rgb.txt depth.txt在同一文件夹下同级目录中。