faces.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Faces-specific computer vision algorithms.
3  *
4  **********************************************************************
5  *
6  * Software License Agreement (BSD License)
7  *
8  * Copyright (c) 2008, Willow Garage, Inc.
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the Willow Garage nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *********************************************************************/
38 
39 /* Author: Caroline Pantofaru */
40 
41 #include <face_detector/faces.h>
42 #include <cfloat>
43 #include <algorithm>
44 #include <iostream>
45 
46 #include <ros/time.h>
47 #include <ros/console.h>
48 
49 #include <opencv2/imgproc/imgproc_c.h>
50 #include <string>
51 #include <vector>
52 
53 namespace people
54 {
55 
56 Faces::Faces():
57  list_(),
58  cam_model_(NULL)
59 {
60 }
61 
62 Faces::~Faces()
63 {
64  // Kill all the threads.
65  threads_.interrupt_all();
66  threads_.join_all();
67  delete face_go_mutex_;
68  face_go_mutex_ = NULL;
69 
70  for (int i = list_.size(); i > 0; i--)
71  {
72  list_.pop_back();
73  }
74 
75  cam_model_ = 0;
76 }
77 
78 /* Note: The multi-threading in this file is left over from a previous incarnation that allowed multiple
79  * cascades to be run at once. It hasn't been removed in case we want to return to that model, and since
80  * there isn't much overhead. Right now, however, only one classifier is being run per instantiation
81  * of the face_detector node.
82  */
83 
84 void Faces::initFaceDetectionDisparity(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m,
85  double face_size_max_m, double max_face_z_m, double face_sep_dist_m)
86 {
87  images_ready_ = 0;
88 
89  face_size_min_m_ = face_size_min_m;
90  face_size_max_m_ = face_size_max_m;
91  max_face_z_m_ = max_face_z_m;
92  face_sep_dist_m_ = face_sep_dist_m;
93 
94  face_go_mutex_ = new boost::mutex();
95  bool cascade_ok = cascade_.load(haar_classifier_filename);
96 
97  if (!cascade_ok)
98  {
99  ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
100  return;
101  }
102  threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDisparity, this, 0));
103 }
104 
106 std::vector<Box2D3D> Faces::detectAllFacesDisparity(const cv::Mat &image, double threshold,
107  const cv::Mat &disparity_image,
108  image_geometry::StereoCameraModel *cam_model)
109 {
110  faces_.clear();
111 
112  // Convert the image to grayscale, if necessary.
113  if (image.channels() == 1)
114  {
115  cv_image_gray_.create(image.size(), CV_8UC1);
116  image.copyTo(cv_image_gray_);
117  }
118  else if (image.channels() == 3)
119  {
120  cv_image_gray_.create(image.size(), CV_8UC1);
121  cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
122  }
123  else
124  {
125  std::cerr << "Unknown image type" << std::endl;
126  return faces_;
127  }
128 
129  disparity_image_ = &disparity_image;
130  cam_model_ = cam_model;
131 
132  // Tell the face detection threads that they can run once.
133  num_threads_to_wait_for_ = threads_.size();
134  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
135  images_ready_++;
136  fgmlock.unlock();
137 
138  face_detection_ready_cond_.notify_all();
139 
140  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
141  while (num_threads_to_wait_for_ > 0)
142  {
143  face_detection_done_cond_.wait(fdmlock);
144  }
145 
146  return faces_;
147 }
148 
150 void Faces::faceDetectionThreadDisparity(uint i)
151 {
152  while (true)
153  {
154  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
155  boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
156  while (true)
157  {
158  tlock.lock();
159  if (images_ready_)
160  {
161  --images_ready_;
162  tlock.unlock();
163  break;
164  }
165  tlock.unlock();
166  face_detection_ready_cond_.wait(fgmlock);
167  }
168 
169  // Find the faces using OpenCV's haar cascade object detector.
170  cv::Point3d p3_1(0, 0, max_face_z_m_);
171  cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_);
172  cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
173  cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
174  int this_min_face_size = static_cast<int>(floor(fabs(p2_2.x - p2_1.x)));
175 
176  std::vector<cv::Rect> faces_vec;
177  cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, cv::CASCADE_DO_CANNY_PRUNING,
178  cv::Size(this_min_face_size, this_min_face_size));
179 
180  // Filter the faces using depth information, if available.
181  // Currently checks that the actual face size is within the given limits.
182  cv::Scalar color(0, 255, 0);
183  Box2D3D one_face;
184  double avg_disp = 0.0;
185  cv::Mat uvd(1, 3, CV_32FC1);
186  cv::Mat xyz(1, 3, CV_32FC1);
187  // For each face...
188  for (uint iface = 0; iface < faces_vec.size(); iface++) // face_seq->total; iface++) {
189  {
190  one_face.status = "good";
191 
192  one_face.box2d = faces_vec[iface];
193  one_face.id = i; // The cascade that computed this face.
194 
195  // Get the median disparity in the middle half of the bounding box.
196  cv::Mat disp_roi_shallow(*disparity_image_,
197  cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width),
198  floor(one_face.box2d.y + 0.25 * one_face.box2d.height),
199  floor(one_face.box2d.x + 0.75 * one_face.box2d.width) -
200  floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1,
201  floor(one_face.box2d.y + 0.75 * one_face.box2d.height) -
202  floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1));
203  cv::Mat disp_roi = disp_roi_shallow.clone();
204  cv::Mat tmat = disp_roi.reshape(1, disp_roi.rows * disp_roi.cols);
205  cv::Mat tmat_sorted;
206  cv::sort(tmat, tmat_sorted, CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING);
207 
208  // Get the middle valid disparity (-1 disparities are invalid)
209  avg_disp = tmat_sorted.at<float>(floor(cv::countNonZero(tmat_sorted >= 0.0) / 2.0));
210 
211  // Fill in the rest of the face data structure.
212  one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0,
213  one_face.box2d.y + one_face.box2d.height / 2.0);
214  one_face.width2d = one_face.box2d.width;
215 
216  // If the median disparity was valid and the face is a reasonable size, the face status is "good".
217  // If the median disparity was valid but the face isn't a reasonable size, the face status is "bad".
218  // Otherwise, the face status is "unknown".
219  if (avg_disp > 0)
220  {
221  cam_model_->projectDisparityTo3d(cv::Point2d(0.0, 0.0), avg_disp, p3_1);
222  cam_model_->projectDisparityTo3d(cv::Point2d(one_face.box2d.width, 0.0), avg_disp, p3_2);
223  one_face.width3d = fabs(p3_2.x - p3_1.x);
224  cam_model_->projectDisparityTo3d(one_face.center2d, avg_disp, one_face.center3d);
225  if (one_face.center3d.z > max_face_z_m_ ||
226  one_face.width3d < face_size_min_m_ ||
227  one_face.width3d > face_size_max_m_)
228  {
229  one_face.status = "bad";
230  }
231  }
232  else
233  {
234  one_face.width3d = 0.0;
235  one_face.center3d = cv::Point3d(0.0, 0.0, 0.0);
236  one_face.status = "unknown";
237  }
238 
239  // Add faces to the output .
240  // lock faces
241  boost::mutex::scoped_lock lock(face_mutex_);
242  faces_.push_back(one_face);
243  lock.unlock();
244  }
245 
246  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
247  num_threads_to_wait_for_--;
248  fdmlock.unlock();
249  face_detection_done_cond_.notify_one();
250  }
251 }
252 
254 void Faces::initFaceDetectionDepth(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m,
255  double face_size_max_m, double max_face_z_m, double face_sep_dist_m)
256 {
257  images_ready_ = 0;
258 
259  face_size_min_m_ = face_size_min_m;
260  face_size_max_m_ = face_size_max_m;
261  max_face_z_m_ = max_face_z_m;
262  face_sep_dist_m_ = face_sep_dist_m;
263 
264  face_go_mutex_ = new boost::mutex();
265  bool cascade_ok = cascade_.load(haar_classifier_filename);
266 
267  if (!cascade_ok)
268  {
269  ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
270  return;
271  }
272  threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDepth, this, 0));
273 }
274 
276 std::vector<Box2D3D> Faces::detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &dimage,
277  image_geometry::StereoCameraModel *cam_model)
278 {
279  faces_.clear();
280 
281  // Convert the image to grayscale, if necessary.
282 
283  if (image.channels() == 1)
284  {
285  cv_image_gray_.create(image.size(), CV_8UC1);
286  image.copyTo(cv_image_gray_);
287  }
288  else if (image.channels() == 3)
289  {
290  cv_image_gray_.create(image.size(), CV_8UC1);
291  cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
292  }
293  else
294  {
295  std::cerr << "Unknown image type" << std::endl;
296  return faces_;
297  }
298 
299  depth_image_ = &dimage;
300  cam_model_ = cam_model;
301 
302  // Tell the face detection threads that they can run once.
303  num_threads_to_wait_for_ = threads_.size();
304  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
305  images_ready_++;
306  fgmlock.unlock();
307 
308  face_detection_ready_cond_.notify_all();
309 
310  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
311  while (num_threads_to_wait_for_ > 0)
312  {
313  face_detection_done_cond_.wait(fdmlock);
314  }
315 
316  return faces_;
317 }
318 
320 void Faces::faceDetectionThreadDepth(uint i)
321 {
322  while (true)
323  {
324  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
325  boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
326  while (true)
327  {
328  tlock.lock();
329  if (images_ready_)
330  {
331  --images_ready_;
332  tlock.unlock();
333  break;
334  }
335  tlock.unlock();
336  face_detection_ready_cond_.wait(fgmlock);
337  }
338 
339  // Find the faces using OpenCV's haar cascade object detector.
340  cv::Point3d p3_1(0, 0, max_face_z_m_);
341  cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_);
342  cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
343  cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
344  int this_min_face_size = static_cast<int>(floor(fabs(p2_2.x - p2_1.x)));
345 
346  std::vector<cv::Rect> faces_vec;
347  cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, cv::CASCADE_DO_CANNY_PRUNING,
348  cv::Size(this_min_face_size, this_min_face_size));
349 
350  // Filter the faces using depth information, if available.
351  // Currently checks that the actual face size is within the given limits.
352  cv::Scalar color(0, 255, 0);
353  Box2D3D one_face;
354  double avg_d = 0.0;
355  cv::Mat uvd(1, 3, CV_32FC1);
356  cv::Mat xyz(1, 3, CV_32FC1);
357  // For each face...
358  for (uint iface = 0; iface < faces_vec.size(); iface++)
359  {
360  one_face.status = "good";
361 
362  one_face.box2d = faces_vec[iface];
363  one_face.id = i; // The cascade that computed this face.
364 
365  // Get the median depth in the middle half of the bounding box.
366  cv::Mat depth_roi_shallow(*depth_image_,
367  cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width),
368  floor(one_face.box2d.y + 0.25 * one_face.box2d.height),
369  floor(one_face.box2d.x + 0.75 * one_face.box2d.width) -
370  floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1,
371  floor(one_face.box2d.y + 0.75 * one_face.box2d.height) -
372  floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1));
373 
374  // Fill in the rest of the face data structure.
375  one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0,
376  one_face.box2d.y + one_face.box2d.height / 2.0);
377  one_face.width2d = one_face.box2d.width;
378 
379  // If the median depth was valid and the face is a reasonable size, the face status is "good".
380  // If the median depth was valid but the face isn't a reasonable size, the face status is "bad".
381  // Otherwise, the face status is "unknown".
382 
383  std::vector<float> depths;
384  // Copy the depths removing the nans.
385  for (int i = 0; i < depth_roi_shallow.rows; i++)
386  {
387  const float* dptr = depth_roi_shallow.ptr<float>(i);
388  for (int j = 0; j < depth_roi_shallow.cols; j++)
389  {
390  if (dptr[j] == dptr[j])
391  {
392  depths.push_back(dptr[j]);
393  }
394  }
395  }
396 
397  std::vector<float>::iterator dbegin = depths.begin();
398  std::vector<float>::iterator dend = depths.end();
399 
400  if (depths.size() > 0)
401  {
402  std::sort(dbegin, dend);
403  avg_d = depths[floor(depths.size() / 2.0)];
404 
405  one_face.width3d = fabs((cam_model_->left()).getDeltaX(one_face.box2d.width, avg_d));
406  one_face.center3d = (cam_model_->left()).projectPixelTo3dRay(one_face.center2d);
407 
408  one_face.center3d = (avg_d / one_face.center3d.z) * one_face.center3d;
409  if (one_face.center3d.z > max_face_z_m_ ||
410  one_face.width3d < face_size_min_m_ ||
411  one_face.width3d > face_size_max_m_)
412  {
413  one_face.status = "bad";
414  }
415  }
416  else
417  {
418  one_face.width3d = 0.0;
419  one_face.center3d = cv::Point3d(0.0, 0.0, 0.0);
420  one_face.status = "unknown";
421  }
422 
423  // Add faces to the output vector.
424  // lock faces
425  boost::mutex::scoped_lock lock(face_mutex_);
426  faces_.push_back(one_face);
427  lock.unlock();
428  }
429 
430  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
431  num_threads_to_wait_for_--;
432  fdmlock.unlock();
433  face_detection_done_cond_.notify_one();
434  }
435 }
436 }; // namespace people
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
time.h
console.h
people
Definition: faces.h:62
image_geometry::StereoCameraModel
faces.h
people::Faces::Faces
Faces()
Definition: faces.cpp:92

face_detector
Author(s): Caroline Pantofaru
autogenerated on Wed Mar 2 2022 00:45:07