faces.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Faces-specific computer vision algorithms.
00003  *
00004  **********************************************************************
00005  *
00006  * Software License Agreement (BSD License)
00007  * 
00008  *  Copyright (c) 2008, Willow Garage, Inc.
00009  *  All rights reserved.
00010  * 
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  * 
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of the Willow Garage nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  * 
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *********************************************************************/
00038 
00039 /* Author: Caroline Pantofaru */
00040 
00041 #include "face_detector/faces.h"
00042 #include <cfloat>
00043 #include <algorithm>
00044 
00045 namespace people {
00046 
00047 Faces::Faces():
00048   list_(NULL),
00049   cam_model_(NULL) {
00050 }
00051 
00052 Faces::~Faces() {
00053   // Kill all the threads.
00054   threads_.interrupt_all();
00055   threads_.join_all();
00056   delete face_go_mutex_;
00057   face_go_mutex_ = NULL;
00058 
00059   for (int i=list_.size(); i>0; i--) {
00060     list_.pop_back();
00061   }
00062 
00063   cam_model_ = 0;
00064 
00065 }
00066 
00067 
00068 /* Note: The multi-threading in this file is left over from a previous incarnation that allowed multiple 
00069  * cascades to be run at once. It hasn't been removed in case we want to return to that model, and since 
00070  * there isn't much overhead. Right now, however, only one classifier is being run per instantiation 
00071  * of the face_detector node.
00072  */
00073 
00074 
00075 void Faces::initFaceDetectionDisparity(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m) {
00076   images_ready_ = 0;
00077 
00078   face_size_min_m_ = face_size_min_m;
00079   face_size_max_m_ = face_size_max_m;
00080   max_face_z_m_ = max_face_z_m;
00081   face_sep_dist_m_ = face_sep_dist_m;
00082 
00083   face_go_mutex_ = new boost::mutex();
00084   bool cascade_ok = cascade_.load(haar_classifier_filename);
00085   
00086   if (!cascade_ok) {
00087     ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
00088     return;
00089   }
00090   threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDisparity,this,0));
00091 }
00092 
00094 
00095 vector<Box2D3D> Faces::detectAllFacesDisparity(const cv::Mat &image, double threshold, const cv::Mat &disparity_image, image_geometry::StereoCameraModel *cam_model) {
00096 
00097   faces_.clear();
00098 
00099   // Convert the image to grayscale, if necessary.
00100   
00101   if (image.channels() == 1) {
00102     cv_image_gray_.create(image.size(), CV_8UC1);
00103     image.copyTo(cv_image_gray_);
00104   }
00105   else if (image.channels() == 3) {
00106     cv_image_gray_.create(image.size(), CV_8UC1);
00107     cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
00108   }
00109   else {
00110     std::cerr << "Unknown image type"<<std::endl;
00111     return faces_;
00112   }
00113 
00114   disparity_image_ = &disparity_image;
00115   cam_model_ = cam_model;
00116   
00117   // Tell the face detection threads that they can run once.
00118   num_threads_to_wait_for_ = threads_.size();
00119   boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
00120   images_ready_++;
00121   fgmlock.unlock();
00122 
00123   face_detection_ready_cond_.notify_all();
00124 
00125   boost::mutex::scoped_lock fdmlock(face_done_mutex_);
00126   while (num_threads_to_wait_for_ > 0) {
00127     face_detection_done_cond_.wait(fdmlock);
00128   }  
00129 
00130   return faces_;
00131 }
00132 
00134 
00135 void Faces::faceDetectionThreadDisparity(uint i) {
00136 
00137   while (1) {
00138     boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
00139     boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
00140     while (1) {
00141       tlock.lock();
00142       if (images_ready_) {
00143         --images_ready_;
00144         tlock.unlock();
00145         break;
00146       }
00147       tlock.unlock();
00148       face_detection_ready_cond_.wait(fgmlock);
00149     }
00150 
00151     // Find the faces using OpenCV's haar cascade object detector.
00152     cv::Point3d p3_1(0,0,max_face_z_m_);
00153     cv::Point3d p3_2(face_size_min_m_,0,max_face_z_m_);
00154     cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
00155     cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
00156     int this_min_face_size = (int)(floor(fabs(p2_2.x-p2_1.x)));
00157 
00158     std::vector<cv::Rect> faces_vec;
00159     cascade_.detectMultiScale(cv_image_gray_, faces_vec,  1.2, 2, CV_HAAR_DO_CANNY_PRUNING, cv::Size(this_min_face_size,this_min_face_size));
00160 
00161     // Filter the faces using depth information, if available. Currently checks that the actual face size is within the given limits.
00162     cv::Scalar color(0,255,0);
00163     Box2D3D one_face;
00164     double avg_disp = 0.0;
00165     cv::Mat uvd(1,3,CV_32FC1);
00166     cv::Mat xyz(1,3,CV_32FC1);
00167     // For each face...
00168     for (uint iface = 0; iface < faces_vec.size(); iface++) {//face_seq->total; iface++) {
00169 
00170       one_face.status = "good";
00171 
00172       one_face.box2d = faces_vec[iface];
00173       one_face.id = i; // The cascade that computed this face.
00174 
00175       // Get the median disparity in the middle half of the bounding box.
00176       cv::Mat disp_roi_shallow(*disparity_image_,cv::Rect(floor(one_face.box2d.x+0.25*one_face.box2d.width),  
00177                                                           floor(one_face.box2d.y+0.25*one_face.box2d.height),
00178                                                           floor(one_face.box2d.x+0.75*one_face.box2d.width) - floor(one_face.box2d.x+0.25*one_face.box2d.width) + 1,
00179                                                           floor(one_face.box2d.y+0.75*one_face.box2d.height) - floor(one_face.box2d.y+0.25*one_face.box2d.height) + 1));
00180       cv::Mat disp_roi = disp_roi_shallow.clone();
00181       cv::Mat tmat = disp_roi.reshape(1,disp_roi.rows*disp_roi.cols);
00182       cv::Mat tmat_sorted;
00183       cv::sort(tmat, tmat_sorted, CV_SORT_EVERY_COLUMN+CV_SORT_DESCENDING);
00184       avg_disp = tmat_sorted.at<float>(floor(cv::countNonZero(tmat_sorted>=0.0)/2.0)); // Get the middle valid disparity (-1 disparities are invalid)
00185 
00186       // Fill in the rest of the face data structure.
00187       one_face.center2d = cv::Point2d(one_face.box2d.x+one_face.box2d.width/2.0,
00188                                       one_face.box2d.y+one_face.box2d.height/2.0);
00189       one_face.width2d = one_face.box2d.width;
00190 
00191       // If the median disparity was valid and the face is a reasonable size, the face status is "good".
00192       // If the median disparity was valid but the face isn't a reasonable size, the face status is "bad".
00193       // Otherwise, the face status is "unknown".
00194       if (avg_disp > 0) {
00195         cam_model_->projectDisparityTo3d(cv::Point2d(0.0,0.0),avg_disp,p3_1); 
00196         cam_model_->projectDisparityTo3d(cv::Point2d(one_face.box2d.width,0.0),avg_disp,p3_2);
00197         one_face.width3d = fabs(p3_2.x-p3_1.x);
00198         cam_model_->projectDisparityTo3d(one_face.center2d, avg_disp, one_face.center3d);
00199         if (one_face.center3d.z > max_face_z_m_ || one_face.width3d < face_size_min_m_ || one_face.width3d > face_size_max_m_) {
00200           one_face.status = "bad";
00201         }
00202       }
00203       else {
00204         one_face.width3d = 0.0;     
00205         one_face.center3d = cv::Point3d(0.0,0.0,0.0);
00206         one_face.status = "unknown";
00207       }
00208 
00209       // Add faces to the output vector.
00210       // lock faces
00211       boost::mutex::scoped_lock lock(face_mutex_);
00212       faces_.push_back(one_face);
00213       lock.unlock();
00214     }
00215 
00216     boost::mutex::scoped_lock fdmlock(face_done_mutex_);
00217     num_threads_to_wait_for_--;
00218     fdmlock.unlock();
00219     face_detection_done_cond_.notify_one();
00220   }
00221 }
00222 
00224 
00225 void Faces::initFaceDetectionDepth(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m) {
00226 
00227   images_ready_ = 0;
00228 
00229   face_size_min_m_ = face_size_min_m;
00230   face_size_max_m_ = face_size_max_m;
00231   max_face_z_m_ = max_face_z_m;
00232   face_sep_dist_m_ = face_sep_dist_m;
00233 
00234   face_go_mutex_ = new boost::mutex();
00235   bool cascade_ok = cascade_.load(haar_classifier_filename);
00236   
00237   if (!cascade_ok) {
00238     ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
00239     return;
00240   }
00241   threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDepth,this,0));
00242 }
00243 
00244 
00246 
00247 vector<Box2D3D> Faces::detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &dimage, image_geometry::StereoCameraModel *cam_model) {
00248 
00249   faces_.clear();
00250 
00251   // Convert the image to grayscale, if necessary.
00252   
00253   if (image.channels() == 1) {
00254     cv_image_gray_.create(image.size(), CV_8UC1);
00255     image.copyTo(cv_image_gray_);
00256   }
00257   else if (image.channels() == 3) {
00258     cv_image_gray_.create(image.size(), CV_8UC1);
00259     cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
00260   }
00261   else {
00262     std::cerr << "Unknown image type"<<std::endl;
00263     return faces_;
00264   }
00265 
00266   depth_image_ = &dimage;
00267   cam_model_ = cam_model;
00268 
00269   // Tell the face detection threads that they can run once.
00270   num_threads_to_wait_for_ = threads_.size();
00271   boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
00272   images_ready_++;
00273   fgmlock.unlock();
00274 
00275   face_detection_ready_cond_.notify_all();
00276 
00277   boost::mutex::scoped_lock fdmlock(face_done_mutex_);
00278   while (num_threads_to_wait_for_ > 0) {
00279     face_detection_done_cond_.wait(fdmlock);
00280   }  
00281 
00282   return faces_;
00283 }
00284 
00286 
00287 void Faces::faceDetectionThreadDepth(uint i) {
00288 
00289   while (1) {
00290     boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
00291     boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
00292     while (1) {
00293       tlock.lock();
00294       if (images_ready_) {
00295         --images_ready_;
00296         tlock.unlock();
00297         break;
00298       }
00299       tlock.unlock();
00300       face_detection_ready_cond_.wait(fgmlock);
00301     }
00302 
00303     // Find the faces using OpenCV's haar cascade object detector.
00304     cv::Point3d p3_1(0,0,max_face_z_m_);
00305     cv::Point3d p3_2(face_size_min_m_,0,max_face_z_m_);
00306     cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
00307     cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
00308     int this_min_face_size = (int)(floor(fabs(p2_2.x-p2_1.x)));
00309 
00310     std::vector<cv::Rect> faces_vec;
00311     cascade_.detectMultiScale(cv_image_gray_, faces_vec,  1.2, 2, CV_HAAR_DO_CANNY_PRUNING, cv::Size(this_min_face_size,this_min_face_size));
00312 
00313     // Filter the faces using depth information, if available. Currently checks that the actual face size is within the given limits.
00314     cv::Scalar color(0,255,0);
00315     Box2D3D one_face;
00316     double avg_d = 0.0;
00317     cv::Mat uvd(1,3,CV_32FC1);
00318     cv::Mat xyz(1,3,CV_32FC1);
00319     // For each face...
00320     for (uint iface = 0; iface < faces_vec.size(); iface++) {
00321 
00322       one_face.status = "good";
00323 
00324       one_face.box2d = faces_vec[iface];
00325       one_face.id = i; // The cascade that computed this face.
00326 
00327       // Get the median depth in the middle half of the bounding box.
00328       cv::Mat depth_roi_shallow(*depth_image_,cv::Rect(floor(one_face.box2d.x+0.25*one_face.box2d.width),  
00329                                                           floor(one_face.box2d.y+0.25*one_face.box2d.height),
00330                                                           floor(one_face.box2d.x+0.75*one_face.box2d.width) - floor(one_face.box2d.x+0.25*one_face.box2d.width) + 1,
00331                                                           floor(one_face.box2d.y+0.75*one_face.box2d.height) - floor(one_face.box2d.y+0.25*one_face.box2d.height) + 1));
00332  
00333       // Fill in the rest of the face data structure.
00334       one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width/2.0,
00335                                       one_face.box2d.y + one_face.box2d.height/2.0);
00336       one_face.width2d = one_face.box2d.width;
00337 
00338 
00339       // If the median depth was valid and the face is a reasonable size, the face status is "good".
00340       // If the median depth was valid but the face isn't a reasonable size, the face status is "bad".
00341       // Otherwise, the face status is "unknown".
00342 
00343       std::vector<float> depths;
00344       // Copy the depths removing the nans.
00345       for (int i=0; i < depth_roi_shallow.rows; i++) {
00346         const float* dptr = depth_roi_shallow.ptr<float>(i);
00347         for (int j=0; j<depth_roi_shallow.cols; j++) {
00348           if (dptr[j] == dptr[j]) {
00349             depths.push_back(dptr[j]);
00350           }
00351         }
00352       }
00353 
00354       std::vector<float>::iterator dbegin = depths.begin();
00355       std::vector<float>::iterator dend = depths.end();
00356 
00357       if (depths.size() > 0) {
00358         std::sort(dbegin, dend);
00359         avg_d = depths[floor(depths.size()/2.0)];
00360         
00361         one_face.width3d = fabs((cam_model_->left()).getDeltaX(one_face.box2d.width, avg_d));
00362         one_face.center3d = (cam_model_->left()).projectPixelTo3dRay(one_face.center2d);
00363 
00364         one_face.center3d = (avg_d/one_face.center3d.z) * one_face.center3d;
00365         if (one_face.center3d.z > max_face_z_m_ || one_face.width3d < face_size_min_m_ || one_face.width3d > face_size_max_m_) {
00366           one_face.status = "bad";
00367         }
00368       }
00369       else {
00370         one_face.width3d = 0.0;     
00371         one_face.center3d = cv::Point3d(0.0,0.0,0.0);
00372         one_face.status = "unknown";
00373       }
00374 
00375       // Add faces to the output vector.
00376       // lock faces
00377       boost::mutex::scoped_lock lock(face_mutex_);
00378       faces_.push_back(one_face);
00379       lock.unlock();
00380     }
00381 
00382     boost::mutex::scoped_lock fdmlock(face_done_mutex_);
00383     num_threads_to_wait_for_--;
00384     fdmlock.unlock();
00385     face_detection_done_cond_.notify_one();
00386   }
00387 
00388 }
00389 
00390 };


face_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Aug 27 2015 14:18:13