face_detection.cpp
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Caroline Pantofaru */
00037 
00038 
00039 
00040 #include <stdio.h>
00041 #include <iostream>
00042 #include <vector>
00043 #include <fstream>
00044 
00045 #include <ros/ros.h>
00046 #include <ros/console.h>
00047 #include <boost/filesystem.hpp>
00048 #include <boost/thread/mutex.hpp>
00049 
00050 #include <people_msgs/PositionMeasurement.h>
00051 #include <people_msgs/PositionMeasurementArray.h>
00052 #include <message_filters/subscriber.h>
00053 #include <message_filters/time_synchronizer.h>
00054 #include <message_filters/sync_policies/exact_time.h>
00055 #include <message_filters/sync_policies/approximate_time.h>
00056 #include <image_transport/subscriber_filter.h>
00057 #include "sensor_msgs/CameraInfo.h"
00058 #include "sensor_msgs/Image.h"
00059 #include "stereo_msgs/DisparityImage.h"
00060 #include "sensor_msgs/image_encodings.h"
00061 #include "cv_bridge/cv_bridge.h"
00062 #include "tf/transform_listener.h"
00063 #include "sensor_msgs/PointCloud.h"
00064 #include "geometry_msgs/Point32.h"
00065 
00066 #include "opencv/cxcore.hpp"
00067 #include "opencv/cv.hpp"
00068 #include "opencv/highgui.h"
00069 
00070 #include "face_detector/faces.h"
00071 
00072 #include "image_geometry/stereo_camera_model.h"
00073 
00074 #include <actionlib/server/simple_action_server.h>
00075 #include <face_detector/FaceDetectorAction.h>
00076 
00077 using namespace std;
00078 
00079 namespace people
00080 {
00081 
00084 class FaceDetector
00085 {
00086 public:
00087   // Constants
00088   const double BIGDIST_M;// = 1000000.0;
00089 
00090   // Node handle
00091   ros::NodeHandle nh_;
00092 
00093   boost::mutex connect_mutex_;
00094   bool use_rgbd_;
00095 
00096   // Subscription topics
00097   string image_image_;
00098   // Stereo
00099   string stereo_namespace_;
00100   string left_topic_;
00101   string disparity_topic_;
00102   string left_camera_info_topic_;
00103   string right_camera_info_topic_;
00104   // RGB-D
00105   string camera_;
00106   string camera_topic_;
00107   string depth_image_;
00108   string depth_topic_;
00109   string camera_info_topic_;
00110   string depth_info_topic_;
00111   string rgb_ns_;
00112   string depth_ns_;
00113 
00114   // Images and conversion for both the stereo camera and rgb-d camera cases.
00115   image_transport::ImageTransport it_;
00116   image_transport::SubscriberFilter image_sub_; 
00117   image_transport::SubscriberFilter depth_image_sub_; 
00118   message_filters::Subscriber<stereo_msgs::DisparityImage> disp_image_sub_; 
00119   message_filters::Subscriber<sensor_msgs::CameraInfo> c1_info_sub_; 
00120   message_filters::Subscriber<sensor_msgs::CameraInfo> c2_info_sub_; 
00122   // Disparity:
00123   typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactDispPolicy; 
00124   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateDispPolicy; 
00125   typedef message_filters::Synchronizer<ExactDispPolicy> ExactDispSync;
00126   typedef message_filters::Synchronizer<ApproximateDispPolicy> ApproximateDispSync;
00127   boost::shared_ptr<ExactDispSync> exact_disp_sync_;
00128   boost::shared_ptr<ApproximateDispSync> approximate_disp_sync_;
00129 
00130   // Depth:
00131   typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactDepthPolicy; 
00132   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateDepthPolicy; 
00133   typedef message_filters::Synchronizer<ExactDepthPolicy> ExactDepthSync;
00134   typedef message_filters::Synchronizer<ApproximateDepthPolicy> ApproximateDepthSync;
00135   boost::shared_ptr<ExactDepthSync> exact_depth_sync_;
00136   boost::shared_ptr<ApproximateDepthSync> approximate_depth_sync_;
00137 
00138   // Action
00139   actionlib::SimpleActionServer<face_detector::FaceDetectorAction> as_;
00140   face_detector::FaceDetectorFeedback feedback_;
00141   face_detector::FaceDetectorResult result_;
00142 
00143   // If running the face detector as a component in part of a larger person tracker, this subscribes to the tracker's position measurements and whether it was initialized by some other node.
00144   // Todo: resurrect the person tracker.
00145   ros::Subscriber pos_sub_;
00146   bool external_init_;
00147 
00148 
00149   // Publishers
00150   // A point cloud of the face positions, meant for visualization in rviz.
00151   // This could be replaced by visualization markers, but they can't be modified
00152   // in rviz at runtime (eg the alpha, display time, etc. can't be changed.)
00153   ros::Publisher cloud_pub_;
00154   ros::Publisher pos_array_pub_;
00155 
00156   // Display
00157   bool do_display_; 
00158   bool do_display_face_id_; 
00159   cv::Mat cv_image_out_; 
00161   // Depth
00162   bool use_depth_; 
00163   image_geometry::StereoCameraModel cam_model_; 
00165   // Face detector params and output
00166   Faces *faces_; 
00167   string name_; 
00168   string haar_filename_; 
00169   double reliability_; 
00171   struct RestampedPositionMeasurement
00172   {
00173     ros::Time restamp;
00174     people_msgs::PositionMeasurement pos;
00175     double dist;
00176   };
00177   map<string, RestampedPositionMeasurement> pos_list_; 
00179   int max_id_;
00180 
00181   bool quit_;
00182 
00183   tf::TransformListener tf_;
00184 
00185   std::string fixed_frame_;
00186 
00187   boost::mutex cv_mutex_, pos_mutex_, limage_mutex_, dimage_mutex_;
00188 
00189   bool do_continuous_; 
00191   bool do_publish_unknown_; 
00193   FaceDetector(std::string name) :
00194     BIGDIST_M(1000000.0),
00195     it_(nh_),
00196     as_(nh_, name, false),
00197     faces_(0),
00198     max_id_(-1),
00199     quit_(false)
00200   {
00201     ROS_INFO_STREAM_NAMED("face_detector", "Constructing FaceDetector.");
00202 
00203     // Action stuff
00204     as_.registerGoalCallback(boost::bind(&FaceDetector::goalCB, this));
00205     as_.registerPreemptCallback(boost::bind(&FaceDetector::preemptCB, this));
00206     as_.start();
00207 
00208     faces_ = new Faces();
00209     double face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m;
00210     int queue_size;
00211     bool approx;
00212 
00213     // Parameters
00214     ros::NodeHandle local_nh("~");
00215     local_nh.param("classifier_name", name_, std::string(""));
00216     local_nh.param("classifier_filename", haar_filename_, std::string(""));
00217     local_nh.param("classifier_reliability", reliability_, 0.0);
00218     local_nh.param("do_display", do_display_, false);
00219     local_nh.param("do_display_face_id", do_display_face_id_, false);
00220     local_nh.param("do_continuous", do_continuous_, true);
00221     local_nh.param("do_publish_faces_of_unknown_size", do_publish_unknown_, false);
00222     local_nh.param("use_depth", use_depth_, true);
00223     local_nh.param("use_external_init", external_init_, false);
00224     local_nh.param("face_size_min_m", face_size_min_m, FACE_SIZE_MIN_M);
00225     local_nh.param("face_size_max_m", face_size_max_m, FACE_SIZE_MAX_M);
00226     local_nh.param("max_face_z_m", max_face_z_m, MAX_FACE_Z_M);
00227     local_nh.param("face_separation_dist_m", face_sep_dist_m, FACE_SEP_DIST_M);
00228     local_nh.param("use_rgbd", use_rgbd_, false);
00229     local_nh.param("queue_size", queue_size, 5);
00230     local_nh.param("approximate_sync", approx, false);
00231 
00232     if (do_display_)
00233     {
00234       // OpenCV: pop up an OpenCV highgui window
00235       cv::namedWindow("Face detector: Face Detection", CV_WINDOW_AUTOSIZE);
00236     }
00237 
00238     // Init the detector and subscribe to the images and camera parameters. One case for rgbd, one for stereo.
00239     if (use_rgbd_)
00240     {
00241 
00242       faces_->initFaceDetectionDepth(1, haar_filename_, face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m);
00243 
00244       camera_ = nh_.resolveName("camera");
00245       image_image_ = nh_.resolveName("image_topic");
00246       depth_image_ = nh_.resolveName("depth_topic");
00247       rgb_ns_ = nh_.resolveName("rgb_ns");
00248       depth_ns_ = nh_.resolveName("depth_ns");
00249       camera_topic_ = ros::names::clean(camera_ + "/" + rgb_ns_ + "/" + image_image_);
00250       depth_topic_ = ros::names::clean(camera_ + "/" + depth_ns_ + "/" + depth_image_);
00251       camera_info_topic_ = ros::names::clean(camera_ + "/" + rgb_ns_ + "/camera_info");
00252       depth_info_topic_ = ros::names::clean(camera_ + "/" + depth_ns_ + "/camera_info");
00253 
00254       ROS_DEBUG("camera_topic_: %s", camera_topic_.c_str());
00255       ROS_DEBUG("depth_topic_: %s", depth_topic_.c_str());
00256       ROS_DEBUG("camera_info_topic_: %s", camera_info_topic_.c_str());
00257       ROS_DEBUG("depth_info_topic_: %s", depth_info_topic_.c_str());
00258 
00259       local_nh.param("fixed_frame", fixed_frame_, std::string("camera_rgb_optical_frame"));
00260 
00261       if (approx)
00262       {
00263         approximate_depth_sync_.reset(new ApproximateDepthSync(ApproximateDepthPolicy(queue_size),
00264                                       image_sub_, depth_image_sub_, c1_info_sub_, c2_info_sub_));
00265         approximate_depth_sync_->registerCallback(boost::bind(&FaceDetector::imageCBAllDepth,
00266             this, _1, _2, _3, _4));
00267       }
00268       else
00269       {
00270         exact_depth_sync_.reset(new ExactDepthSync(ExactDepthPolicy(queue_size),
00271                                 image_sub_, depth_image_sub_, c1_info_sub_, c2_info_sub_));
00272         exact_depth_sync_->registerCallback(boost::bind(&FaceDetector::imageCBAllDepth,
00273                                             this, _1, _2, _3, _4));
00274       }
00275 
00276     }
00277     else
00278     {
00279       faces_->initFaceDetectionDisparity(1, haar_filename_, face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m);
00280 
00281       stereo_namespace_ = nh_.resolveName("camera");
00282       image_image_ = nh_.resolveName("image_topic");
00283       left_topic_ = ros::names::clean(stereo_namespace_ + "/left/" + image_image_);
00284       disparity_topic_ = ros::names::clean(stereo_namespace_ + "/disparity");
00285       left_camera_info_topic_ = ros::names::clean(stereo_namespace_ + "/left/camera_info");
00286       right_camera_info_topic_ = ros::names::clean(stereo_namespace_ + "/right/camera_info");
00287 
00288       local_nh.param("fixed_frame", fixed_frame_, stereo_namespace_.append("_optical_frame"));
00289 
00290       if (approx)
00291       {
00292         approximate_disp_sync_.reset(new ApproximateDispSync(ApproximateDispPolicy(queue_size),
00293                                      image_sub_, disp_image_sub_, c1_info_sub_, c2_info_sub_));
00294         approximate_disp_sync_->registerCallback(boost::bind(&FaceDetector::imageCBAllDisp,
00295             this, _1, _2, _3, _4));
00296       }
00297       else
00298       {
00299         exact_disp_sync_.reset(new ExactDispSync(ExactDispPolicy(queue_size),
00300                                image_sub_, disp_image_sub_, c1_info_sub_, c2_info_sub_));
00301         exact_disp_sync_->registerCallback(boost::bind(&FaceDetector::imageCBAllDisp,
00302                                            this, _1, _2, _3, _4));
00303       }
00304     }
00305 
00306     // Connection callbacks and advertise
00307     ros::SubscriberStatusCallback pos_pub_connect_cb = boost::bind(&FaceDetector::connectCb, this);
00308     ros::SubscriberStatusCallback cloud_pub_connect_cb = boost::bind(&FaceDetector::connectCb, this);
00309     if (do_continuous_)
00310       ROS_INFO_STREAM_NAMED("face_detector", "You must subscribe to one of FaceDetector's outbound topics or else it will not publish anything.");
00311 
00312     {
00313       boost::mutex::scoped_lock lock(connect_mutex_);
00314 
00315       // Advertise a position measure message.
00316       pos_array_pub_ = nh_.advertise<people_msgs::PositionMeasurementArray>("face_detector/people_tracker_measurements_array", 1, pos_pub_connect_cb, pos_pub_connect_cb);
00317 
00318       cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("face_detector/faces_cloud", 0, cloud_pub_connect_cb, cloud_pub_connect_cb);
00319     }
00320 
00321 
00322     // If running as an action server, just stay connected so that action calls are fast.
00323     if (!do_continuous_) connectCb();
00324 
00325     ros::MultiThreadedSpinner s(2);
00326     ros::spin(s);
00327 
00328   }
00329 
00330   ~FaceDetector()
00331   {
00332 
00333     cv_image_out_.release();
00334 
00335     if (do_display_)
00336     {
00337       cv::destroyWindow("Face detector: Face Detection");
00338     }
00339 
00340     if (faces_)
00341     {
00342       delete faces_;
00343       faces_ = 0;
00344     }
00345   }
00346 
00347   // Handles (un)subscribing when clients (un)subscribe
00348   void connectCb()
00349   {
00350     boost::mutex::scoped_lock lock(connect_mutex_);
00351     if (use_rgbd_)
00352     {
00353       if (do_continuous_ && cloud_pub_.getNumSubscribers() == 0 && pos_array_pub_.getNumSubscribers() == 0)
00354       {
00355         ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, so it will no longer publish anything.");
00356         image_sub_.unsubscribe();
00357         depth_image_sub_.unsubscribe();
00358         c1_info_sub_.unsubscribe();
00359         c2_info_sub_.unsubscribe();
00360         pos_sub_.shutdown();
00361       }
00362       else if (!do_continuous_ || !image_sub_.getSubscriber())
00363       {
00364         image_sub_.subscribe(it_, camera_topic_, 3);
00365         depth_image_sub_.subscribe(it_, depth_topic_, 3);
00366         c1_info_sub_.subscribe(nh_, camera_info_topic_, 3);
00367         c2_info_sub_.subscribe(nh_, depth_info_topic_, 3);
00368         // // Subscribe to filter measurements.
00369         // if (external_init_) {
00370         //   //pos_sub_ = nh_.subscribe("people_tracker_filter",1,&FaceDetector::posCallback,this);
00371         //   pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array",1,&FaceDetector::posCallback,this);
00372         // }
00373       }
00374     }
00375     else
00376     {
00377       if (do_continuous_ && cloud_pub_.getNumSubscribers() == 0 && pos_array_pub_.getNumSubscribers() == 0)
00378       {
00379         ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, so it will no longer publish anything.");
00380         image_sub_.unsubscribe();
00381         disp_image_sub_.unsubscribe();
00382         c1_info_sub_.unsubscribe();
00383         c2_info_sub_.unsubscribe();
00384         pos_sub_.shutdown();
00385       }
00386       else if (!do_continuous_ || !image_sub_.getSubscriber())
00387       {
00388         image_sub_.subscribe(it_, left_topic_, 3);
00389         disp_image_sub_.subscribe(nh_, disparity_topic_, 3);
00390         c1_info_sub_.subscribe(nh_, left_camera_info_topic_, 3);
00391         c2_info_sub_.subscribe(nh_, right_camera_info_topic_, 3);
00392         // // Subscribe to filter measurements.
00393         // if (external_init_) {
00394         //   //pos_sub_ = nh_.subscribe("people_tracker_filter",1,&FaceDetector::posCallback,this);
00395         //   pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array",1,&FaceDetector::posCallback,this);
00396         // }
00397       }
00398     }
00399   }
00400 
00401 
00402   void goalCB()
00403   {
00404     as_.acceptNewGoal();
00405   }
00406 
00407   void preemptCB()
00408   {
00409     as_.setPreempted();
00410   }
00411 
00412 
00419   // void posCallback(const people_msgs::PositionMeasurementArrayConstPtr& pos_array_ptr) {
00420 
00421   //   // Put the incoming position into the position queue. It'll be processed in the next image callback.
00422   //   boost::mutex::scoped_lock lock(pos_mutex_);
00423 
00424   //   map<string, RestampedPositionMeasurement>::iterator it;
00425   //   for (uint ipa = 0; ipa < pos_array_ptr->people.size(); ipa++) {
00426   //     it = pos_list_.find(pos_array_ptr->people[ipa].object_id);
00427   //     RestampedPositionMeasurement rpm;
00428   //     rpm.pos = pos_array_ptr->people[ipa];
00429   //     rpm.restamp = pos_array_ptr->people[ipa].header.stamp;
00430   //     rpm.dist = BIGDIST_M;
00431   //     if (it == pos_list_.end()) {
00432   //  pos_list_.insert(pair<string, RestampedPositionMeasurement>(pos_array_ptr->people[ipa].object_id, rpm));
00433   //     }
00434   //     else if ((pos_array_ptr->people[ipa].header.stamp - (*it).second.pos.header.stamp) > ros::Duration().fromSec(-1.0) ){
00435   //  (*it).second = rpm;
00436   //     }
00437   //   }
00438   //   lock.unlock();
00439 
00440   // }
00441 
00442   // Workaround to convert a DisparityImage->Image into a shared pointer for cv_bridge in imageCBAll.
00443   struct NullDeleter
00444   {
00445     void operator()(void const *) const {}
00446   };
00447 
00456   void imageCBAllDepth(const sensor_msgs::Image::ConstPtr &image, const sensor_msgs::Image::ConstPtr& depth_image, const sensor_msgs::CameraInfo::ConstPtr& c1_info, const sensor_msgs::CameraInfo::ConstPtr& c2_info)
00457   {
00458 
00459     // Only run the detector if in continuous mode or the detector was turned on through an action invocation.
00460     if (!do_continuous_ && !as_.isActive())
00461       return;
00462 
00463     // Clear out the result vector.
00464     result_.face_positions.clear();
00465 
00466     if (do_display_)
00467     {
00468       cv_mutex_.lock();
00469     }
00470 
00471     // ROS --> OpenCV
00472     cv_bridge::CvImageConstPtr cv_image_ptr = cv_bridge::toCvShare(image, "bgr8");
00473     cv_bridge::CvImageConstPtr cv_depth_ptr = cv_bridge::toCvShare(depth_image);
00474     cv::Mat depth_32fc1 = cv_depth_ptr->image;
00475     if (depth_image->encoding != "32FC1")
00476     {
00477       cv_depth_ptr->image.convertTo(depth_32fc1, CV_32FC1, 0.001);
00478     }
00479 
00480 
00481 
00482     cam_model_.fromCameraInfo(c1_info, c2_info);
00483 
00484     // For display, keep a copy of the image that we can draw on.
00485     if (do_display_)
00486     {
00487       cv_image_out_ = (cv_image_ptr->image).clone();
00488     }
00489 
00490     struct timeval timeofday;
00491     gettimeofday(&timeofday, NULL);
00492     ros::Time starttdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec);
00493 
00494     vector<Box2D3D> faces_vector = faces_->detectAllFacesDepth(cv_image_ptr->image, 1.0, depth_32fc1, &cam_model_);
00495     gettimeofday(&timeofday, NULL);
00496     ros::Time endtdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec);
00497     ros::Duration diffdetect = endtdetect - starttdetect;
00498     ROS_INFO_STREAM_NAMED("face_detector", "Detection duration = " << diffdetect.toSec() << "sec");
00499 
00500     matchAndDisplay(faces_vector, image->header);
00501   }
00502 
00511   void imageCBAllDisp(const sensor_msgs::Image::ConstPtr &image, const stereo_msgs::DisparityImage::ConstPtr& disp_image, const sensor_msgs::CameraInfo::ConstPtr& c1_info, const sensor_msgs::CameraInfo::ConstPtr& c2_info)
00512   {
00513 
00514     // Only run the detector if in continuous mode or the detector was turned on through an action invocation.
00515     if (!do_continuous_ && !as_.isActive())
00516       return;
00517 
00518     // Clear out the result vector.
00519     result_.face_positions.clear();
00520 
00521     if (do_display_)
00522     {
00523       cv_mutex_.lock();
00524     }
00525 
00526 
00527     // ROS --> OpenCV
00528     cv_bridge::CvImageConstPtr cv_image_ptr = cv_bridge::toCvShare(image, sensor_msgs::image_encodings::BGR8);
00529     cv_bridge::CvImageConstPtr cv_disp_ptr = cv_bridge::toCvShare(disp_image->image, disp_image);
00530     cam_model_.fromCameraInfo(c1_info, c2_info);
00531 
00532     // For display, keep a copy of the image that we can draw on.
00533     if (do_display_)
00534     {
00535       cv_image_out_ = (cv_image_ptr->image).clone();
00536     }
00537 
00538     struct timeval timeofday;
00539     gettimeofday(&timeofday, NULL);
00540     ros::Time starttdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec);
00541 
00542     vector<Box2D3D> faces_vector = faces_->detectAllFacesDisparity(cv_image_ptr->image, 1.0, cv_disp_ptr->image, &cam_model_);
00543     gettimeofday(&timeofday, NULL);
00544     ros::Time endtdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec);
00545     ros::Duration diffdetect = endtdetect - starttdetect;
00546     ROS_INFO_STREAM_NAMED("face_detector", "Detection duration = " << diffdetect.toSec() << "sec");
00547 
00548     matchAndDisplay(faces_vector, image->header);
00549   }
00550 
00551 
00552 private:
00553 
00554   void matchAndDisplay(vector<Box2D3D> faces_vector, std_msgs::Header header)
00555   {
00556     bool found_faces = false;
00557 
00558     int ngood = 0;
00559     sensor_msgs::PointCloud cloud;
00560     cloud.header.stamp = header.stamp;
00561     cloud.header.frame_id = header.frame_id;
00562 
00563     if (faces_vector.size() > 0)
00564     {
00565 
00566       // Transform the positions of the known faces and remove anyone who hasn't had an update in a long time.
00567       //      boost::mutex::scoped_lock pos_lock(pos_mutex_);
00568       map<string, RestampedPositionMeasurement>::iterator it;
00569       it = pos_list_.begin();
00570       while (it != pos_list_.end())
00571       {
00572         if ((header.stamp - (*it).second.restamp) > ros::Duration().fromSec(5.0))
00573         {
00574           // Position is too old, remove the person.
00575           pos_list_.erase(it++);
00576         }
00577         else
00578         {
00579           // Transform the person to this time. Note that the pos time is updated but not the restamp.
00580           tf::Point pt;
00581           tf::pointMsgToTF((*it).second.pos.pos, pt);
00582           tf::Stamped<tf::Point> loc(pt, (*it).second.pos.header.stamp, (*it).second.pos.header.frame_id);
00583           try
00584           {
00585             tf_.transformPoint(header.frame_id, header.stamp, loc, fixed_frame_, loc);
00586             (*it).second.pos.header.stamp = header.stamp;
00587             (*it).second.pos.pos.x = loc[0];
00588             (*it).second.pos.pos.y = loc[1];
00589             (*it).second.pos.pos.z = loc[2];
00590           }
00591           catch (tf::TransformException& ex)
00592           {
00593           }
00594           it++;
00595         }
00596       }
00597       // End filter face position update
00598 
00599       // Associate the found faces with previously seen faces, and publish all good face centers.
00600       Box2D3D *one_face;
00601       people_msgs::PositionMeasurement pos;
00602       people_msgs::PositionMeasurementArray pos_array;
00603 
00604       for (uint iface = 0; iface < faces_vector.size(); iface++)
00605       {
00606         one_face = &faces_vector[iface];
00607 
00608         if (one_face->status == "good" || (one_face->status == "unknown" && do_publish_unknown_))
00609         {
00610 
00611           std::string id = "";
00612 
00613           // Convert the face format to a PositionMeasurement msg.
00614           pos.header.stamp = header.stamp;
00615           pos.name = name_;
00616           pos.pos.x = one_face->center3d.x;
00617           pos.pos.y = one_face->center3d.y;
00618           pos.pos.z = one_face->center3d.z;
00619           pos.header.frame_id = header.frame_id;//"*_optical_frame";
00620           pos.reliability = reliability_;
00621           pos.initialization = 1;//0;
00622           pos.covariance[0] = 0.04;
00623           pos.covariance[1] = 0.0;
00624           pos.covariance[2] = 0.0;
00625           pos.covariance[3] = 0.0;
00626           pos.covariance[4] = 0.04;
00627           pos.covariance[5] = 0.0;
00628           pos.covariance[6] = 0.0;
00629           pos.covariance[7] = 0.0;
00630           pos.covariance[8] = 0.04;
00631 
00632           // Check if this person's face is close enough to one of the previously known faces and associate it with the closest one.
00633           // Otherwise publish it with an empty id.
00634           // Note that multiple face positions can be published with the same id, but ids in the pos_list_ are unique. The position of a face in the list is updated with the closest found face.
00635           double dist, mindist = BIGDIST_M;
00636           map<string, RestampedPositionMeasurement>::iterator close_it = pos_list_.end();
00637           for (it = pos_list_.begin(); it != pos_list_.end(); it++)
00638           {
00639             dist = pow((*it).second.pos.pos.x - pos.pos.x, 2.0) + pow((*it).second.pos.pos.y - pos.pos.y, 2.0) + pow((*it).second.pos.pos.z - pos.pos.z, 2.0);
00640             if (dist <= faces_->face_sep_dist_m_ && dist < mindist)
00641             {
00642               mindist = dist;
00643               close_it = it;
00644             }
00645           }
00646           if (close_it != pos_list_.end())
00647           {
00648             pos.object_id = (*close_it).second.pos.object_id;
00649             if (mindist < (*close_it).second.dist)
00650             {
00651               (*close_it).second.restamp = header.stamp;
00652               (*close_it).second.dist = mindist;
00653               (*close_it).second.pos = pos;
00654             }
00655             ROS_INFO_STREAM_NAMED("face_detector", "Found face to match with id " << pos.object_id);
00656           }
00657           else
00658           {
00659             max_id_++;
00660             pos.object_id = static_cast<ostringstream*>(&(ostringstream() << max_id_))->str();
00661             ROS_INFO_STREAM_NAMED("face_detector", "Didn't find face to match, starting new ID " << pos.object_id);
00662           }
00663           result_.face_positions.push_back(pos);
00664           found_faces = true;
00665 
00666         }
00667 
00668       }
00669       pos_array.header.stamp = header.stamp;
00670       pos_array.header.frame_id = header.frame_id;
00671       pos_array.people = result_.face_positions;
00672       if (pos_array.people.size() > 0)
00673       {
00674         pos_array_pub_.publish(pos_array);
00675 
00676         // Update the position list greedily. This should be rewritten.
00677         map<string, RestampedPositionMeasurement>::iterator it;
00678         for (uint ipa = 0; ipa < pos_array.people.size(); ipa++)
00679         {
00680           it = pos_list_.find(pos_array.people[ipa].object_id);
00681           RestampedPositionMeasurement rpm;
00682           rpm.pos = pos_array.people[ipa];
00683           rpm.restamp = pos_array.people[ipa].header.stamp;
00684           rpm.dist = BIGDIST_M;
00685           if (it == pos_list_.end())
00686           {
00687             pos_list_.insert(pair<string, RestampedPositionMeasurement>(pos_array.people[ipa].object_id, rpm));
00688           }
00689           else if ((pos_array.people[ipa].header.stamp - (*it).second.pos.header.stamp) > ros::Duration().fromSec(-1.0))
00690           {
00691             (*it).second = rpm;
00692           }
00693         }
00694         //      pos_lock.unlock();
00695 
00696         // Clean out all of the distances in the pos_list_
00697         for (it = pos_list_.begin(); it != pos_list_.end(); it++)
00698         {
00699           (*it).second.dist = BIGDIST_M;
00700         }
00701 
00702       }
00703 
00704 
00705       // Done associating faces
00706 
00707       /******** Display **************************************************************/
00708 
00709       // Publish a point cloud of face centers.
00710 
00711       cloud.channels.resize(1);
00712       cloud.channels[0].name = "intensity";
00713 
00714       for (uint iface = 0; iface < faces_vector.size(); iface++)
00715       {
00716         one_face = &faces_vector[iface];
00717 
00718         // Visualization of good faces as a point cloud
00719         if (one_face->status == "good")
00720         {
00721 
00722           geometry_msgs::Point32 p;
00723           p.x = one_face->center3d.x;
00724           p.y = one_face->center3d.y;
00725           p.z = one_face->center3d.z;
00726           cloud.points.push_back(p);
00727           cloud.channels[0].values.push_back(1.0f);
00728 
00729           ngood ++;
00730         }
00731       }
00732 
00733       cloud_pub_.publish(cloud);
00734       // Done publishing the point cloud.
00735 
00736     } // Done if faces_vector.size() > 0
00737 
00738 
00739     // Draw an appropriately colored rectangle on the display image and in the visualizer.
00740     if (do_display_)
00741     {
00742       displayFacesOnImage(faces_vector);
00743       cv_mutex_.unlock();
00744     }
00745     // Done drawing.
00746 
00747     /******** Done display **********************************************************/
00748 
00749     ROS_INFO_STREAM_NAMED("face_detector", "Number of faces found: " << faces_vector.size() << ", number with good depth and size: " << ngood);
00750 
00751     // If you don't want continuous processing and you've found at least one face, turn off the detector.
00752     if (!do_continuous_ && found_faces)
00753     {
00754       as_.setSucceeded(result_);
00755     }
00756   }
00757 
00758   // Draw bounding boxes around detected faces on the cv_image_out_ and show the image.
00759   void displayFacesOnImage(vector<Box2D3D>  faces_vector)
00760   {
00761 
00762     Box2D3D *one_face;
00763 
00764     for (uint iface = 0; iface < faces_vector.size(); iface++)
00765     {
00766       one_face = &faces_vector[iface];
00767       // Visualization by image display.
00768       if (do_display_)
00769       {
00770         cv::Scalar color;
00771         if (one_face->status == "good")
00772         {
00773           color = cv::Scalar(0, 255, 0);
00774         }
00775         else if (one_face->status == "unknown")
00776         {
00777           color = cv::Scalar(255, 0, 0);
00778         }
00779         else
00780         {
00781           color = cv::Scalar(0, 0, 255);
00782         }
00783 
00784         cv::rectangle(cv_image_out_,
00785                       cv::Point(one_face->box2d.x, one_face->box2d.y),
00786                       cv::Point(one_face->box2d.x + one_face->box2d.width, one_face->box2d.y + one_face->box2d.height), color, 4);
00787 
00788         if (do_display_face_id_) {
00789 
00790             char id_str[16];
00791             sprintf(id_str, "Id: %d", one_face->id);
00792             cv::putText(cv_image_out_, id_str, cv::Point(one_face->box2d.x, one_face->box2d.y- 6), cv::FONT_HERSHEY_PLAIN, 1, color);
00793 
00794         }
00795 
00796       }
00797     }
00798 
00799     cv::imshow("Face detector: Face Detection", cv_image_out_);
00800     cv::waitKey(2);
00801 
00802   }
00803 
00804 
00805 }; // end class
00806 
00807 }; // end namespace people
00808 
00809 // Main
00810 int main(int argc, char **argv)
00811 {
00812   ros::init(argc, argv, "face_detector");
00813 
00814   people::FaceDetector fd(ros::this_node::getName());
00815 
00816   return 0;
00817 }


face_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Apr 13 2017 02:41:46