process_stereo.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 <sensor_msgs/Image.h>
00032 #include <sensor_msgs/CameraInfo.h>
00033 
00034 #include <stereo_image_proc/processor.h>
00035 #include <cv_bridge/cv_bridge.h>
00036 
00037 #include <bag_tools/stereo_bag_processor.h>
00038 
00039 class StereoImageProcessor
00040 {
00041 public:
00042   StereoImageProcessor(const std::string& base_topic, const std::string& out_bag, int processing_flags) :
00043     stereo_base_topic_(base_topic),
00044     flags_(processing_flags)
00045   {
00046     std::cout << "Opening bagfile '" << out_bag << "' for writing." << std::endl;
00047     bag_.open(out_bag, rosbag::bagmode::Write);
00048   }
00049 
00050   ~StereoImageProcessor()
00051   {
00052     std::cout << "Closing bagfile." << std::endl;
00053     bag_.close();
00054   }
00055 
00056   void process(const sensor_msgs::Image::ConstPtr &l_img, 
00057                const sensor_msgs::Image::ConstPtr &r_img, 
00058                const sensor_msgs::CameraInfo::ConstPtr &l_info,
00059                const sensor_msgs::CameraInfo::ConstPtr &r_info)
00060   {
00061     model_.fromCameraInfo(l_info, r_info);
00062     stereo_image_proc::StereoImageSet image_set;
00063     processor_.process(l_img, r_img, model_, image_set, flags_);
00064 
00065     bag_.write(stereo_base_topic_ + "/left/camera_info",
00066         l_info->header.stamp, l_info);
00067     bag_.write(stereo_base_topic_ + "/right/camera_info",
00068         r_info->header.stamp, r_info);
00069 
00070     if (flags_ & stereo_image_proc::StereoProcessor::LEFT_MONO)
00071     {
00072       sensor_msgs::Image::Ptr msg = createMsg(
00073           l_img->header, 
00074           image_set.left.color_encoding, 
00075           image_set.left.mono);
00076       bag_.write(stereo_base_topic_ + "/left/image_mono", 
00077           l_img->header.stamp, msg);
00078     }
00079     if (flags_ & stereo_image_proc::StereoProcessor::LEFT_COLOR)
00080     {
00081       sensor_msgs::Image::Ptr msg = createMsg(
00082           l_img->header, 
00083           image_set.left.color_encoding, 
00084           image_set.left.color);
00085       bag_.write(stereo_base_topic_ + "/left/image_color", 
00086           l_img->header.stamp, msg);
00087     }
00088     if (flags_ & stereo_image_proc::StereoProcessor::LEFT_RECT)
00089     {
00090       sensor_msgs::Image::Ptr msg = createMsg(
00091           l_img->header, 
00092           image_set.left.color_encoding, 
00093           image_set.left.rect);
00094       bag_.write(stereo_base_topic_ + "/left/image_rect", 
00095           l_img->header.stamp, msg);
00096     }
00097     if (flags_ & stereo_image_proc::StereoProcessor::LEFT_RECT_COLOR)
00098     {
00099       sensor_msgs::Image::Ptr msg = createMsg(
00100           l_img->header, 
00101           image_set.left.color_encoding, 
00102           image_set.left.rect_color);
00103       bag_.write(stereo_base_topic_ + "/left/image_rect_color", 
00104           l_img->header.stamp, msg);
00105     }
00106     if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_MONO)
00107     {
00108       sensor_msgs::Image::Ptr msg = createMsg(
00109           r_img->header, 
00110           image_set.right.color_encoding, 
00111           image_set.right.mono);
00112       bag_.write(stereo_base_topic_ + "/right/image_mono", 
00113           r_img->header.stamp, msg);
00114     }
00115     if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_COLOR)
00116     {
00117       sensor_msgs::Image::Ptr msg = createMsg(
00118           r_img->header, 
00119           image_set.right.color_encoding, 
00120           image_set.right.color);
00121       bag_.write(stereo_base_topic_ + "/right/image_color", 
00122           r_img->header.stamp, msg);
00123     }
00124     if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_RECT)
00125     {
00126       sensor_msgs::Image::Ptr msg = createMsg(
00127           r_img->header, 
00128           image_set.right.color_encoding, 
00129           image_set.right.rect);
00130       bag_.write(stereo_base_topic_ + "/right/image_rect", 
00131           r_img->header.stamp, msg);
00132     }
00133     if (flags_ & stereo_image_proc::StereoProcessor::RIGHT_RECT_COLOR)
00134     {
00135       sensor_msgs::Image::Ptr msg = createMsg(
00136           r_img->header, 
00137           image_set.right.color_encoding, 
00138           image_set.right.rect_color);
00139       bag_.write(stereo_base_topic_ + "/right/image_rect_color", 
00140           r_img->header.stamp, msg);
00141     }
00142 
00143     if (flags_ & stereo_image_proc::StereoProcessor::POINT_CLOUD2)
00144     {
00145       bag_.write(stereo_base_topic_ + "/points2",
00146           l_img->header.stamp, image_set.points2);
00147     }
00148 
00149   }
00150 
00151   sensor_msgs::Image::Ptr createMsg(
00152       const std_msgs::Header& header, 
00153       const std::string& encoding, 
00154       const cv::Mat& img)
00155   {
00156     cv_bridge::CvImage cv_image;
00157     cv_image.header = header;
00158     cv_image.encoding = encoding;
00159     cv_image.image = img;
00160     return cv_image.toImageMsg();
00161   }
00162  
00163 private:
00164 
00165   rosbag::Bag bag_;
00166   std::string stereo_base_topic_;
00167   int flags_;
00168   image_geometry::StereoCameraModel model_;
00169   stereo_image_proc::StereoProcessor processor_;
00170 
00171 };
00172  
00173 int main(int argc, char** argv)
00174 {
00175   if (argc < 4)
00176   {
00177     std::cout << "Takes raw images from an input bagfile, processes them and writes the output to a new bagfile." << std::endl;
00178     std::cout << "Usage: " << argv[0] << " INBAG STEREO_BASE_TOPIC OUTBAG" << std::endl;
00179     std::cout << "  Example: " << argv[0] << " bag.bag /stereo_forward bag_processed.bag" << std::endl;
00180     return 0;
00181   }
00182 
00183   std::string in_bag(argv[1]);
00184   std::string base_topic(argv[2]);
00185   std::string out_bag(argv[3]);
00186   
00187   int flags = 0;
00188   flags |= stereo_image_proc::StereoProcessor::LEFT_RECT_COLOR;
00189   flags |= stereo_image_proc::StereoProcessor::RIGHT_RECT_COLOR;
00190   flags |= stereo_image_proc::StereoProcessor::POINT_CLOUD2;
00191   StereoImageProcessor image_processor(base_topic, out_bag, flags);
00192   bag_tools::StereoBagProcessor bag_processor(base_topic);
00193   bag_processor.registerCallback(boost::bind(&StereoImageProcessor::process, &image_processor, _1, _2, _3, _4));
00194 
00195   bag_processor.processBag(in_bag);
00196 
00197   return 0;
00198 }
00199 


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