extract_stereo_images.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include <boost/format.hpp>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

#include <image_proc/processor.h>
#include <camera_calibration_parsers/parse.h>

#include <opencv2/highgui/highgui.hpp> // for cv::imwrite

#include <bag_tools/stereo_bag_processor.h>

class ImageSaver
{
public:
  ImageSaver(const std::string& save_dir, const std::string& filetype, 
      const std::string& prefix) :
    save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0)
  {}

  void save(const sensor_msgs::Image::ConstPtr &img, 
            const sensor_msgs::CameraInfo::ConstPtr &info)
  {
    camera_model_.fromCameraInfo(info);
    std::string calib_filename_ini = 
      boost::str(boost::format("%s/calibration_%scamera.ini") 
          % save_dir_ % prefix_ );
    std::string calib_filename_yaml = 
      boost::str(boost::format("%s/calibration_%scamera.yaml") 
          % save_dir_ % prefix_ );
    camera_calibration_parsers::writeCalibration(calib_filename_yaml, prefix_ + "camera", *info);
    image_proc::ImageSet output;
    if (!processor_.process(img, camera_model_, output, image_proc::Processor::RECT_COLOR))
    {
      std::cerr << "ERROR Processing image" << std::endl;
      return;
    }
    std::string filename = 
      boost::str(boost::format("%s/%s%lu.%s") 
          % save_dir_ 
          % prefix_ 
          % img->header.stamp.toNSec() 
          % filetype_);
    if (!cv::imwrite(filename, output.rect_color))
      ROS_ERROR_STREAM("ERROR Saving " << filename);
    else
    {
      ROS_DEBUG_STREAM("Saved " << filename);
      num_saved_++;
    }
  }

private:

  image_proc::Processor processor_;
  image_geometry::PinholeCameraModel camera_model_;
  std::string save_dir_;
  std::string filetype_;
  std::string prefix_;
  int num_saved_;

};

class StereoImageSaver
{
public:
  StereoImageSaver(const std::string& save_dir, const std::string& filetype) :
    l_saver_(save_dir, filetype, "left_"),
    r_saver_(save_dir, filetype, "right_")
  {}

  void save(const sensor_msgs::Image::ConstPtr &l_img, 
            const sensor_msgs::Image::ConstPtr &r_img, 
            const sensor_msgs::CameraInfo::ConstPtr &l_info,
            const sensor_msgs::CameraInfo::ConstPtr &r_info)
  {
    l_saver_.save(l_img, l_info);
    r_saver_.save(r_img, r_info);
  }

private:

  ImageSaver l_saver_, r_saver_;

};
 
int main(int argc, char** argv)
{
  if (argc < 4)
  {
    std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE STEREO_BASE_TOPIC BAGFILE [BAGFILE...]" << std::endl;
    std::cout << "  Example: " << argv[0] << " /tmp jpg /stereo_down bag1.bag bag2.bag" << std::endl;
    return 0;
  }

  std::string out_dir(argv[1]);
  std::string filetype(argv[2]);
  std::string base_topic(argv[3]);
  
  ros::Time::init();

  StereoImageSaver saver(out_dir, filetype);
  bag_tools::StereoBagProcessor processor(base_topic);
  processor.registerCallback(boost::bind(&StereoImageSaver::save, saver, _1, _2, _3, _4));

  for (int i = 4; i < argc; ++i)
    processor.processBag(argv[i]);

  return 0;
}