camshiftdemo.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <jsk_topic_tools/log_utils.h>
00003 #include <geometry_msgs/PolygonStamped.h>
00004 #include <dynamic_reconfigure/server.h>
00005 #include <jsk_perception/camshiftdemoConfig.h>
00006 #include <jsk_recognition_msgs/RotatedRectStamped.h>
00007 #include <sensor_msgs/SetCameraInfo.h>
00008 
00009 // opencv/samples/cp/camshiftdemo.c
00010 
00011 #include <sensor_msgs/Image.h>
00012 #include <sensor_msgs/fill_image.h>
00013 
00014 #include <image_transport/image_transport.h>
00015 #include <cv_bridge/cv_bridge.h>
00016 
00017 #include <opencv2/video/tracking.hpp>
00018 #include <opencv2/imgproc/imgproc.hpp>
00019 #include <opencv2/highgui/highgui.hpp>
00020 
00021 #include <iostream>
00022 #include <ctype.h>
00023 
00024 using namespace cv;
00025 using namespace std;
00026 
00027 class CamShiftDemo
00028 {
00029 protected:
00030   ros::NodeHandle nh_;
00031   image_transport::ImageTransport it_;
00032   image_transport::Subscriber sub_;
00033   image_transport::Publisher pub_, pub_hist_;
00034   sensor_msgs::Image img_;
00035   ros::Subscriber sub_rectangle_;
00036   ros::Publisher pub_result_;
00037   ros::ServiceServer roi_service_;
00038   int max_queue_size_;
00039   bool debug_;
00040  
00041   typedef jsk_perception::camshiftdemoConfig Config;
00042   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00043   ReconfigureServer reconfigure_server_;
00044   Config config_;
00045 
00046   Mat image_;
00047   Mat hsv_, hue_, mask_, histimg_, backproj_;
00048 
00049   // opencv 2.1 uses MatND which is removed in 2.2
00050   // this code will be removend in D-Turtle
00051 #if (CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION < 2)
00052   MatND hist_;
00053 #else
00054   Mat hist_;
00055 #endif
00056 
00057   bool backprojMode_;
00058   bool selectObject_;
00059   int trackObject_;
00060   bool showHist_;
00061   Point origin_;
00062   Rect selection_;
00063   int vmin_, vmax_, smin_;
00064 
00065   Rect trackWindow_;
00066   RotatedRect trackBox_;
00067   int hsize_;
00068   float hranges_[2];
00069   const float* phranges_;
00070 
00071   typedef struct {
00072     Mat *image;
00073     bool *selectObject;
00074     int *trackObject;
00075     Rect *selection;
00076     Point *origin;
00077   } on_mouse_param_type;
00078   on_mouse_param_type on_mouse_param_;
00079 
00080 public:
00081   CamShiftDemo(ros::NodeHandle nh)
00082     :  it_(nh)
00083   {
00084     // ros setup
00085     ros::NodeHandle local_nh("~");
00086     local_nh.param("max_queue_size", max_queue_size_, 3);
00087     local_nh.param("debug", debug_, true);
00088 
00089     image_transport::ImageTransport it(nh);
00090     sub_ = it.subscribe(nh.resolveName("image"), 1, &CamShiftDemo::imageCB, this);
00091     pub_ = it.advertise(local_nh.resolveName("image"), 1);
00092     pub_hist_ = it.advertise(local_nh.resolveName("histimg"), 1);
00093 
00094     sub_rectangle_ = nh.subscribe(nh.resolveName("screenrectangle"), 1, &CamShiftDemo::setRectangleCB, this);
00095     pub_result_ = nh.advertise<jsk_recognition_msgs::RotatedRectStamped>(local_nh.resolveName("result"), 1);
00096 
00097     //roi_service_ = local_nh.advertiseService("set_roi", sensor_msgs::SetCameraInfo);
00098     roi_service_ = local_nh.advertiseService("set_roi", &CamShiftDemo::setROICb, this);
00099     
00100     // camshiftdemo.cpp
00101     backprojMode_ = false;
00102     selectObject_ = false;
00103     trackObject_ = 0;
00104     showHist_ = true;
00105     vmin_ = 10; vmax_ = 256; smin_ = 30;
00106 
00107     hsize_ = 16;
00108     hranges_[0] = 0; hranges_[1] = 180;
00109     phranges_ = hranges_;
00110 
00111     // Set up reconfigure server
00112     config_.vmin = vmin_;
00113     config_.vmax = vmax_;
00114     config_.smin = smin_;
00115     ReconfigureServer::CallbackType f = boost::bind(&CamShiftDemo::configCb, this, _1, _2);
00116     reconfigure_server_.setCallback(f);
00117 
00118 
00119     on_mouse_param_.image = &image_;
00120     on_mouse_param_.selectObject = &selectObject_;
00121     on_mouse_param_.trackObject = &trackObject_;
00122     on_mouse_param_.selection = &selection_;
00123     on_mouse_param_.origin = &origin_;
00124 
00125     if ( debug_ )
00126       {
00127         namedWindow( "Histogram", 1 );
00128         namedWindow( "CamShift Demo", 1 );
00129 
00130         setMouseCallback( "CamShift Demo", onMouse, (void *)&on_mouse_param_ );
00131         createTrackbar( "Vmin", "CamShift Demo", &vmin_, 256, 0 );
00132         createTrackbar( "Vmax", "CamShift Demo", &vmax_, 256, 0 );
00133         createTrackbar( "Smin", "CamShift Demo", &smin_, 256, 0 );
00134 
00135         histimg_ = Mat::zeros(200, 320, CV_8UC3);
00136       }
00137 
00138     if ( ! debug_ )
00139       {
00140         startWindowThread();
00141       }
00142   }
00143 
00144   ~CamShiftDemo()
00145   {
00146   }
00147 
00148   bool setROICb(sensor_msgs::SetCameraInfo::Request &req,
00149                 sensor_msgs::SetCameraInfo::Response &res)
00150   {
00151 
00152     onMouse(CV_EVENT_LBUTTONDOWN,
00153             req.camera_info.roi.x_offset,
00154             req.camera_info.roi.y_offset,
00155             0,
00156             &on_mouse_param_);
00157     onMouse(CV_EVENT_LBUTTONUP,
00158             req.camera_info.roi.x_offset + req.camera_info.roi.width,
00159             req.camera_info.roi.y_offset + req.camera_info.roi.height,
00160             0,
00161             &on_mouse_param_);
00162     res.success = true;
00163     return true;
00164   }
00165 
00166   void imageCB(const sensor_msgs::ImageConstPtr& msg_ptr)
00167   {
00168     cv_bridge::CvImagePtr cv_ptr;
00169     Mat frame;
00170 
00171     try
00172       {
00173         cv_ptr = cv_bridge::toCvCopy(msg_ptr, "bgr8");
00174         frame = cv_ptr->image;
00175       }
00176     catch (cv_bridge::Exception error)
00177       {
00178         ROS_ERROR("error");
00179       }
00180 
00181     frame.copyTo(image_);
00182     cvtColor(image_, hsv_, CV_BGR2HSV);
00183 
00184     if( trackObject_ )
00185       {
00186         int _vmin = vmin_, _vmax = vmax_;
00187 
00188         inRange(hsv_, Scalar(0, smin_, MIN(_vmin,_vmax)),
00189                 Scalar(180, 256, MAX(_vmin, _vmax)), mask_);
00190         int ch[] = {0, 0};
00191         hue_.create(hsv_.size(), hsv_.depth());
00192         mixChannels(&hsv_, 1, &hue_, 1, ch, 1);
00193 
00194         if( trackObject_ < 0 )
00195           {
00196             Mat roi(hue_, selection_), maskroi(mask_, selection_);
00197             calcHist(&roi, 1, 0, maskroi, hist_, 1, &hsize_, &phranges_);
00198             normalize(hist_, hist_, 0, 255, CV_MINMAX);
00199 
00200             trackWindow_ = selection_;
00201             trackObject_ = 1;
00202 
00203             histimg_ = Scalar::all(0);
00204             int binW = histimg_.cols / hsize_;
00205             Mat buf(1, hsize_, CV_8UC3);
00206             for( int i = 0; i < hsize_; i++ )
00207               buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize_), 255, 255);
00208             cvtColor(buf, buf, CV_HSV2BGR);
00209 
00210             for( int i = 0; i < hsize_; i++ )
00211               {
00212                 int val = saturate_cast<int>(hist_.at<float>(i)*histimg_.rows/255);
00213                 rectangle( histimg_, Point(i*binW,histimg_.rows),
00214                            Point((i+1)*binW,histimg_.rows - val),
00215                            Scalar(buf.at<Vec3b>(i)), -1, 8 );
00216               }
00217           }
00218 
00219         try {
00220           calcBackProject(&hue_, 1, 0, hist_, backproj_, &phranges_);
00221           backproj_ &= mask_;
00222           trackBox_ = CamShift(backproj_, trackWindow_,
00223                                           TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
00224 
00225           if( backprojMode_ )
00226             cvtColor( backproj_, image_, CV_GRAY2BGR );
00227           ellipse( image_, trackBox_, Scalar(0,0,255), 3, CV_AA );
00228 
00229           jsk_recognition_msgs::RotatedRectStamped result_msg;
00230           result_msg.header = msg_ptr->header;
00231           result_msg.rect.x = trackBox_.center.x;
00232           result_msg.rect.y = trackBox_.center.y;
00233           result_msg.rect.width = trackBox_.size.width;
00234           result_msg.rect.height = trackBox_.size.height;
00235           result_msg.rect.angle = trackBox_.angle; // degree->rad
00236           pub_result_.publish(result_msg);
00237 
00238         } catch (...) {
00239           ROS_WARN("illegal tracBox = x:%f y:%f width:%f height:%f angle:%f",
00240                    trackBox_.center.x, trackBox_.center.y,
00241                    trackBox_.size.width, trackBox_.size.height,
00242                    trackBox_.angle);
00243         }
00244       }
00245 
00246     if( selectObject_ && selection_.width > 0 && selection_.height > 0 )
00247       {
00248         Mat roi(image_, selection_);
00249         bitwise_not(roi, roi);
00250       }
00251 
00252     img_.header = msg_ptr->header;
00253     fillImage(img_, "bgr8", image_.rows, image_.cols, image_.step, const_cast<uint8_t*>(image_.data));
00254     pub_.publish(img_);
00255     fillImage(img_, "bgr8", histimg_.rows, histimg_.cols, histimg_.step, const_cast<uint8_t*>(histimg_.data));
00256     pub_hist_.publish(img_);
00257 
00258     if ( debug_ )
00259       {
00260         imshow( "CamShift Demo", image_ );
00261         imshow( "Histogram", histimg_ );
00262 
00263         char c = (char)waitKey(10);
00264         switch(c)
00265           {
00266           case 'b':
00267             backprojMode_ = !backprojMode_;
00268             break;
00269           case 'c':
00270             trackObject_ = 0;
00271             histimg_ = Scalar::all(0);
00272             break;
00273           case 'h':
00274             showHist_ = !showHist_;
00275             if( !showHist_ )
00276               destroyWindow( "Histogram" );
00277             else
00278               namedWindow( "Histogram", 1 );
00279             break;
00280           default:
00281             ;
00282           }
00283       }
00284   }
00285 
00286   void setRectangleCB(const geometry_msgs::PolygonStampedConstPtr& msg_ptr)
00287   {
00288     selection_.x = msg_ptr->polygon.points[0].x;
00289     selection_.y = msg_ptr->polygon.points[0].y;
00290     selection_.width  = msg_ptr->polygon.points[1].x - msg_ptr->polygon.points[0].x;
00291     selection_.height = msg_ptr->polygon.points[1].y - msg_ptr->polygon.points[0].y;
00292     selection_ &= Rect(0, 0, image_.cols, image_.rows);
00293     trackObject_ = -1;
00294   }
00295 
00296   void configCb(Config &config, uint32_t level)
00297   {
00298     config_ = config;
00299     vmin_ = config_.vmin;
00300     vmax_ = config_.vmax;
00301     smin_ = config_.smin;
00302     switch ( config_.display_mode )
00303       {
00304       case jsk_perception::camshiftdemo_RGB:
00305         backprojMode_ = false;
00306         break;
00307       case jsk_perception::camshiftdemo_Backproject:
00308         backprojMode_ = true;
00309         break;
00310       }
00311   }
00312 
00313 
00314   static void onMouse( int event, int x, int y, int flags, void* param)
00315   {
00316     on_mouse_param_type* on_mouse_param = (on_mouse_param_type *)param;
00317     Mat* image = on_mouse_param->image;
00318     bool *selectObject = on_mouse_param->selectObject;
00319     int *trackObject = on_mouse_param->trackObject;
00320     Rect *selection = on_mouse_param->selection;
00321     Point *origin = on_mouse_param->origin;
00322 
00323     if( *selectObject )
00324       {
00325         selection->x = MIN(x, origin->x);
00326         selection->y = MIN(y, origin->y);
00327         selection->width = std::abs(x - origin->x);
00328         selection->height = std::abs(y - origin->y);
00329 
00330         *selection &= Rect(0, 0, image->cols, image->rows);
00331       }
00332 
00333     switch( event )
00334       {
00335       case CV_EVENT_LBUTTONDOWN:
00336         *origin = Point(x,y);
00337         *selection = Rect(x,y,0,0);
00338         *selectObject = true;
00339         break;
00340       case CV_EVENT_LBUTTONUP:
00341         *selectObject = false;
00342         if( selection->width > 0 && selection->height > 0 )
00343           *trackObject = -1;
00344         break;
00345       }
00346   }
00347 
00348 };
00349 
00350 
00351 int main(int argc, char** argv)
00352 {
00353   ros::init(argc, argv, "camshiftdemo");
00354   ros::NodeHandle n;
00355   if (n.resolveName("image") == "/image") {
00356     ROS_WARN("%s: image has not been remapped! Typical command-line usage:\n"
00357              "\t$ ./%s image:=<image topic>", argv[0], argv[0]);
00358   }
00359 
00360   CamShiftDemo c(n);
00361 
00362   ros::spin();
00363 
00364   return 0;
00365 }


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23