faces.cpp

Go to the documentation of this file.

/*********************************************************************
 * Faces-specific computer vision algorithms.
 *
 **********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Caroline Pantofaru */

#include <face_detector/faces.h>
#include <cfloat>
#include <algorithm>
#include <iostream>

#include <ros/time.h>
#include <ros/console.h>

#include <string>
#include <vector>

namespace people
{

Faces::Faces():
  list_(),
  cam_model_(NULL)
{
}

Faces::~Faces()
{
  // Kill all the threads.
  threads_.interrupt_all();
  threads_.join_all();
  delete face_go_mutex_;
  face_go_mutex_ = NULL;

  for (int i = list_.size(); i > 0; i--)
  {
    list_.pop_back();
  }

  cam_model_ = 0;
}

/* Note: The multi-threading in this file is left over from a previous incarnation that allowed multiple
 * cascades to be run at once. It hasn't been removed in case we want to return to that model, and since
 * there isn't much overhead. Right now, however, only one classifier is being run per instantiation
 * of the face_detector node.
 */

void Faces::initFaceDetectionDisparity(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m,
                                       double face_size_max_m, double max_face_z_m, double face_sep_dist_m)
{
  images_ready_ = 0;

  face_size_min_m_ = face_size_min_m;
  face_size_max_m_ = face_size_max_m;
  max_face_z_m_ = max_face_z_m;
  face_sep_dist_m_ = face_sep_dist_m;

  face_go_mutex_ = new boost::mutex();
  bool cascade_ok = cascade_.load(haar_classifier_filename);

  if (!cascade_ok)
  {
    ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
    return;
  }
  threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDisparity, this, 0));
}

std::vector<Box2D3D> Faces::detectAllFacesDisparity(const cv::Mat &image, double threshold,
                                                    const cv::Mat &disparity_image,
                                                    image_geometry::StereoCameraModel *cam_model)
{
  faces_.clear();

  // Convert the image to grayscale, if necessary.
  if (image.channels() == 1)
  {
    cv_image_gray_.create(image.size(), CV_8UC1);
    image.copyTo(cv_image_gray_);
  }
  else if (image.channels() == 3)
  {
    cv_image_gray_.create(image.size(), CV_8UC1);
    cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
  }
  else
  {
    std::cerr << "Unknown image type" << std::endl;
    return faces_;
  }

  disparity_image_ = &disparity_image;
  cam_model_ = cam_model;

  // Tell the face detection threads that they can run once.
  num_threads_to_wait_for_ = threads_.size();
  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
  images_ready_++;
  fgmlock.unlock();

  face_detection_ready_cond_.notify_all();

  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
  while (num_threads_to_wait_for_ > 0)
  {
    face_detection_done_cond_.wait(fdmlock);
  }

  return faces_;
}

void Faces::faceDetectionThreadDisparity(uint i)
{
  while (true)
  {
    boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
    boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
    while (true)
    {
      tlock.lock();
      if (images_ready_)
      {
        --images_ready_;
        tlock.unlock();
        break;
      }
      tlock.unlock();
      face_detection_ready_cond_.wait(fgmlock);
    }

    // Find the faces using OpenCV's haar cascade object detector.
    cv::Point3d p3_1(0, 0, max_face_z_m_);
    cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_);
    cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
    cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
    int this_min_face_size = static_cast<int>(floor(fabs(p2_2.x - p2_1.x)));

    std::vector<cv::Rect> faces_vec;
    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));

    // Filter the faces using depth information, if available.
    // Currently checks that the actual face size is within the given limits.
    cv::Scalar color(0, 255, 0);
    Box2D3D one_face;
    double avg_disp = 0.0;
    cv::Mat uvd(1, 3, CV_32FC1);
    cv::Mat xyz(1, 3, CV_32FC1);
    // For each face...
    for (uint iface = 0; iface < faces_vec.size(); iface++)  // face_seq->total; iface++) {
    {
      one_face.status = "good";

      one_face.box2d = faces_vec[iface];
      one_face.id = i;  // The cascade that computed this face.

      // Get the median disparity in the middle half of the bounding box.
      cv::Mat disp_roi_shallow(*disparity_image_,
          cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width),
                   floor(one_face.box2d.y + 0.25 * one_face.box2d.height),
                   floor(one_face.box2d.x + 0.75 * one_face.box2d.width) -
                      floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1,
                   floor(one_face.box2d.y + 0.75 * one_face.box2d.height) -
                      floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1));
      cv::Mat disp_roi = disp_roi_shallow.clone();
      cv::Mat tmat = disp_roi.reshape(1, disp_roi.rows * disp_roi.cols);
      cv::Mat tmat_sorted;
      cv::sort(tmat, tmat_sorted, CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING);

      // Get the middle valid disparity (-1 disparities are invalid)
      avg_disp = tmat_sorted.at<float>(floor(cv::countNonZero(tmat_sorted >= 0.0) / 2.0));

      // Fill in the rest of the face data structure.
      one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0,
                                      one_face.box2d.y + one_face.box2d.height / 2.0);
      one_face.width2d = one_face.box2d.width;

      // If the median disparity was valid and the face is a reasonable size, the face status is "good".
      // If the median disparity was valid but the face isn't a reasonable size, the face status is "bad".
      // Otherwise, the face status is "unknown".
      if (avg_disp > 0)
      {
        cam_model_->projectDisparityTo3d(cv::Point2d(0.0, 0.0), avg_disp, p3_1);
        cam_model_->projectDisparityTo3d(cv::Point2d(one_face.box2d.width, 0.0), avg_disp, p3_2);
        one_face.width3d = fabs(p3_2.x - p3_1.x);
        cam_model_->projectDisparityTo3d(one_face.center2d, avg_disp, one_face.center3d);
        if (one_face.center3d.z > max_face_z_m_ ||
            one_face.width3d < face_size_min_m_ ||
            one_face.width3d > face_size_max_m_)
        {
          one_face.status = "bad";
        }
      }
      else
      {
        one_face.width3d = 0.0;
        one_face.center3d = cv::Point3d(0.0, 0.0, 0.0);
        one_face.status = "unknown";
      }

      // Add faces to the output .
      // lock faces
      boost::mutex::scoped_lock lock(face_mutex_);
      faces_.push_back(one_face);
      lock.unlock();
    }

    boost::mutex::scoped_lock fdmlock(face_done_mutex_);
    num_threads_to_wait_for_--;
    fdmlock.unlock();
    face_detection_done_cond_.notify_one();
  }
}

void Faces::initFaceDetectionDepth(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m,
                                   double face_size_max_m, double max_face_z_m, double face_sep_dist_m)
{
  images_ready_ = 0;

  face_size_min_m_ = face_size_min_m;
  face_size_max_m_ = face_size_max_m;
  max_face_z_m_ = max_face_z_m;
  face_sep_dist_m_ = face_sep_dist_m;

  face_go_mutex_ = new boost::mutex();
  bool cascade_ok = cascade_.load(haar_classifier_filename);

  if (!cascade_ok)
  {
    ROS_ERROR_STREAM("Cascade file " << haar_classifier_filename << " doesn't exist.");
    return;
  }
  threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDepth, this, 0));
}

std::vector<Box2D3D> Faces::detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &dimage,
                                                image_geometry::StereoCameraModel *cam_model)
{
  faces_.clear();

  // Convert the image to grayscale, if necessary.

  if (image.channels() == 1)
  {
    cv_image_gray_.create(image.size(), CV_8UC1);
    image.copyTo(cv_image_gray_);
  }
  else if (image.channels() == 3)
  {
    cv_image_gray_.create(image.size(), CV_8UC1);
    cv::cvtColor(image, cv_image_gray_, CV_BGR2GRAY);
  }
  else
  {
    std::cerr << "Unknown image type" << std::endl;
    return faces_;
  }

  depth_image_ = &dimage;
  cam_model_ = cam_model;

  // Tell the face detection threads that they can run once.
  num_threads_to_wait_for_ = threads_.size();
  boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
  images_ready_++;
  fgmlock.unlock();

  face_detection_ready_cond_.notify_all();

  boost::mutex::scoped_lock fdmlock(face_done_mutex_);
  while (num_threads_to_wait_for_ > 0)
  {
    face_detection_done_cond_.wait(fdmlock);
  }

  return faces_;
}

void Faces::faceDetectionThreadDepth(uint i)
{
  while (true)
  {
    boost::mutex::scoped_lock fgmlock(*(face_go_mutex_));
    boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock);
    while (true)
    {
      tlock.lock();
      if (images_ready_)
      {
        --images_ready_;
        tlock.unlock();
        break;
      }
      tlock.unlock();
      face_detection_ready_cond_.wait(fgmlock);
    }

    // Find the faces using OpenCV's haar cascade object detector.
    cv::Point3d p3_1(0, 0, max_face_z_m_);
    cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_);
    cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1);
    cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2);
    int this_min_face_size = static_cast<int>(floor(fabs(p2_2.x - p2_1.x)));

    std::vector<cv::Rect> faces_vec;
    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));

    // Filter the faces using depth information, if available.
    // Currently checks that the actual face size is within the given limits.
    cv::Scalar color(0, 255, 0);
    Box2D3D one_face;
    double avg_d = 0.0;
    cv::Mat uvd(1, 3, CV_32FC1);
    cv::Mat xyz(1, 3, CV_32FC1);
    // For each face...
    for (uint iface = 0; iface < faces_vec.size(); iface++)
    {
      one_face.status = "good";

      one_face.box2d = faces_vec[iface];
      one_face.id = i;  // The cascade that computed this face.

      // Get the median depth in the middle half of the bounding box.
      cv::Mat depth_roi_shallow(*depth_image_,
          cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width),
                   floor(one_face.box2d.y + 0.25 * one_face.box2d.height),
                   floor(one_face.box2d.x + 0.75 * one_face.box2d.width) -
                      floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1,
                   floor(one_face.box2d.y + 0.75 * one_face.box2d.height) -
                      floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1));

      // Fill in the rest of the face data structure.
      one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0,
                                      one_face.box2d.y + one_face.box2d.height / 2.0);
      one_face.width2d = one_face.box2d.width;

      // If the median depth was valid and the face is a reasonable size, the face status is "good".
      // If the median depth was valid but the face isn't a reasonable size, the face status is "bad".
      // Otherwise, the face status is "unknown".

      std::vector<float> depths;
      // Copy the depths removing the nans.
      for (int i = 0; i < depth_roi_shallow.rows; i++)
      {
        const float* dptr = depth_roi_shallow.ptr<float>(i);
        for (int j = 0; j < depth_roi_shallow.cols; j++)
        {
          if (dptr[j] == dptr[j])
          {
            depths.push_back(dptr[j]);
          }
        }
      }

      std::vector<float>::iterator dbegin = depths.begin();
      std::vector<float>::iterator dend = depths.end();

      if (depths.size() > 0)
      {
        std::sort(dbegin, dend);
        avg_d = depths[floor(depths.size() / 2.0)];

        one_face.width3d = fabs((cam_model_->left()).getDeltaX(one_face.box2d.width, avg_d));
        one_face.center3d = (cam_model_->left()).projectPixelTo3dRay(one_face.center2d);

        one_face.center3d = (avg_d / one_face.center3d.z) * one_face.center3d;
        if (one_face.center3d.z > max_face_z_m_ ||
            one_face.width3d < face_size_min_m_ ||
            one_face.width3d > face_size_max_m_)
        {
          one_face.status = "bad";
        }
      }
      else
      {
        one_face.width3d = 0.0;
        one_face.center3d = cv::Point3d(0.0, 0.0, 0.0);
        one_face.status = "unknown";
      }

      // Add faces to the output vector.
      // lock faces
      boost::mutex::scoped_lock lock(face_mutex_);
      faces_.push_back(one_face);
      lock.unlock();
    }

    boost::mutex::scoped_lock fdmlock(face_done_mutex_);
    num_threads_to_wait_for_--;
    fdmlock.unlock();
    face_detection_done_cond_.notify_one();
  }
}
};  // namespace people