00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #define CV_NO_BACKWARD_COMPATIBILITY
00020
00021 #include "cv.h"
00022 #include "highgui.h"
00023
00024 #include <iostream>
00025 #include <cstdio>
00026
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
00035
00036 #ifdef YARP
00037 #include <yarp/os/all.h>
00038 #include <yarp/sig/all.h>
00039
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
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
00143 }
00144
00145 if( !image_.empty() )
00146 {
00147 detect_and_draw( image_, cascade_, nested_cascade_, scale_ );
00148 waitKey(3);
00149 }
00150
00151
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
00180
00181
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
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
00215
00216
00217 smallImgROI = smallImg(*r);
00218
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
00228
00229
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 }
