image_tracking.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <image_transport/image_transport.h>
00003 #include <cv_bridge/cv_bridge.h>
00004 #include <opencv/cv.h>
00005 #include <opencv/highgui.h>
00006 #include <sensor_msgs/image_encodings.h>
00007 #include <dynamic_reconfigure/server.h>
00008 #include <opencv_ros_bridge_tutorial/ImageTrackingConfig.h>
00009 
00010 namespace enc = sensor_msgs::image_encodings;
00011 
00012 class HistogramTracking{
00013 private:
00014   ros::NodeHandle nh_;
00015 
00016   int i, hist_size;
00017   double min_val, max_val;
00018   CvSize dst_size;
00019   CvPoint min_loc, max_loc;
00020 
00021   cv_bridge::CvImagePtr src;
00022   static IplImage *src_img;
00023   static IplImage *tmp_img;// tmp
00024   IplImage *dst_img;
00025   IplImage *src_hsv, *tmp_hsv;
00026   IplImage **src_planes, *tmp_planes[3];
00027 
00028   static CvRect tmp_area_;
00029   static bool is_getting_tmp;
00030 
00031   // ros topic subscriber/publiser initialize
00032   image_transport::ImageTransport it_;
00033   image_transport::Subscriber image_subscriber_;
00034   image_transport::Publisher image_publisher_;
00035   sensor_msgs::Image published_image_;
00036 
00037   //static bool flag;
00038   std::string window_name;
00039   bool autosize;
00040 
00041   int cmp_methods;
00042   int result_r, result_b, result_g;
00043 
00044   float h_ranges[2];
00045 
00046   // parameter
00047   std::string template_filename;
00048 
00049   dynamic_reconfigure::Server<opencv_ros_bridge_tutorial::ImageTrackingConfig> srv;
00050   dynamic_reconfigure::Server<opencv_ros_bridge_tutorial::ImageTrackingConfig>::CallbackType f;
00051   
00052 public:
00053   HistogramTracking() : it_(nh_){
00054     ros::NodeHandle pnh("~");
00055     pnh.param("autosize", autosize, true);
00056     pnh.param("window_name", window_name, std::string("histogram_tracking"));
00057     cvNamedWindow (window_name.c_str(), autosize ? CV_WINDOW_AUTOSIZE : 0);
00058     cvNamedWindow ("template_image", autosize ? CV_WINDOW_AUTOSIZE : 0);
00059 
00060     is_getting_tmp = false;
00061     h_ranges[0] = 0;
00062     h_ranges[1] = 181;
00063     cmp_methods = CV_COMP_CORREL;
00064 
00065     cvSetMouseCallback(window_name.c_str(), &HistogramTracking::mouse_cb, this);
00066 
00067     pnh.param("template_filename", template_filename, std::string(window_name.c_str()));
00068     pnh.param("histogram_size", hist_size, 10);
00069     
00070     std::string image_name = nh_.resolveName("image");
00071     image_subscriber_ = it_.subscribe(image_name, 1, &HistogramTracking::image_cb, this);
00072     std::string out_image_name = nh_.resolveName("out_image");
00073     image_publisher_ = it_.advertise(out_image_name, 1);
00074 
00075     // for dynamic reconfigure
00076     f = boost::bind(&HistogramTracking::dynamic_reconfigure_cb, this, _1, _2);
00077     srv.setCallback(f);
00078 
00079     ROS_INFO("initialize finish");
00080   }
00081 
00082   ~HistogramTracking(){
00083     cvDestroyWindow (window_name.c_str());
00084     cvReleaseImage (&src_img);
00085     cvReleaseImage (&dst_img);
00086     cvReleaseImage (&tmp_img);
00087     cvReleaseImage (&src_hsv);
00088     cvReleaseImage (&tmp_hsv);
00089     for (i = 0; i < 3; i++) {
00090       cvReleaseImage (&src_planes[i]);
00091       cvReleaseImage (&tmp_planes[i]);
00092     }
00093     cvFree (&src_planes);
00094   }
00095 
00096   void image_cb(const sensor_msgs::ImageConstPtr &img){
00097     ROS_INFO("image call back is called");
00098     IplImage buf;
00099     src = cv_bridge::toCvCopy(img, enc::RGB8);
00100     cv::imshow (window_name, src->image);    
00101     buf = src->image;
00102     src_img = &buf;
00103     if (is_getting_tmp) {draw_area();}
00104     //draw_area();
00105 
00106     cvWaitKey(5);
00107 
00108     if ((tmp_img == NULL) || (is_getting_tmp)
00109         || (tmp_area_.width == 0) || (tmp_area_.height == 0)){return;}
00110 
00111     CvHistogram *hist = 0;
00112 
00113     float *ranges[] = { h_ranges };
00114     src_planes = (IplImage **) cvAlloc (sizeof (IplImage *) * 3);
00115     for (i = 0; i < 3; i++) {
00116       src_planes[i] = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 1);
00117       tmp_planes[i] = cvCreateImage (cvGetSize (tmp_img), IPL_DEPTH_8U, 1);
00118     }
00119 
00120     // (1)2つ入力画像(探索画像,テンプレート画像)の色空間を,RGBからHSVに変換
00121     src_hsv = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 3);
00122     tmp_hsv = cvCreateImage (cvGetSize (tmp_img), IPL_DEPTH_8U, 3);
00123     cvCvtColor (src_img, src_hsv, CV_BGR2HSV);
00124     cvCvtColor (tmp_img, tmp_hsv, CV_BGR2HSV);
00125     cvCvtPixToPlane (src_hsv, src_planes[0], src_planes[1], src_planes[2], 0);
00126     cvCvtPixToPlane (tmp_hsv, tmp_planes[0], tmp_planes[1], tmp_planes[2], 0);
00127     // (2)テンプレート画像のヒストグラムを計算
00128     hist = cvCreateHist (1, &hist_size, CV_HIST_ARRAY, ranges, 1);
00129     cvCalcHist (&tmp_planes[0], hist, 0, 0);
00130     // (3)探索画像全体に対して,テンプレートのヒストグラムとの距離(手法に依存)を計算
00131     dst_size = cvSize (src_img->width - tmp_img->width + 1, src_img->height - tmp_img->height + 1);
00132     dst_img = cvCreateImage (dst_size, IPL_DEPTH_32F, 1);
00133     cvCalcBackProjectPatch (src_planes, dst_img, cvGetSize (tmp_img), hist, cmp_methods, 1.0);
00134     cvMinMaxLoc (dst_img, &min_val, &max_val, &min_loc, &max_loc, NULL);
00135 
00136     // (4)テンプレートに対応する位置に矩形を描画
00137     cvRectangle (src_img, max_loc,
00138                  cvPoint (max_loc.x + tmp_img->width,
00139                           max_loc.y + tmp_img->height),
00140                  CV_RGB (result_r, result_g, result_b), 3);
00141 
00142     image_publisher_.publish(src->toImageMsg());
00143   }
00144 
00145   static void set_template(){
00146     ROS_INFO("set_template is called");
00147     cvSetImageROI(src_img, tmp_area_);
00148     cvReleaseImage(&tmp_img);
00149     tmp_img = cvCreateImage(cvGetSize(src_img), IPL_DEPTH_8U, 3);
00150     cvCopy(src_img, tmp_img);
00151     cvResetImageROI(src_img);
00152     cvShowImage("template_image", tmp_img);
00153     cvWaitKey(1);
00154   }
00155 
00156   void draw_area (){
00157     ROS_INFO("draw_area is called");
00158     cvRectangle(src_img,
00159                 cvPoint(tmp_area_.x, tmp_area_.y),
00160                 cvPoint(tmp_area_.x+tmp_area_.width, tmp_area_.y+tmp_area_.height),
00161                 cvScalar(0xff, 0x00, 0x00));
00162   }
00163 
00164   static void mouse_cb(int event, int x, int y, int flags, void *param){
00165     ROS_INFO("mouse call back is called");
00166     switch (event){
00167     case CV_EVENT_MOUSEMOVE:{
00168       if(is_getting_tmp){
00169         tmp_area_.width = x - tmp_area_.x;
00170         tmp_area_.height = y - tmp_area_.y;
00171       }
00172       break;
00173     }
00174     case CV_EVENT_LBUTTONDOWN:{
00175       is_getting_tmp = true;
00176       tmp_area_ = cvRect(x, y, 0, 0);
00177       break;
00178     }
00179     case CV_EVENT_LBUTTONUP:{
00180       is_getting_tmp = false;
00181       if (tmp_area_.width < 0){
00182         tmp_area_.x += tmp_area_.width;
00183         tmp_area_.width *= -1;
00184       }
00185       if (tmp_area_.height < 0){
00186         tmp_area_.y += tmp_area_.height;
00187         tmp_area_.height *= -1;
00188       }
00189         
00190       set_template();
00191       break;
00192     }
00193     }    
00194   }
00195   void dynamic_reconfigure_cb(opencv_ros_bridge_tutorial::ImageTrackingConfig &config,
00196                               uint32_t level){
00197     hist_size = config.hist_size;
00198     h_ranges[0] = config.h_limit_min;
00199     h_ranges[1] = config.h_limit_max;
00200     result_r = config.tracking_result_line_r;
00201     result_b = config.tracking_result_line_b;
00202     result_g = config.tracking_result_line_g;
00203 
00204     switch(config.compare_methods){
00205     case 0:
00206       cmp_methods = CV_COMP_CORREL;
00207       break;
00208     case 1:
00209       cmp_methods = CV_COMP_CHISQR;
00210       break;
00211     case 2:
00212       cmp_methods = CV_COMP_INTERSECT;
00213       break;
00214     case 3:
00215       cmp_methods = CV_COMP_BHATTACHARYYA;
00216       break;
00217     }
00218 
00219   }
00220 };
00221 
00222 CvRect HistogramTracking::tmp_area_ = cvRect(0,0,0,0);
00223 IplImage* HistogramTracking::src_img;
00224 IplImage* HistogramTracking::tmp_img;
00225 bool HistogramTracking::is_getting_tmp;
00226 
00227 int main(int argc, char** argv){
00228   ros::init(argc, argv, "histogram_tracking");
00229   HistogramTracking ht;
00230   ros::spin();
00231   
00232   return 0;
00233 }


opencv_ros_bridge_tutorial
Author(s): Kei Okada
autogenerated on Mon Oct 6 2014 12:08:15