extract_stereo_images.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 #include <ros/ros.h>
00030 
00031 #include <boost/format.hpp>
00032 
00033 #include <sensor_msgs/Image.h>
00034 #include <sensor_msgs/CameraInfo.h>
00035 
00036 #include <image_proc/processor.h>
00037 #include <camera_calibration_parsers/parse.h>
00038 
00039 #include <opencv2/highgui/highgui.hpp> // for cv::imwrite
00040 
00041 #include <bag_tools/stereo_bag_processor.h>
00042 
00046 class ImageSaver
00047 {
00048 public:
00049   ImageSaver(const std::string& save_dir, const std::string& filetype, 
00050       const std::string& prefix) :
00051     save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0)
00052   {}
00053 
00054   void save(const sensor_msgs::Image::ConstPtr &img, 
00055             const sensor_msgs::CameraInfo::ConstPtr &info)
00056   {
00057     camera_model_.fromCameraInfo(info);
00058     std::string calib_filename_ini = 
00059       boost::str(boost::format("%s/calibration_%scamera.ini") 
00060           % save_dir_ % prefix_ );
00061     std::string calib_filename_yaml = 
00062       boost::str(boost::format("%s/calibration_%scamera.yaml") 
00063           % save_dir_ % prefix_ );
00064     camera_calibration_parsers::writeCalibration(calib_filename_yaml, prefix_ + "camera", *info);
00065     image_proc::ImageSet output;
00066     if (!processor_.process(img, camera_model_, output, image_proc::Processor::RECT_COLOR))
00067     {
00068       std::cerr << "ERROR Processing image" << std::endl;
00069       return;
00070     }
00071     std::string filename = 
00072       boost::str(boost::format("%s/%s%lu.%s") 
00073           % save_dir_ 
00074           % prefix_ 
00075           % img->header.stamp.toNSec() 
00076           % filetype_);
00077     if (!cv::imwrite(filename, output.rect_color))
00078       ROS_ERROR_STREAM("ERROR Saving " << filename);
00079     else
00080     {
00081       ROS_DEBUG_STREAM("Saved " << filename);
00082       num_saved_++;
00083     }
00084   }
00085 
00086 private:
00087 
00088   image_proc::Processor processor_;
00089   image_geometry::PinholeCameraModel camera_model_;
00090   std::string save_dir_;
00091   std::string filetype_;
00092   std::string prefix_;
00093   int num_saved_;
00094 
00095 };
00096 
00097 class StereoImageSaver
00098 {
00099 public:
00100   StereoImageSaver(const std::string& save_dir, const std::string& filetype) :
00101     l_saver_(save_dir, filetype, "left_"),
00102     r_saver_(save_dir, filetype, "right_")
00103   {}
00104 
00105   void save(const sensor_msgs::Image::ConstPtr &l_img, 
00106             const sensor_msgs::Image::ConstPtr &r_img, 
00107             const sensor_msgs::CameraInfo::ConstPtr &l_info,
00108             const sensor_msgs::CameraInfo::ConstPtr &r_info)
00109   {
00110     l_saver_.save(l_img, l_info);
00111     r_saver_.save(r_img, r_info);
00112   }
00113 
00114 private:
00115 
00116   ImageSaver l_saver_, r_saver_;
00117 
00118 };
00119  
00120 int main(int argc, char** argv)
00121 {
00122   if (argc < 4)
00123   {
00124     std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE STEREO_BASE_TOPIC BAGFILE [BAGFILE...]" << std::endl;
00125     std::cout << "  Example: " << argv[0] << " /tmp jpg /stereo_down bag1.bag bag2.bag" << std::endl;
00126     return 0;
00127   }
00128 
00129   std::string out_dir(argv[1]);
00130   std::string filetype(argv[2]);
00131   std::string base_topic(argv[3]);
00132   
00133   ros::Time::init();
00134 
00135   StereoImageSaver saver(out_dir, filetype);
00136   bag_tools::StereoBagProcessor processor(base_topic);
00137   processor.registerCallback(boost::bind(&StereoImageSaver::save, saver, _1, _2, _3, _4));
00138 
00139   for (int i = 4; i < argc; ++i)
00140     processor.processBag(argv[i]);
00141 
00142   return 0;
00143 }
00144 


bag_tools
Author(s): Stephan Wirth , Miquel Massot
autogenerated on Fri Aug 28 2015 13:15:13