00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042 #include <ros/ros.h>
00043 #include "opencv_apps/nodelet.h"
00044 #include <image_transport/image_transport.h>
00045 #include <cv_bridge/cv_bridge.h>
00046 #include <sensor_msgs/image_encodings.h>
00047
00048 #include <opencv2/objdetect/objdetect.hpp>
00049 #include <opencv2/highgui/highgui.hpp>
00050 #include <opencv2/imgproc/imgproc.hpp>
00051
00052 #include <dynamic_reconfigure/server.h>
00053 #include "opencv_apps/FaceDetectionConfig.h"
00054 #include "opencv_apps/Face.h"
00055 #include "opencv_apps/FaceArray.h"
00056 #include "opencv_apps/FaceArrayStamped.h"
00057
00058 namespace face_detection {
00059 class FaceDetectionNodelet : public opencv_apps::Nodelet
00060 {
00061 image_transport::Publisher img_pub_;
00062 image_transport::Publisher face_img_pub_;
00063 image_transport::Subscriber img_sub_;
00064 image_transport::CameraSubscriber cam_sub_;
00065 ros::Publisher msg_pub_;
00066
00067 boost::shared_ptr<image_transport::ImageTransport> it_;
00068
00069 typedef face_detection::FaceDetectionConfig Config;
00070 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00071 Config config_;
00072 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00073
00074 bool debug_view_;
00075 ros::Time prev_stamp_;
00076
00077 cv::CascadeClassifier face_cascade_;
00078 cv::CascadeClassifier eyes_cascade_;
00079
00080 void reconfigureCallback(Config &new_config, uint32_t level)
00081 {
00082 config_ = new_config;
00083 }
00084
00085 const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
00086 {
00087 if (frame.empty())
00088 return image_frame;
00089 return frame;
00090 }
00091
00092 void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00093 {
00094 do_work(msg, cam_info->header.frame_id);
00095 }
00096
00097 void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00098 {
00099 do_work(msg, msg->header.frame_id);
00100 }
00101
00102 void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00103 {
00104
00105 try
00106 {
00107
00108 cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
00109
00110
00111 opencv_apps::FaceArrayStamped faces_msg;
00112 faces_msg.header = msg->header;
00113
00114
00115 std::vector<cv::Rect> faces;
00116 cv::Mat frame_gray;
00117
00118 if ( frame.channels() > 1 ) {
00119 cv::cvtColor( frame, frame_gray, cv::COLOR_BGR2GRAY );
00120 } else {
00121 frame_gray = frame;
00122 }
00123 cv::equalizeHist( frame_gray, frame_gray );
00124
00125 #ifndef CV_VERSION_EPOCH
00126 face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30) );
00127 #else
00128 face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
00129 #endif
00130
00131 cv::Mat face_image;
00132 if (faces.size() > 0) {
00133 cv::Rect max_area = faces[0];
00134 for ( size_t i = 0; i < faces.size(); i++ ) {
00135 if (max_area.width * max_area.height > faces[i].width * faces[i].height) {
00136 max_area = faces[i];
00137 }
00138 }
00139 face_image = frame(max_area).clone();
00140 }
00141
00142 for( size_t i = 0; i < faces.size(); i++ )
00143 {
00144 cv::Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 );
00145 cv::ellipse( frame, center, cv::Size( faces[i].width/2, faces[i].height/2), 0, 0, 360, cv::Scalar( 255, 0, 255 ), 2, 8, 0 );
00146 opencv_apps::Face face_msg;
00147 face_msg.face.x = center.x;
00148 face_msg.face.y = center.y;
00149 face_msg.face.width = faces[i].width;
00150 face_msg.face.height = faces[i].height;
00151
00152 cv::Mat faceROI = frame_gray( faces[i] );
00153 std::vector<cv::Rect> eyes;
00154
00155
00156 #ifndef CV_VERSION_EPOCH
00157 eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30) );
00158 #else
00159 eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
00160 #endif
00161
00162 for( size_t j = 0; j < eyes.size(); j++ )
00163 {
00164 cv::Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 );
00165 int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
00166 cv::circle( frame, eye_center, radius, cv::Scalar( 255, 0, 0 ), 3, 8, 0 );
00167
00168 opencv_apps::Rect eye_msg;
00169 eye_msg.x = eye_center.x;
00170 eye_msg.y = eye_center.y;
00171 eye_msg.width = eyes[j].width;
00172 eye_msg.height = eyes[j].height;
00173 face_msg.eyes.push_back(eye_msg);
00174 }
00175
00176 faces_msg.faces.push_back(face_msg);
00177 }
00178
00179 if( debug_view_) {
00180 cv::imshow( "Face detection", frame );
00181 int c = cv::waitKey(1);
00182 }
00183
00184
00185 sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg();
00186 img_pub_.publish(out_img);
00187 msg_pub_.publish(faces_msg);
00188 if (faces.size() > 0) {
00189 sensor_msgs::Image::Ptr out_face_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, face_image).toImageMsg();
00190 face_img_pub_.publish(out_face_img);
00191 }
00192 }
00193 catch (cv::Exception &e)
00194 {
00195 NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00196 }
00197
00198 prev_stamp_ = msg->header.stamp;
00199 }
00200
00201 void subscribe()
00202 {
00203 NODELET_DEBUG("Subscribing to image topic.");
00204 if (config_.use_camera_info)
00205 cam_sub_ = it_->subscribeCamera("image", 3, &FaceDetectionNodelet::imageCallbackWithInfo, this);
00206 else
00207 img_sub_ = it_->subscribe("image", 3, &FaceDetectionNodelet::imageCallback, this);
00208 }
00209
00210 void unsubscribe()
00211 {
00212 NODELET_DEBUG("Unsubscribing from image topic.");
00213 img_sub_.shutdown();
00214 cam_sub_.shutdown();
00215 }
00216
00217 public:
00218 virtual void onInit()
00219 {
00220 Nodelet::onInit();
00221 it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
00222
00223 pnh_->param("debug_view", debug_view_, false);
00224 if (debug_view_ ) {
00225 always_subscribe_ = true;
00226 }
00227 prev_stamp_ = ros::Time(0, 0);
00228
00229 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00230 dynamic_reconfigure::Server<Config>::CallbackType f =
00231 boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
00232 reconfigure_server_->setCallback(f);
00233
00234 img_pub_ = advertiseImage(*pnh_, "image", 1);
00235 face_img_pub_ = advertiseImage(*pnh_, "face_image", 1);
00236 msg_pub_ = advertise<opencv_apps::FaceArrayStamped>(*pnh_, "faces", 1);
00237
00238 std::string face_cascade_name, eyes_cascade_name;
00239 pnh_->param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
00240 pnh_->param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
00241
00242 if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
00243 if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };
00244
00245 onInitPostProcess();
00246 }
00247 };
00248 }
00249
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS(face_detection::FaceDetectionNodelet, nodelet::Nodelet);