detect_circle_2d.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 <string.h>
00009 #include "math.h"
00010 #include <iostream>
00011 #include <stdio.h>
00012 #include <math.h>
00013 #include <string.h>
00014 
00015 using namespace std;
00016 
00017 class DetectCircle2D 
00018 {
00019 public:
00020   int c_;
00021   int px_[0], py_[0], radius_;
00022   //for CvThreshold
00023   double threshold_, max_value_;
00024   //for CvSmooth
00025   int aperture_width_, aperture_height_;
00026   // for cvHoughCircles
00027   int accumulator_, canny_high_threshold_, accumulator_center_, min_radius_, max_radius_;
00028   int max_circles_;
00029   double sleep_;
00030   std::string image_topic_;
00031   CvMemStorage* cstorage_;
00032   bool hough_circle_;
00033   int ellipse_width_, ellipse_height_;
00034   DetectCircle2D(ros::NodeHandle &n) :
00035     n_(n), it_(n_)
00036   {
00037     px_[0] = 0; py_[0] = 0; radius_ = 0;
00038     n_.param("threshold", threshold_, 100.00);
00039     n_.param("max_value", max_value_, 255.00);
00040     n_.param("aperture_height_", aperture_height_, 9);
00041     n_.param("aperture_width_", aperture_width_, 9);
00042     n_.param("accumulator", accumulator_, 2);
00043     n_.param("canny_high_threshold", canny_high_threshold_, 5);
00044     n_.param("accumulator_center", accumulator_center_, 35);
00045     n_.param("min_radius", min_radius_, 80);
00046     n_.param("max_radius", max_radius_, 150);
00047     n_.param("max_circles", max_circles_, 2);
00048     n_.param("sleep", sleep_, 0.1);
00049     n_.param("image_topic", image_topic_, std::string("/narrow_stereo/right/image_rect"));
00050     n_.param("hough_circle", hough_circle_, false);
00051     n_.param("ellipse_width", ellipse_width_, 80);
00052     n_.param("ellipse_height", ellipse_height_, 80);
00053     image_sub_ = it_.subscribe(
00054                                image_topic_, 1, &DetectCircle2D::imageCallback, this);
00055     ROS_INFO("DetectCircle2D Ready!!!");
00056     cstorage_ = cvCreateMemStorage(0);
00057     if (hough_circle_)
00058     {
00059       cvNamedWindow("src",1);
00060       cvNamedWindow("gray",1);
00061     }
00062     else 
00063     {
00064       cvNamedWindow("Source", 1);
00065       cvNamedWindow("Result", 1);
00066     }
00067   }
00068 
00069   ~DetectCircle2D()
00070   {
00071     //release all windows
00072     cvDestroyAllWindows();
00073   }
00074 
00075   
00076   // Define trackbar callback functon. This function find contours,
00077 // draw it and approximate it by ellipses.
00078   void process_image(int h, IplImage *image02, IplImage *image03, IplImage *image04)
00079     {
00080       CvMemStorage* storage;
00081       CvSeq* contour;
00082       
00083       // Create dynamic structure and sequence.
00084       storage = cvCreateMemStorage(0);
00085       contour = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , storage);
00086       
00087       // Threshold the source image. This needful for cvFindContours().
00088       cvThreshold( image03, image02, threshold_, 255, CV_THRESH_BINARY );
00089       
00090       // Find all contours.
00091       cvFindContours( image02, storage, &contour, sizeof(CvContour),
00092                       CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
00093       
00094       // Clear images. IPL use.
00095       //cvZero(image02);
00096       cvZero(image04);
00097       // This cycle draws all contours and approximates them by ellipses.
00098       int count_contours = 0;
00099       for(;contour;contour = contour->h_next)
00100       {
00101         count_contours++;
00102         int count = contour->total; // This is number of points in contour
00103         CvPoint center;
00104         CvSize size;
00105         CvBox2D box;
00106         
00107         // Number points must be greater or equal 6 (for cvFitEllipse_32f).
00108         if( count < 6 )
00109           continue;
00110         
00111         CvMat* points_f = cvCreateMat( 1, count, CV_32FC2 );
00112         CvMat points_i = cvMat( 1, count, CV_32SC2, points_f->data.ptr );
00113         cvCvtSeqToArray( contour, points_f->data.ptr, CV_WHOLE_SEQ );
00114         cvConvert( &points_i, points_f );
00115         
00116         // Fits ellipse to current contour.
00117         box = cvFitEllipse2( points_f );
00118         
00119         // Draw current contour.
00120         cvDrawContours(image04,contour,CV_RGB(255,255,255),CV_RGB(255,255,255),0,1,8,cvPoint(0,0));
00121         
00122         // Convert ellipse data from float to integer representation.
00123         center = cvPointFrom32f(box.center);
00124         size.width = cvRound(box.size.width*0.5);
00125         size.height = cvRound(box.size.height*0.5);
00126         // Draw ellipse.
00127         if (size.width > ellipse_width_ && size.height > ellipse_height_ && size.width < 2.0 * ellipse_width_ && size.height < 2.0 * ellipse_height_)
00128         //      if (size.width > ellipse_width_ && size.height > ellipse_height_)
00129         {
00130           ROS_INFO_STREAM("size: " << size.width << " " << size.height);
00131           cvEllipse(image03, center, size,
00132                     -box.angle, 0, 360,
00133                     CV_RGB(0,0,0), 1, CV_AA, 0);
00134         }
00135         cvReleaseMat(&points_f);
00136       }
00137       ROS_INFO_STREAM("Number of contours " << count_contours);
00138       
00139       // Show image. HighGUI use.
00140       cvShowImage( "Result", image03 );
00141       //cvShowImage( "Result", image04 );
00142     }
00143 
00144   void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr)
00145     {
00146       ROS_INFO("New image received");
00147       IplImage *cv_image = NULL;
00148       try
00149       {
00150         cv_image = bridge_.imgMsgToCv(msg_ptr, "passthrough");
00151       }
00152       catch (sensor_msgs::CvBridgeException error)
00153       {
00154         ROS_ERROR("error");
00155       }
00156 
00157       if (hough_circle_)
00158       {
00159         IplImage *gray_ = NULL;
00160         gray_=cvCloneImage(cv_image);
00161         //color threshold
00162         cvThreshold(gray_, gray_, threshold_, max_value_, CV_THRESH_BINARY);
00163         //smooth the image to reduce unneccesary results
00164         cvSmooth( gray_, gray_, CV_GAUSSIAN, aperture_width_, aperture_height_);
00165         // cerr <<  accumulator_ << " " << gray_->height/5 << " " << canny_high_threshold_ << " " << accumulator_center_ << " " << min_radius_ << " " << max_radius_ << endl;
00166         //get circles
00167         CvSeq* circles =  cvHoughCircles( gray_, cstorage_, CV_HOUGH_GRADIENT, accumulator_, gray_->height/5, 
00168                                           canny_high_threshold_, accumulator_center_, min_radius_, max_radius_);
00169         //output all the circle detected
00170         ROS_INFO_STREAM("circles total: " << circles->total);
00171         //start drawing all the circles
00172         int i;
00173         for( i = 0; circles->total>=max_circles_?i<max_circles_:i < circles->total; i++ )
00174         { 
00175           //just make a filter to limit only <=2 circles to draw
00176           float* p = (float*)cvGetSeqElem( circles, i );
00177           cvCircle( cv_image, cvPoint(cvRound(p[0]),cvRound(p[1])), 3, CV_RGB(255,0,0), -1, 8, 0 );
00178           cvCircle( cv_image, cvPoint(cvRound(p[0]),cvRound(p[1])), cvRound(p[2]), CV_RGB(200,0,0), 1, 8, 0 );
00179           px_[i]=cvRound(p[0]); py_[i]=cvRound(p[1]); radius_ = cvRound(p[2]);
00180         }
00181         
00182         //output two circles' center position
00183         ROS_INFO_STREAM("center x: "<<px_[0]<<"  center y: "<<py_[0] << " radius: " << radius_);
00184         cvSetImageROI(gray_, cvRect(0,0,cv_image->width,cv_image->height));
00185         cvShowImage("gray",gray_);
00186         cvShowImage("src",cv_image);
00187         //release memory
00188         cvClearSeq(circles);
00189         cvReleaseImage(&gray_);
00190         cvClearMemStorage( cstorage_ );
00191         //ready to exit loop
00192         
00193         c_=cvWaitKey(10);
00194         if(c_==27)
00195           exit(2);
00196         sleep(sleep_);
00197       }
00198       else //fit ellipse
00199       {
00200 //        const char* filename = argc == 2 ? argv[1] : (char*)"stuff.jpg";
00201         
00202         // load image and force it to be grayscale
00203 
00204         
00205         // Create the destination images
00206         IplImage * image02 = cvCloneImage( cv_image );
00207         IplImage * image04 = cvCloneImage( cv_image );
00208         
00209         // Create windows.
00210 //        cvNamedWindow("Source", 1);
00211         //      cvNamedWindow("Result", 1);
00212         
00213         // Show the image.
00214         cvShowImage("Source", cv_image);
00215         
00216         // Create toolbars. HighGUI use.
00217 //        cvCreateTrackbar( "Threshold", "Result", &slider_pos, 255, process_image );
00218         
00219         process_image(0, image02, cv_image, image04);
00220         
00221         // Wait for a key stroke; the same function arranges events processing
00222         //cvWaitKey(0);
00223         c_=cvWaitKey(10);
00224         if(c_==27)
00225           exit(2);
00226         sleep(sleep_);
00227 
00228         cvReleaseImage(&image02);
00229         cvReleaseImage(&image04);
00230         
00231 //        cvDestroyWindow("Source");
00232         //cvDestroyWindow("Result");
00233       }
00234     }
00235   
00236 protected:
00237   ros::NodeHandle n_;
00238   image_transport::ImageTransport it_;
00239   image_transport::Subscriber image_sub_;
00240   sensor_msgs::CvBridge bridge_;
00241 };
00242 
00243 int main(int argc, char** argv)
00244 {
00245   ros::init(argc, argv, "detect_circle2D");
00246   ros::NodeHandle nh("~");
00247   DetectCircle2D dc(nh);
00248   ros::spin();
00249   return 0;
00250 }
 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