dosegment_node.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <cstdlib>
00003 #include <cstdio>
00004 #include <vector>
00005 #include "pyra/tpimageutil.h"
00006 
00007 #include "fgbgsegment.h"
00008 #include "timercpu.h"
00009 
00010 
00011 #include <ros/ros.h>
00012 #include <sensor_msgs/Image.h>
00013 #include <stereo_msgs/DisparityImage.h>
00014 
00021 
00022 class ActiveRealTimeSegmenter
00023 {
00024 public:
00026   ActiveRealTimeSegmenter():
00027     nh_("")
00028   {
00029   }
00030   
00032   ~ ActiveRealTimeSegmenter()
00033   {
00034   }
00035 
00037 
00040   void execute( int loops );
00041   
00042 private:
00043 
00045 
00050   bool assembleSensorData(sensor_msgs::Image::ConstPtr &recent_image,
00051                           stereo_msgs::DisparityImage::ConstPtr &recent_disparity_image,
00052                           ros::Duration time_out);
00053 
00055   ros::NodeHandle nh_;
00056 };
00057 
00058 bool ActiveRealTimeSegmenter::assembleSensorData(sensor_msgs::Image::ConstPtr &recent_image,
00059                                                  stereo_msgs::DisparityImage::ConstPtr &recent_disparity_image,
00060                                                  ros::Duration time_out)
00061 {
00062   ROS_INFO("Segmentation through User Interaction: waiting for messages...");
00063   std::string image_topic("/narrow_stereo/left/image_rect");
00064   std::string disparity_image_topic("/narrow_stereo_textured/disparity");
00065   
00066   ros::Time start_time = ros::Time::now();
00067   while (!recent_image || !recent_disparity_image )
00068     {
00069       if (!recent_image)
00070         {
00071           ROS_INFO_STREAM("  Waiting for message on topic " << image_topic);
00072           recent_image = ros::topic::waitForMessage<sensor_msgs::Image>(image_topic, nh_, ros::Duration(0.5));
00073         }
00074       if (!recent_disparity_image)
00075         {
00076           ROS_INFO_STREAM("  Waiting for message on topic " << disparity_image_topic);
00077           recent_disparity_image = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00078             (disparity_image_topic, nh_, ros::Duration(0.5));
00079         }
00080       
00081       ros::Time current_time = ros::Time::now();
00082       if (time_out >= ros::Duration(0) && current_time - start_time >= time_out)
00083         {
00084           ROS_INFO("Timed out");
00085           return false;
00086         }
00087     }
00088   return true;
00089 }
00090 
00091 void ActiveRealTimeSegmenter::execute( int loops )
00092 {
00093   // Read number of loops
00094   int fstidx = 0;
00095   int lstidx = loops;
00096 
00097   //Getting ROS messages 
00098   
00099   ros::NodeHandle nh_;
00100   
00101   // The image and disparity image
00102   sensor_msgs::Image::ConstPtr ros_image;
00103   stereo_msgs::DisparityImage::ConstPtr disparity_image;
00104   
00105   if(assembleSensorData(ros_image, disparity_image,ros::Duration(5.0)))
00106     ROS_INFO("All required messages received");
00107   else 
00108     ROS_INFO("Could not gather all required messages.");
00109 
00110   int w = ros_image->width;
00111   int h = ros_image->height;
00112 
00113   ROS_ASSERT(w==disparity_image->image.width && h==disparity_image->image.height);
00114 
00115   int drange = (int)(disparity_image->max_disparity-disparity_image->min_disparity);
00116   std::cout << "Disparity Range " << drange << std::endl;
00117 
00118   /* Initialise Segmenter with width and height of images, disparity range and 
00119    * gradient cost
00120    */
00121   FgBgSegment fgbgsegment(w, h, drange, 50.0);
00122 #ifdef USE_CUDA
00123   fgbgsegment.UseGPU(true);
00124 #else 
00125   fgbgsegment.UseGPU(false);
00126 #endif //USE_CUDA
00127   Image<uint8_t> image(3*w, h);
00128   Image<float> disparities(w, h);
00129   
00130   /* Since PR2 got only monochrome narrow stereo images
00131    * make RGB image with all three channels having the same value.
00132    * Therefore, last argument set to true. For true RGB or single 
00133    * channel images, set to false.
00134    */ 
00135   image.SetDataAlign(*ros_image, 3*w, h, true);
00136   disparities.SetDataAlign(disparity_image->image, w, h);
00137   
00138   // Just for debugging
00139   // image.StoreRGB("CurrentImage.pgm");
00140   // disparities.Store("CurrentDisparityImage.pgm");
00141   
00142   for (int i=fstidx;i<=lstidx;i++) {
00143     char name[80];
00144     
00145     TimerCPU timer(2800);
00146     // number of iterations for belief propagation
00147     int n_iter = 5;
00148     /* Segmentation is executed on RGB image and disparity for n_iter iterations.
00149      * If third argument is true, segmentation is initialised.
00150      */
00151     fgbgsegment.Execute(image, disparities, (i==fstidx), n_iter);
00152     if (i==fstidx) {
00153       /* Default KTH: Initial Foreground in image center due to gaze shifts and fixation mechanism */
00154       //      fgbgsegment.SetNewForeground(w/2, h/2, disparities, drange);
00155       /* New foreground object is initialised around specific point (here (400,100)). */
00156       fgbgsegment.SetNewForeground(400, 100, disparities, drange);
00157     }
00158     std::cout << "Loop time: " << timer.read() << " ms" << std::endl;
00159 
00160     // Helper function to monitor segmentation and store intermediate results.
00161     Image<uint8_t> segm(w, h);
00162     fgbgsegment.MakeSegmentImage(segm);
00163     sprintf(name, "segm%04dx.pgm", i);
00164     segm.Store(name, true, false); 
00165     Image<uint8_t> border(3*w, h);
00166     border = image;
00167     fgbgsegment.MakeBorderImage(border);
00168     sprintf(name, "segm%04dx.ppm", i);
00169     border.StoreRGB(name);  
00170     Image<uint8_t> mask(w, h);
00171     fgbgsegment.MakeMaskImage(mask, 255, 1);
00172     sprintf(name, "mask%04dx.pgm", i);
00173     mask.Store(name);
00174   }
00175 
00176 }
00177 
00178 int main(int argc, char **argv)
00179 {
00180   if (argc<2) {
00181     std::cerr << "Usage: " << argv[0] << " <loops>" << std::endl;
00182     std::cout << "   Ex:" << argv[0] << "  10" << std::endl;
00183     return 1;
00184   }
00185 
00186   int loops = atoi(argv[1]);
00187 
00188   ros::init(argc, argv, "active_realtime_segmentation_node");
00189   ActiveRealTimeSegmenter node;
00190   node.execute(loops);
00191   return 0;
00192 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


active_realtime_segmentation
Author(s): Mårten Björkman. Maintained by Jeannette Bohg
autogenerated on Wed Jan 23 2013 15:46:43