$search
00001 /********************************************************************* 00002 * Face-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 * Modified for Kinect/Point Cloud support: Richard Bormann */ 00041 00042 #ifndef FACES_H 00043 #define FACES_H 00044 00045 00046 00047 #include <stdio.h> 00048 #include <stdlib.h> 00049 #include <iostream> 00050 #include <vector> 00051 00052 #include <opencv/cv.hpp> 00053 #include <opencv/cxcore.hpp> 00054 #include <opencv/cvaux.hpp> 00055 00056 #include "image_geometry/stereo_camera_model.h" 00057 #include "ros/time.h" 00058 #include <boost/thread/mutex.hpp> 00059 #include <boost/bind.hpp> 00060 #include <boost/thread/thread.hpp> 00061 #include <boost/thread/condition.hpp> 00062 00063 #include <pcl/point_types.h> 00064 #include <pcl_ros/point_cloud.h> 00065 00066 using namespace std; 00067 00068 00069 namespace people { 00070 00071 00075 struct Box2D3D { 00076 cv::Point2d center2d; 00077 cv::Point3d center3d; 00078 double radius2d; 00079 double radius3d; 00080 cv::Rect box2d; 00081 string status; 00082 int id; 00083 }; 00084 00085 00089 struct Face { 00090 string id; 00091 string name; 00092 }; 00093 00094 00099 class Faces 00100 { 00101 public: 00102 00103 // Default thresholds for the face detection algorithm. 00104 static const double FACE_SIZE_MIN_M=0.1; 00105 static const double FACE_SIZE_MAX_M=0.5; 00106 static const double MAX_FACE_Z_M=8.0; 00107 // Default thresholds for face tracking. 00108 static const double FACE_SEP_DIST_M=1.0; 00110 // Thresholds for the face detection algorithm. 00111 double face_size_min_m_; 00112 double face_size_max_m_; 00113 double max_face_z_m_; 00114 // Thresholds for face tracking. 00115 double face_sep_dist_m_; 00119 // Create an empty list of people. 00120 Faces(); 00121 00122 // Destroy a list of people. 00123 ~Faces(); 00124 00125 00138 vector<Box2D3D> detectAllFaces(cv::Mat &image, double threshold, cv::Mat &disparity_image, image_geometry::StereoCameraModel *cam_model, pcl::PointCloud<pcl::PointXYZ>* depth_cloud = 0); 00139 00148 void initFaceDetection(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); 00149 00151 private: 00152 00153 vector<Face> list_; 00154 vector<Box2D3D> faces_; 00156 cv::Mat cv_image_gray_; 00157 cv::Mat *disparity_image_; 00158 pcl::PointCloud<pcl::PointXYZ>* depth_cloud_; 00159 image_geometry::StereoCameraModel *cam_model_; 00161 boost::mutex face_mutex_, face_done_mutex_, t_mutex_; 00162 boost::mutex* face_go_mutex_; 00163 boost::thread_group threads_; 00164 boost::condition face_detection_ready_cond_, face_detection_done_cond_; 00165 int num_threads_to_wait_for_; 00166 int images_ready_; 00167 00168 /* Structures for the face detector. */ 00169 cv::CascadeClassifier cascade_; 00171 void faceDetectionThread(uint i); 00172 00173 }; 00174 00175 }; 00176 00177 #endif 00178