KITTI數據集測試 - 4 KITTI數據集創建雙目rosbag 及其使用

1. 從KITTI數據集生成stereo rosbag

python img2bag_kitti_StereoBag.py /*/00 img2bag_kitti_StereoBag_seq00.rosbag /*/00/times.txt

Python 腳本可以在此處找到
https://gitee.com/taiping.z.git/image2rosbag_KITTIodometry/tree/master.

2. republish 到compressed rosbag

launch文件用於打開多個程序進行壓縮.

roslaunch kitti_republish.launch

launch 文件也可在此處找到
https://gitee.com/taiping.z.git/image2rosbag_KITTIodometry/tree/master.

3. pulish message

rosbag play --pause /home/*/workspace/KITTI_to_rosbag/img2bag_kitti_StereoBag_seq00.rosbag

將會得到兩個圖像的image messages
/kitti/camera/left/image/compressed 和 /kitti/camera/right/image/compressed

4. 訂閱兩個image messages

a. 在ROS節點中可以使用message_filters對兩個不同的topics進行訂閱.
使用message_filters::Subscriber初始化subscriber
代碼如下:

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
// add code here
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

b. 對於image_transport的圖像則需要image_transport::SubscriberFilter進行初始化.
代碼如下:
第一部分: 初始化subscriber

    ros::NodeHandle node;
    image_transport::ImageTransport it(node);

    image_transport::SubscriberFilter sub_img_left(it, "/image/left", 1);
    image_transport::SubscriberFilter sub_img_right(it, "/image/right", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncType;
    const SyncType sync_policy(20);
    message_filters::Synchronizer<SyncType> sync(sync_policy, sub_img_left, sub_img_right);
   sync.registerCallback(boost::bind(&image_callback, _1, _2));

第二部分:image_callback

void image_callback(const sensor_msgs::ImageConstPtr img_left, const sensor_msgs::ImageConstPtr img_right)
{
// process two image messages inputs
}

Reference

1. http://wiki.ros.org/message_filters
2. https://answers.ros.org/question/9705/synchronizer-and-image_transportsubscriber