roi_bgfg_codebooks.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Florian Zacherl <Florian.Zacherl1860@mytum.de>, Dejan Pangercic <dejan.pangercic@cs.tum.edu>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Intelligent Autonomous Systems Group/
00014  *       Technische Universitaet Muenchen nor the names of its contributors 
00015  *       may be used to endorse or promote products derived from this software 
00016  *       without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <ros/ros.h>
00032 #include <ros/node_handle.h>
00033 #include "sensor_msgs/Image.h"
00034 #include "image_transport/image_transport.h"
00035 #include "cv_bridge/CvBridge.h"
00036 #include <opencv/cv.h>
00037 #include <opencv/highgui.h>
00038 #include "cvaux.h"
00039 #include "cxmisc.h"
00040 #include "highgui.h"
00041 #include <stdio.h>
00042 #include <stdlib.h>
00043 #include <ctype.h>
00044 #include <string.h>
00045 #include "ias_projected_light/cp.h"
00046 
00047 //#define IMAGES
00048 
00049 class BGFGCodeBook
00050 {
00051 
00052 public:
00053   BGFGCodeBook(ros::NodeHandle &n) :
00054     nh_(n), it_(nh_)
00055   {
00056     n.param("input_image_topic", input_image_topic_, std::string("/stereo/left/image_rect"));
00057     n.param("output_topic_", output_topic_, std::string("/stereo/left/roi"));
00058     n.param("modMin", modMin_, 75);
00059     n.param("modMax", modMax_, 75);
00060     n.param("cbBounds_", cbBounds_, 10);
00061     n.param("nframesToLearnBG", nframesToLearnBG_, 10);
00062     n.param("nChannels", nChannels_, 3);
00063     image_sub_ = it_.subscribe(input_image_topic_, 1, &BGFGCodeBook::imageCallback, this);
00064     cl = n.serviceClient<ias_projected_light::cp> ("change_pattern");
00065     pub = n.advertise<sensor_msgs::RegionOfInterest> (output_topic_, 1);
00066 
00067     //VARIABLES for CODEBOOK METHOD:
00068     model_ = 0;
00069     ch_ =
00070     { true,true,true}; // This sets what channels should be adjusted for background bounds
00071         rawImage_ = 0;
00072         yuvImage_ = 0; //yuvImage is for codebook method
00073         ImaskCodeBook_ = 0;
00074         ImaskCodeBookCC_ = 0;
00075         c_ = n_ = nframes_ = 0;
00076         model_ = cvCreateBGCodeBookModel();
00077 
00078         //Set color thresholds to default values
00079         model_->modMin[0] = modMin_;
00080         model_->modMin[1] = model_->modMin[2] = modMin_;
00081         model_->modMax[0] = modMax_;
00082         model_->modMax[1] = model_->modMax[2] = modMax_;
00083         model_->cbBounds[0] = model_->cbBounds[1] = model_->cbBounds[2] = cbBounds_;
00084 
00085         pause_ = false;
00086         singlestep_ = false;
00087 
00088         first_image = true;
00089       }
00090 
00091       ~BGFGCodeBook()
00092       {
00093         cvDestroyWindow( "Raw" );
00094         cvDestroyWindow( "ForegroundCodeBook");
00095         cvDestroyWindow( "CodeBook_ConnectComp");
00096       }
00097 
00098       void help(void)
00099       {
00100         printf("\nLearn background and find foreground using simple average and average difference learning method:\n"
00101             "\nUSAGE:\nbgfg_codebook_node _input_image_topic:=/image_topic\n"
00102             "***Keep the focus on the video windows, NOT the console***\n\n"
00103             "INTERACTIVE PARAMETERS:\n"
00104             "\tESC,q,Q  - quit the program\n"
00105             "\th    - print this help\n"
00106             "\tp    - pause toggle\n"
00107             "\ts    - single step\n"
00108             "\tr    - run mode (single step off)\n"
00109             "=== AVG PARAMS ===\n"
00110             "\t-    - bump high threshold UP by 0.25\n"
00111             "\t=    - bump high threshold DOWN by 0.25\n"
00112             "\t[    - bump low threshold UP by 0.25\n"
00113             "\t]    - bump low threshold DOWN by 0.25\n"
00114             "=== CODEBOOK PARAMS ===\n"
00115             "\ty,u,v- only adjust channel 0(y) or 1(u) or 2(v) respectively\n"
00116             "\ta    - adjust all 3 channels at once\n"
00117             "\tb    - adjust both 2 and 3 at once\n"
00118             "\ti,o  - bump upper threshold up,down by 1\n"
00119             "\tk,l  - bump lower threshold up,down by 1\n"
00120             "\tSPACE - reset the model\n"
00121         );
00122       }
00123 
00124       void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr)
00125       {
00126 
00127         if(first_image) //First image => Black pattern has to be shown
00128 
00129         {
00130           ias_projected_light::cp srv;
00131           srv.request.pattern = 0;
00132           srv.request.block_size = 1;
00133           while (ros::ok() && !cl.call(srv))
00134           {
00135             ROS_INFO("Failed to show black pattern!");
00136             cvWaitKey(50);
00137           }
00138           first_image = false;
00139           cvWaitKey(500);
00140           return;
00141         }
00142 
00143         IplImage *cv_image = NULL;
00144         try
00145         {
00146           cv_image = bridge_.imgMsgToCv(msg_ptr, "bgr8");
00147         }
00148         catch (sensor_msgs::CvBridgeException error)
00149         {
00150           ROS_ERROR("error");
00151         }
00152 
00153         if( !pause_ )
00154         {
00155           //rawImage = cvQueryFrame( capture );
00156           rawImage_ = cv_image;
00157           ++nframes_;
00158           if(!rawImage_)
00159           {
00160             ROS_WARN("[BGFGCodeBook:] No image received");
00161             return;
00162           }
00163         }
00164         if( singlestep_ )
00165         pause_ = true;
00166 
00167         //First time:
00168         if( nframes_ == 1 && rawImage_ )
00169         {
00170           ROS_DEBUG("[BGFGCodeBook:] First image received");
00171           // CODEBOOK METHOD ALLOCATION
00172           yuvImage_ = cvCloneImage(rawImage_);
00173           ImaskCodeBook_ = cvCreateImage( cvGetSize(rawImage_), IPL_DEPTH_8U, 1 );
00174           ImaskCodeBookCC_ = cvCreateImage( cvGetSize(rawImage_), IPL_DEPTH_8U, 1 );
00175           cvSet(ImaskCodeBook_,cvScalar(255));
00176 #ifdef IMAGES
00177           cvNamedWindow( "Raw", 1 );
00178           cvNamedWindow( "ForegroundCodeBook",1);
00179           cvNamedWindow( "CodeBook_ConnectComp",1);
00180 #endif
00181         }
00182 
00183         // If we've got an rawImage and are good to go:
00184         if( rawImage_ )
00185         {
00186           cvCvtColor( rawImage_, yuvImage_, CV_BGR2YCrCb );//YUV For codebook method
00187           //This is where we build our background model
00188           if( !pause_ && nframes_-1 < nframesToLearnBG_ )
00189           {
00190             cvBGCodeBookUpdate( model_, yuvImage_ );
00191             ROS_DEBUG("[BGFGCodeBook:] Learning BG, nframes: %d", nframes_);
00192           }
00193           if( nframes_-1 == nframesToLearnBG_ )
00194           cvBGCodeBookClearStale( model_, model_->t/2 );
00195 
00196           if(nframes_-1 == nframesToLearnBG_) //Background learning ready => Project white pattern
00197 
00198           {
00199             ias_projected_light::cp srv;
00200             srv.request.pattern = 1;
00201             srv.request.block_size = 1;
00202             while (ros::ok() && !cl.call(srv))
00203             {
00204               ROS_INFO("Failed to show white pattern!");
00205               cvWaitKey(50);
00206             }
00207           }
00208 
00209           //Find the foreground if any
00210           if( nframes_-1 >= nframesToLearnBG_ )
00211           {
00212             ROS_DEBUG("[BGFGCodeBook:] Finding FG (modMin: %d , modMax: %d)",model_->modMin[0],model_->modMax[0]);
00213             // Find foreground by codebook method
00214             cvBGCodeBookDiff( model_, yuvImage_, ImaskCodeBook_ );
00215             // This part just to visualize bounding boxes and centers if desired
00216             cvCopy(ImaskCodeBook_,ImaskCodeBookCC_);
00217             cvSegmentFGMask( ImaskCodeBookCC_ );
00218           }
00219 
00220           CvMemStorage* stor = cvCreateMemStorage(0);
00221           CvSeq* seq = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), stor);
00222 
00223           //Find bounding box:
00224           for (int i = 0; i < ImaskCodeBookCC_->width; i++)
00225           {
00226             for (int j = 0; j < ImaskCodeBookCC_->height; j++)
00227             {
00228               if(cvGet2D(ImaskCodeBookCC_, j, i).val[0] != 0)
00229               {
00230                 CvPoint p;
00231                 p.x = i;
00232                 p.y = j;
00233                 cvSeqPush(seq, &p);
00234               }
00235             }
00236           }
00237 
00238           if(seq->total > 0)
00239           {
00240             CvRect roi = cvBoundingRect(seq);
00241             cvRectangle(ImaskCodeBookCC_, cvPoint(roi.x,roi.y), cvPoint(roi.x+roi.width, roi.y + roi.height), cvScalar(255), 5);
00242             sensor_msgs::RegionOfInterest r;
00243             r.x_offset = roi.x;
00244             r.y_offset = roi.y;
00245             r.height = roi.height;
00246             r.width = roi.width;
00247 
00248             pub.publish(r);
00249 
00250           }
00251 #ifdef IMAGES
00252           //Display
00253           cvShowImage( "Raw", rawImage_ );
00254           cvShowImage( "ForegroundCodeBook",ImaskCodeBook_);
00255           cvShowImage( "CodeBook_ConnectComp",ImaskCodeBookCC_);
00256 #endif
00257 
00258         }
00259 #ifdef IMAGES
00260         // User input:
00261         c_ = cvWaitKey(10)&0xFF;
00262         c_ = tolower(c_);
00263 
00264         if(c_ == 27 || c_ == 'q')
00265         {
00266           ros::shutdown();
00267           return;
00268         }
00269         //Else check for user input
00270         switch( c_ )
00271         {
00272           case 'h':
00273           help();
00274           break;
00275           case 'p':
00276           pause_ = !pause_;
00277           break;
00278           case 's':
00279           singlestep_ = !singlestep_;
00280           pause_ = false;
00281           break;
00282           case 'r':
00283           pause_ = false;
00284           singlestep_ = false;
00285           break;
00286           case ' ':
00287           cvBGCodeBookClearStale( model_, 0 );
00288           nframes_ = 0;
00289           break;
00290           //CODEBOOK PARAMS
00291           case 'y': case '0':
00292           case 'u': case '1':
00293           case 'v': case '2':
00294           case 'a': case '3':
00295           case 'b':
00296           ch_[0] = c_ == 'y' || c_ == '0' || c_ == 'a' || c_ == '3';
00297           ch_[1] = c_ == 'u' || c_ == '1' || c_ == 'a' || c_ == '3' || c_ == 'b';
00298           ch_[2] = c_ == 'v' || c_ == '2' || c_ == 'a' || c_ == '3' || c_ == 'b';
00299           printf("CodeBook YUV Channels active: %d, %d, %d\n", ch_[0], ch_[1], ch_[2] );
00300           break;
00301           case 'i': //modify max classification bounds (max bound goes higher)
00302           case 'o': //modify max classification bounds (max bound goes lower)
00303           case 'k': //modify min classification bounds (min bound goes lower)
00304           case 'l': //modify min classification bounds (min bound goes higher)
00305 
00306           {
00307             uchar* ptr = c_ == 'i' || c_ == 'o' ? model_->modMax : model_->modMin;
00308             for(n_=0; n_<nChannels_; n_++)
00309             {
00310               if( ch_[n_] )
00311               {
00312                 int v = ptr[n_] + (c_ == 'i' || c_ == 'l' ? 1 : -1);
00313                 ptr[n_] = CV_CAST_8U(v);
00314               }
00315               //printf("%d,", ptr[n_]);
00316             }
00317             //printf(" CodeBook %s Side\n", c_ == 'i' || c_ == 'o' ? "High" : "Low" );
00318             ROS_INFO("modMax: %d, modMin: %d", model_->modMax[0], model_->modMin[0]);
00319           }
00320           break;
00321         }
00322 #endif
00323 
00324       }
00325 
00326     protected:
00327 
00328       ros::NodeHandle nh_;
00329       image_transport::ImageTransport it_;
00330       image_transport::Subscriber image_sub_;
00331       ros::Publisher pub;
00332       ros::ServiceClient cl;
00333       sensor_msgs::CvBridge bridge_;
00334       std::string input_image_topic_;
00335       std::string output_topic_;
00336 
00337       //VARIABLES for CODEBOOK METHOD:
00338       CvBGCodeBookModel* model_;
00339       int modMin_, modMax_, cbBounds_;
00340       //int NCHANNELS_;
00341       bool ch_[3]; // This sets what channels should be adjusted for background bounds
00342       IplImage* rawImage_, *yuvImage_; //yuvImage is for codebook method
00343       IplImage *ImaskCodeBook_, *ImaskCodeBookCC_;
00344       int c_, n_, nframes_, nChannels_;
00345       int nframesToLearnBG_;
00346       bool pause_;
00347       bool singlestep_;
00348       bool first_image;
00349     };
00350 
00351 int main(int argc, char** argv)
00352 {
00353   ros::init(argc, argv, "bgfg_codebook");
00354   ros::NodeHandle n;
00355   BGFGCodeBook bg(n);
00356   ros::spin();
00357   return 0;
00358 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


ias_projected_light
Author(s): Florian Zacherl, Dejan Pangercic
autogenerated on Thu May 23 2013 17:02:41