facedetect.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2010 by Dejan Pangercic <dejan.pangercic@cs.tum.edu>
00003  * 
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  * 
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  * 
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018  
00019 #define CV_NO_BACKWARD_COMPATIBILITY
00020 //opencv
00021 #include "cv.h"
00022 #include "highgui.h"
00023 
00024 #include <iostream>
00025 #include <cstdio>
00026 //ros
00027 #include <ros/ros.h>
00028 #include <ros/node_handle.h>
00029 #include "sensor_msgs/Image.h"
00030 #include "image_transport/image_transport.h"
00031 #include "face_detector_mono/RectArray.h"
00032 #include "cv_bridge/CvBridge.h"
00033 
00034 //yarp
00035 //#define YARP
00036 #ifdef YARP
00037 #include <yarp/os/all.h>
00038 #include <yarp/sig/all.h>
00039 //namespaces
00040 using namespace yarp::os;
00041 using namespace yarp::sig;
00042 using namespace yarp::sig::draw;
00043 using namespace yarp::sig::file;
00044 #endif
00045 
00046 #ifdef _EiC
00047 #define WIN32
00048 #endif
00049 
00050 using namespace std;
00051 using namespace cv;
00052 
00053 class FaceDetect {
00054 
00055 public:
00056   String cascade_name_;
00057   String nested_cascade_name_;
00058   Mat  image_;
00059   String input_image_topic_, port_name_;
00060   CascadeClassifier cascade_, nested_cascade_;
00061   double scale_;
00062   bool display_, yarp_image_;
00063 #ifdef YARP
00064   Network yarp_;
00065   // Make a port for reading and writing images
00066   BufferedPort<Bottle> port_;
00067 #endif
00068   int hsizes_[2];
00069   CvHistogram *start_hist_;
00070   IplImage* hist_image_; 
00071   ros::Time ts_;
00072 
00073   FaceDetect(ros::NodeHandle &n) :
00074     n_(n), it_(n_)
00075   {
00076     n_.param("cascade_name", cascade_name_, std::string(""));
00077     n_.param("nested_cascade_name", nested_cascade_name_, std::string(""));
00078     n_.param("scale", scale_, 1.5);
00079     n_.param("input_image_topic", input_image_topic_, std::string("/yarp_to_ros_image/yarp_to_ros_image"));
00080     n_.param("display", display_, false);
00081     n_.param("yarp_image", yarp_image_, false);
00082     n_.param("port_name", port_name_, std::string("/icub_look_at_head"));
00083     ts_ = ros::Time::now ();
00084 
00085     image_sub_ = it_.subscribe(input_image_topic_, 1, &FaceDetect::image_callback, this);
00086     face_pub_ = n_.advertise<face_detector_mono::RectArray>("faces",1);
00087 
00088     if (cascade_name_ == "" || nested_cascade_name_ == "")
00089       ROS_ERROR("Classification files missing!");
00090     hsizes_[0] = 128, hsizes_[1] = 128;
00091     start_hist_ = cvCreateHist(2, hsizes_, CV_HIST_ARRAY);
00092     hist_image_ = cvCreateImage(cvSize(30,30), IPL_DEPTH_8U, 1 );
00093     
00094     if(display_)
00095     {
00096       cvNamedWindow( "result", 1 );
00097       cvNamedWindow( "smallImg", 1 );
00098     }
00099 
00100 #ifdef YARP    
00101     if(yarp_image_)
00102       {
00103         ConstString yarp_port_name_(port_name_.c_str());
00104         yarp_.init();
00105         port_.open(yarp_port_name_);
00106       }
00107 #endif
00108   }
00109 
00110   ~FaceDetect()
00111     {
00112       if(display_)
00113       {
00114         cvDestroyWindow("result");
00115         cvDestroyWindow("smallImg");
00116       }
00117       cvReleaseImage(&hist_image_);
00118       cvReleaseHist(&start_hist_);
00119     }
00120 
00121   void image_callback(const sensor_msgs::ImageConstPtr& msg_ptr)
00122   {
00123     try
00124     {
00125       image_ = bridge_.imgMsgToCv(msg_ptr, "bgr8");
00126     }
00127     catch (sensor_msgs::CvBridgeException error)
00128     {
00129       ROS_ERROR("error");
00130     }
00131     
00132     if( !nested_cascade_.load( nested_cascade_name_ ) )
00133       cerr << "WARNING: Could not load classifier cascade for nested objects" << endl;
00134     
00135     if( !cascade_.load( cascade_name_ ) )
00136     {
00137       cerr << "ERROR: Could not load classifier cascade" << endl;
00138       cerr << "Usage: facedetect [--cascade=\"<cascade_path>\"]\n"
00139         "   [--nested-cascade[=\"nested_cascade_path\"]]\n"
00140         "   [--scale[=<image scale>\n"
00141         "   [filename|camera_index]\n" ;
00142       //return -1;
00143     }
00144     
00145     if( !image_.empty() )
00146     {
00147       detect_and_draw( image_, cascade_, nested_cascade_, scale_ );
00148       waitKey(3);
00149     }   
00150     
00151     // return 0;
00152   }
00153 
00154   void detect_and_draw( Mat& img,
00155                       CascadeClassifier& cascade, CascadeClassifier& nestedCascade,
00156                       double scale)
00157   {
00158     int i = 0;
00159     double t = 0;
00160     vector<Rect> faces;
00161     face_detector_mono::RectArray faces_msgs;
00162     const static Scalar colors[] =  { CV_RGB(0,0,255),
00163                                       CV_RGB(0,128,255),
00164                                       CV_RGB(0,255,255),
00165                                       CV_RGB(0,255,0),
00166                                       CV_RGB(255,128,0),
00167                                       CV_RGB(255,255,0),
00168                                       CV_RGB(255,0,0),
00169                                       CV_RGB(255,0,255)} ;
00170     Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );
00171     
00172     cvtColor( img, gray, CV_BGR2GRAY );
00173     resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
00174     equalizeHist( smallImg, smallImg );
00175 
00176     t = (double)cvGetTickCount();
00177     cascade.detectMultiScale( smallImg, faces,
00178                               1.1, 5, 0
00179                               //|CV_HAAR_FIND_BIGGEST_OBJECT
00180 
00181                               //|CV_HAAR_DO_ROUGH_SEARCH
00182                               |CV_HAAR_SCALE_IMAGE
00183                               ,
00184                               Size(30, 30) );
00185     t = (double)cvGetTickCount() - t;
00186     ROS_INFO( "detection time = %g ms", t/((double)cvGetTickFrequency()*1000.) );
00187     faces_msgs.header.stamp = ros::Time::now();
00188     faces_msgs.header.frame_id = "";
00189 
00190     for( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ )
00191     {
00192       Mat smallImgROI;
00193       vector<Rect> nestedObjects;
00194       Point center;
00195       Scalar color = colors[i%8];
00196       int radius;
00197       center.x = cvRound((r->x + r->width*0.5)*scale);
00198       center.y = cvRound((r->y + r->height*0.5)*scale);
00199 #ifdef YARP 
00200       //send center coordinates to the icub head
00201       if(yarp_image_)
00202         send_face_center_to_yarp_port(center.x, center.y);
00203 #else
00204       face_detector_mono::Rect rect;
00205       rect.x = center.x;
00206       rect.y = center.y;
00207       rect.width = r->width;
00208       rect.height = r->height;
00209       faces_msgs.rects.push_back(rect);
00210 #endif      
00211       radius = cvRound((r->width + r->height)*0.25*scale);
00212       circle( img, center, radius, color, 3, 8, 0 );
00213       
00214       //cvSetImageROI(smallImg, *r); 
00215       //cvCopy(smallImg, hist_image_);
00216       //cvResetImageROI(smallImg);
00217       smallImgROI = smallImg(*r);
00218       //cvCalcHist(smallImgROI, start_hist_);
00219       cv::imshow( "smallImg", smallImgROI );    
00220       
00221       if( nestedCascade.empty() )
00222         continue;
00223       smallImgROI = smallImg(*r);
00224       nestedCascade.detectMultiScale( smallImgROI, nestedObjects,
00225                                       1.1, 2, 0
00226                                       |CV_HAAR_FIND_BIGGEST_OBJECT
00227                                       //|CV_HAAR_DO_ROUGH_SEARCH
00228                                       //|CV_HAAR_DO_CANNY_PRUNING
00229                                       //|CV_HAAR_SCALE_IMAGE
00230                                       ,
00231                                       Size(30, 30) );
00232       for( vector<Rect>::const_iterator nr = nestedObjects.begin(); nr != nestedObjects.end(); nr++ )
00233       {
00234         center.x = cvRound((r->x + nr->x + nr->width*0.5)*scale);
00235         center.y = cvRound((r->y + nr->y + nr->height*0.5)*scale);
00236         
00237         radius = cvRound((nr->width + nr->height)*0.25*scale);
00238         circle( img, center, radius, color, 3, 8, 0 );
00239       }
00240     }  
00241 #ifndef YARP
00242     reverse(faces_msgs.rects.begin(),faces_msgs.rects.end());
00243     face_pub_.publish(faces_msgs);
00244 #endif
00245     if(display_)
00246       cv::imshow( "result", img );    
00247   }
00248 #ifdef YARP
00249   void send_face_center_to_yarp_port(double x, double y)
00250   {
00251     Bottle& output = port_.prepare();
00252     output.clear();
00253     output.addDouble(x);
00254     output.addDouble(y);
00255     ROS_INFO("Writting center coordinates %s to port %s. Time elapsed since last sending: %g seconds", 
00256              output.toString().c_str(), port_name_.c_str(), (ros::Time::now () - ts_).toSec ());
00257     ts_ = ros::Time::now();
00258     port_.write();
00259   }
00260 #endif
00261 
00262 protected:
00263   
00264   ros::NodeHandle n_;
00265   image_transport::ImageTransport it_;
00266   image_transport::Subscriber image_sub_;
00267   ros::Publisher face_pub_;
00268   sensor_msgs::CvBridge bridge_;
00269 };
00270 
00271 int main(int argc, char** argv)
00272 {
00273   ros::init(argc, argv, "facedetect");
00274   ros::NodeHandle n("~");
00275   FaceDetect f(n);
00276   ros::spin();
00277   return 0;
00278 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


face_detector_mono
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 15:01:52