bgfg_codebook_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/node_handle.h>
00003 #include "sensor_msgs/Image.h"
00004 #include "image_transport/image_transport.h"
00005 #include "cv_bridge/CvBridge.h"
00006 #include <opencv/cv.h>
00007 #include <opencv/highgui.h>
00008 #include "cvaux.h"
00009 #include "cxmisc.h"
00010 #include "highgui.h"
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <ctype.h>
00014 #include <string.h>
00015 
00016 class BGFGCodeBook {
00017 
00018 public:
00019   BGFGCodeBook(ros::NodeHandle &n) :
00020     nh_(n), it_(nh_)
00021   {
00022     
00023     n.param("input_image_topic", input_image_topic_, std::string("/narrow_stereo/left/image_color_rect"));
00024     n.param("modMin", modMin_, 35);
00025     n.param("modMax", modMax_, 35);
00026     n.param("cbBounds_", cbBounds_, 10);
00027     n.param("nframesToLearnBG", nframesToLearnBG_, 10);
00028     n.param("nChannels", nChannels_, 3);
00029     image_sub_ = it_.subscribe(
00030                                input_image_topic_, 1, &BGFGCodeBook::imageCallback, this);
00031 
00032     //VARIABLES for CODEBOOK METHOD:
00033     model_ = 0;
00034     ch_={true,true,true}; // This sets what channels should be adjusted for background bounds
00035     rawImage_ = 0;
00036     yuvImage_ = 0; //yuvImage is for codebook method
00037     ImaskCodeBook_ = 0;
00038     ImaskCodeBookCC_ = 0;
00039     c_ =  n_ = nframes_ = 0;
00040     model_ = cvCreateBGCodeBookModel();
00041     
00042     //Set color thresholds to default values
00043     model_->modMin[0] = modMin_;
00044     model_->modMin[1] = model_->modMin[2] = modMin_;
00045     model_->modMax[0] = modMax_;
00046     model_->modMax[1] = model_->modMax[2] = modMax_;
00047     model_->cbBounds[0] = model_->cbBounds[1] = model_->cbBounds[2] = cbBounds_;
00048 
00049     pause_ = false;
00050     singlestep_ = false;
00051   }
00052 
00053   ~BGFGCodeBook()
00054   {
00055     cvDestroyWindow( "Raw" );
00056     cvDestroyWindow( "ForegroundCodeBook");
00057     cvDestroyWindow( "CodeBook_ConnectComp");
00058   }
00059 
00060   void help(void)
00061   {
00062     printf("\nLearn background and find foreground using simple average and average difference learning method:\n"
00063            "\nUSAGE:\nbgfg_codebook_node _input_image_topic:=/image_topic\n"
00064            "***Keep the focus on the video windows, NOT the console***\n\n"
00065            "INTERACTIVE PARAMETERS:\n"
00066            "\tESC,q,Q  - quit the program\n"
00067            "\th    - print this help\n"
00068            "\tp    - pause toggle\n"
00069            "\ts    - single step\n"
00070            "\tr    - run mode (single step off)\n"
00071            "=== AVG PARAMS ===\n"
00072            "\t-    - bump high threshold UP by 0.25\n"
00073            "\t=    - bump high threshold DOWN by 0.25\n"
00074            "\t[    - bump low threshold UP by 0.25\n"
00075            "\t]    - bump low threshold DOWN by 0.25\n"
00076            "=== CODEBOOK PARAMS ===\n"
00077            "\ty,u,v- only adjust channel 0(y) or 1(u) or 2(v) respectively\n"
00078            "\ta    - adjust all 3 channels at once\n"
00079            "\tb    - adjust both 2 and 3 at once\n"
00080            "\ti,o  - bump upper threshold up,down by 1\n"
00081            "\tk,l  - bump lower threshold up,down by 1\n"
00082            "\tSPACE - reset the model\n"
00083            );
00084   }
00085 
00086   void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr)
00087   {
00088 
00089     IplImage *cv_image = NULL;
00090     try
00091       {
00092         cv_image = bridge_.imgMsgToCv(msg_ptr, "bgr8");
00093       }
00094     catch (sensor_msgs::CvBridgeException error)
00095       {
00096         ROS_ERROR("error");
00097       }
00098 
00099     if( !pause_ )
00100       {
00101         //rawImage = cvQueryFrame( capture );
00102         rawImage_ = cv_image;
00103         ++nframes_;
00104         if(!rawImage_) 
00105           {
00106             ROS_WARN("[BGFGCodeBook:] No image received");
00107             return;
00108           }
00109       }
00110     if( singlestep_ )
00111       pause_ = true;
00112         
00113     //First time:
00114     if( nframes_ == 1 && rawImage_ )
00115       {
00116         ROS_DEBUG("[BGFGCodeBook:] First image received");
00117         // CODEBOOK METHOD ALLOCATION
00118         yuvImage_ = cvCloneImage(rawImage_);
00119         ImaskCodeBook_ = cvCreateImage( cvGetSize(rawImage_), IPL_DEPTH_8U, 1 );
00120         ImaskCodeBookCC_ = cvCreateImage( cvGetSize(rawImage_), IPL_DEPTH_8U, 1 );
00121         cvSet(ImaskCodeBook_,cvScalar(255));
00122             
00123         cvNamedWindow( "Raw", 1 );
00124         cvNamedWindow( "ForegroundCodeBook",1);
00125         cvNamedWindow( "CodeBook_ConnectComp",1);
00126       }
00127 
00128     // If we've got an rawImage and are good to go:                
00129     if( rawImage_ )
00130       {
00131         cvCvtColor( rawImage_, yuvImage_, CV_BGR2YCrCb );//YUV For codebook method
00132         //This is where we build our background model
00133         if( !pause_ && nframes_-1 < nframesToLearnBG_  )
00134           {
00135             cvBGCodeBookUpdate( model_, yuvImage_ );
00136             ROS_DEBUG("[BGFGCodeBook:] Learning BG, nframes: %d", nframes_);
00137           }
00138         if( nframes_-1 == nframesToLearnBG_  )
00139           cvBGCodeBookClearStale( model_, model_->t/2 );
00140             
00141         //Find the foreground if any
00142         if( nframes_-1 >= nframesToLearnBG_  )
00143           {
00144             ROS_DEBUG("[BGFGCodeBook:] Finding FG (modMin: %d , modMax: %d)",model_->modMin[0],model_->modMax[0]);
00145             // Find foreground by codebook method
00146             cvBGCodeBookDiff( model_, yuvImage_, ImaskCodeBook_ );
00147             // This part just to visualize bounding boxes and centers if desired
00148             cvCopy(ImaskCodeBook_,ImaskCodeBookCC_);    
00149             cvSegmentFGMask( ImaskCodeBookCC_ );
00150           }
00151         //Display
00152         cvShowImage( "Raw", rawImage_ );
00153         cvShowImage( "ForegroundCodeBook",ImaskCodeBook_);
00154         cvShowImage( "CodeBook_ConnectComp",ImaskCodeBookCC_);
00155       }
00156     // User input:
00157     c_ = cvWaitKey(10)&0xFF;
00158     c_ = tolower(c_);
00159 
00160     if(c_ == 27 || c_ == 'q')
00161       {
00162         ros::shutdown();
00163         return;
00164       }
00165     //Else check for user input
00166     switch( c_ )
00167       {
00168       case 'h':
00169         help();
00170         break;
00171       case 'p':
00172         pause_ = !pause_;
00173         break;
00174       case 's':
00175         singlestep_ = !singlestep_;
00176         pause_ = false;
00177         break;
00178       case 'r':
00179         pause_ = false;
00180         singlestep_ = false;
00181         break;
00182       case ' ':
00183         cvBGCodeBookClearStale( model_, 0 );
00184         nframes_ = 0;
00185         break;
00186         //CODEBOOK PARAMS
00187       case 'y': case '0':
00188       case 'u': case '1':
00189       case 'v': case '2':
00190       case 'a': case '3':
00191       case 'b': 
00192         ch_[0] = c_ == 'y' || c_ == '0' || c_ == 'a' || c_ == '3';
00193         ch_[1] = c_ == 'u' || c_ == '1' || c_ == 'a' || c_ == '3' || c_ == 'b';
00194         ch_[2] = c_ == 'v' || c_ == '2' || c_ == 'a' || c_ == '3' || c_ == 'b';
00195         printf("CodeBook YUV Channels active: %d, %d, %d\n", ch_[0], ch_[1], ch_[2] );
00196         break;
00197       case 'i': //modify max classification bounds (max bound goes higher)
00198       case 'o': //modify max classification bounds (max bound goes lower)
00199       case 'k': //modify min classification bounds (min bound goes lower)
00200       case 'l': //modify min classification bounds (min bound goes higher)
00201         {
00202           uchar* ptr = c_ == 'i' || c_ == 'o' ? model_->modMax : model_->modMin;
00203           for(n_=0; n_<nChannels_; n_++)
00204             {
00205               if( ch_[n_] )
00206                 {
00207                   int v = ptr[n_] + (c_ == 'i' || c_ == 'l' ? 1 : -1);
00208                   ptr[n_] = CV_CAST_8U(v);
00209                 }
00210               printf("%d,", ptr[n_]);
00211             }
00212           printf(" CodeBook %s Side\n", c_ == 'i' || c_ == 'o' ? "High" : "Low" );
00213         }
00214         break;
00215       }
00216 
00217   }
00218 
00219 
00220 protected:
00221 
00222   ros::NodeHandle nh_;
00223   image_transport::ImageTransport it_;
00224   image_transport::Subscriber image_sub_;
00225   sensor_msgs::CvBridge bridge_;
00226   std::string input_image_topic_;
00227 
00228   //VARIABLES for CODEBOOK METHOD:
00229   CvBGCodeBookModel* model_;
00230   int modMin_, modMax_, cbBounds_;
00231   //int NCHANNELS_;
00232   bool ch_[3]; // This sets what channels should be adjusted for background bounds
00233   IplImage* rawImage_, *yuvImage_; //yuvImage is for codebook method
00234   IplImage *ImaskCodeBook_, *ImaskCodeBookCC_;  
00235   int c_, n_, nframes_, nChannels_;
00236   int nframesToLearnBG_;
00237   bool pause_;
00238   bool singlestep_;
00239 };
00240 
00241 int main(int argc, char** argv)
00242 {
00243   ros::init(argc, argv, "bgfg_codebook");
00244   ros::NodeHandle n("~");
00245   BGFGCodeBook bg(n);
00246   ros::spin();
00247   return 0;
00248 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31