在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

在同一檔案夾下同級目錄中。