extract_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/image_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::ImageConstPtr &img)
  {
    image_proc::ImageSet output;
    image_geometry::PinholeCameraModel camera_model; // empty, we don't need it here
    if (!processor_.process(img, camera_model, output, image_proc::Processor::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.color))
      ROS_ERROR_STREAM("ERROR Saving " << filename);
    else
    {
      ROS_DEBUG_STREAM("Saved " << filename);
      num_saved_++;
    }
  }

private:

  image_proc::Processor processor_;
  std::string save_dir_;
  std::string filetype_;
  std::string prefix_;
  int num_saved_;
};

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

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

  std::string prefix("image");

  ImageSaver saver(out_dir, filetype, prefix);
  bag_tools::ImageBagProcessor processor(image_topic);
  processor.registerCallback(boost::bind(&ImageSaver::save, saver, _1));

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

  return 0;
}