face_detection_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, Kei Okada.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Kei Okada nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/objectDetection/objectDetection.cpp
00043 #include <ros/ros.h>
00044 #include "opencv_apps/nodelet.h"
00045 #include <image_transport/image_transport.h>
00046 #include <cv_bridge/cv_bridge.h>
00047 #include <sensor_msgs/image_encodings.h>
00048 
00049 #include <opencv2/objdetect/objdetect.hpp>
00050 #include <opencv2/highgui/highgui.hpp>
00051 #include <opencv2/imgproc/imgproc.hpp>
00052 
00053 #include <dynamic_reconfigure/server.h>
00054 #include "opencv_apps/FaceDetectionConfig.h"
00055 #include "opencv_apps/Face.h"
00056 #include "opencv_apps/FaceArray.h"
00057 #include "opencv_apps/FaceArrayStamped.h"
00058 
00059 namespace opencv_apps
00060 {
00061 class FaceDetectionNodelet : public opencv_apps::Nodelet
00062 {
00063   image_transport::Publisher img_pub_;
00064   image_transport::Publisher face_img_pub_;
00065   image_transport::Subscriber img_sub_;
00066   image_transport::CameraSubscriber cam_sub_;
00067   ros::Publisher msg_pub_;
00068 
00069   boost::shared_ptr<image_transport::ImageTransport> it_;
00070 
00071   typedef opencv_apps::FaceDetectionConfig Config;
00072   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00073   Config config_;
00074   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00075 
00076   int queue_size_;
00077   bool debug_view_;
00078   ros::Time prev_stamp_;
00079 
00080   cv::CascadeClassifier face_cascade_;
00081   cv::CascadeClassifier eyes_cascade_;
00082 
00083   void reconfigureCallback(Config& new_config, uint32_t level)
00084   {
00085     config_ = new_config;
00086   }
00087 
00088   const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
00089   {
00090     if (frame.empty())
00091       return image_frame;
00092     return frame;
00093   }
00094 
00095   void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00096   {
00097     doWork(msg, cam_info->header.frame_id);
00098   }
00099 
00100   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00101   {
00102     doWork(msg, msg->header.frame_id);
00103   }
00104 
00105   void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
00106   {
00107     // Work on the image.
00108     try
00109     {
00110       // Convert the image into something opencv can handle.
00111       cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
00112 
00113       // Messages
00114       opencv_apps::FaceArrayStamped faces_msg;
00115       faces_msg.header = msg->header;
00116 
00117       // Do the work
00118       std::vector<cv::Rect> faces;
00119       cv::Mat frame_gray;
00120 
00121       if (frame.channels() > 1)
00122       {
00123         cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
00124       }
00125       else
00126       {
00127         frame_gray = frame;
00128       }
00129       cv::equalizeHist(frame_gray, frame_gray);
00130 //-- Detect faces
00131 #ifndef CV_VERSION_EPOCH
00132       face_cascade_.detectMultiScale(frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30));
00133 #else
00134       face_cascade_.detectMultiScale(frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30));
00135 #endif
00136 
00137       cv::Mat face_image;
00138       if (!faces.empty())
00139       {
00140         cv::Rect max_area = faces[0];
00141         for (const cv::Rect& face : faces)
00142         {
00143           if (max_area.width * max_area.height > face.width * face.height)
00144           {
00145             max_area = face;
00146           }
00147         }
00148         face_image = frame(max_area).clone();
00149       }
00150 
00151       for (const cv::Rect& face : faces)
00152       {
00153         cv::Point center(face.x + face.width / 2, face.y + face.height / 2);
00154         cv::ellipse(frame, center, cv::Size(face.width / 2, face.height / 2), 0, 0, 360, cv::Scalar(255, 0, 255), 2, 8,
00155                     0);
00156         opencv_apps::Face face_msg;
00157         face_msg.face.x = center.x;
00158         face_msg.face.y = center.y;
00159         face_msg.face.width = face.width;
00160         face_msg.face.height = face.height;
00161 
00162         cv::Mat face_roi = frame_gray(face);
00163         std::vector<cv::Rect> eyes;
00164 
00165 //-- In each face, detect eyes
00166 #ifndef CV_VERSION_EPOCH
00167         eyes_cascade_.detectMultiScale(face_roi, eyes, 1.1, 2, 0, cv::Size(30, 30));
00168 #else
00169         eyes_cascade_.detectMultiScale(face_roi, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30));
00170 #endif
00171 
00172         for (const cv::Rect& eye : eyes)
00173         {
00174           cv::Point eye_center(face.x + eye.x + eye.width / 2, face.y + eye.y + eye.height / 2);
00175           int radius = cvRound((eye.width + eye.height) * 0.25);
00176           cv::circle(frame, eye_center, radius, cv::Scalar(255, 0, 0), 3, 8, 0);
00177 
00178           opencv_apps::Rect eye_msg;
00179           eye_msg.x = eye_center.x;
00180           eye_msg.y = eye_center.y;
00181           eye_msg.width = eye.width;
00182           eye_msg.height = eye.height;
00183           face_msg.eyes.push_back(eye_msg);
00184         }
00185 
00186         faces_msg.faces.push_back(face_msg);
00187       }
00188       //-- Show what you got
00189       if (debug_view_)
00190       {
00191         cv::imshow("Face detection", frame);
00192         int c = cv::waitKey(1);
00193       }
00194 
00195       // Publish the image.
00196       sensor_msgs::Image::Ptr out_img =
00197           cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg();
00198       img_pub_.publish(out_img);
00199       msg_pub_.publish(faces_msg);
00200       if (!faces.empty())
00201       {
00202         sensor_msgs::Image::Ptr out_face_img =
00203             cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, face_image).toImageMsg();
00204         face_img_pub_.publish(out_face_img);
00205       }
00206     }
00207     catch (cv::Exception& e)
00208     {
00209       NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00210     }
00211 
00212     prev_stamp_ = msg->header.stamp;
00213   }
00214 
00215   void subscribe()  // NOLINT(modernize-use-override)
00216   {
00217     NODELET_DEBUG("Subscribing to image topic.");
00218     if (config_.use_camera_info)
00219       cam_sub_ = it_->subscribeCamera("image", queue_size_, &FaceDetectionNodelet::imageCallbackWithInfo, this);
00220     else
00221       img_sub_ = it_->subscribe("image", queue_size_, &FaceDetectionNodelet::imageCallback, this);
00222   }
00223 
00224   void unsubscribe()  // NOLINT(modernize-use-override)
00225   {
00226     NODELET_DEBUG("Unsubscribing from image topic.");
00227     img_sub_.shutdown();
00228     cam_sub_.shutdown();
00229   }
00230 
00231 public:
00232   virtual void onInit()  // NOLINT(modernize-use-override)
00233   {
00234     Nodelet::onInit();
00235     it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
00236 
00237     pnh_->param("queue_size", queue_size_, 3);
00238     pnh_->param("debug_view", debug_view_, false);
00239     if (debug_view_)
00240     {
00241       always_subscribe_ = true;
00242     }
00243     prev_stamp_ = ros::Time(0, 0);
00244 
00245     reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00246     dynamic_reconfigure::Server<Config>::CallbackType f =
00247         boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
00248     reconfigure_server_->setCallback(f);
00249 
00250     img_pub_ = advertiseImage(*pnh_, "image", 1);
00251     face_img_pub_ = advertiseImage(*pnh_, "face_image", 1);
00252     msg_pub_ = advertise<opencv_apps::FaceArrayStamped>(*pnh_, "faces", 1);
00253 
00254     std::string face_cascade_name, eyes_cascade_name;
00255     pnh_->param("face_cascade_name", face_cascade_name,
00256                 std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
00257     pnh_->param("eyes_cascade_name", eyes_cascade_name,
00258                 std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
00259 
00260     if (!face_cascade_.load(face_cascade_name))
00261     {
00262       NODELET_ERROR("--Error loading %s", face_cascade_name.c_str());
00263     };
00264     if (!eyes_cascade_.load(eyes_cascade_name))
00265     {
00266       NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str());
00267     };
00268 
00269     onInitPostProcess();
00270   }
00271 };
00272 }  // namespace opencv_apps
00273 
00274 namespace face_detection
00275 {
00276 class FaceDetectionNodelet : public opencv_apps::FaceDetectionNodelet
00277 {
00278 public:
00279   virtual void onInit()  // NOLINT(modernize-use-override)
00280   {
00281     ROS_WARN("DeprecationWarning: Nodelet face_detection/face_detection is deprecated, "
00282              "and renamed to opencv_apps/face_detection.");
00283     opencv_apps::FaceDetectionNodelet::onInit();
00284   }
00285 };
00286 }  // namespace face_detection
00287 
00288 #include <pluginlib/class_list_macros.h>
00289 PLUGINLIB_EXPORT_CLASS(opencv_apps::FaceDetectionNodelet, nodelet::Nodelet);
00290 PLUGINLIB_EXPORT_CLASS(face_detection::FaceDetectionNodelet, nodelet::Nodelet);


opencv_apps
Author(s): Kei Okada
autogenerated on Mon Apr 22 2019 02:18:26