$search

people_detection.cpp

Go to the documentation of this file.

/****************************************************************
 * 
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob_vision
 * ROS package name: cob_people_detection
 * Description: Takes the positions of detected or recognized faces and
 * a people segmentation of the environment as input and outputs people
 * positions.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
 * Author: Richard Bormann, email: richard.bormann@ipa.fhg.de
 * Supervised by:
 *
 * Date of creation: 03/2011
 * ToDo:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * 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 Fraunhofer Institute for Manufacturing
 * Engineering and Automation (IPA) nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

#ifndef _PEOPLE_DETECTION_
#define _PEOPLE_DETECTION_


//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <ros/package.h>

// ROS message includes
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <cob_people_detection_msgs/PeopleDetectionArray.h>

// services
#include <cob_people_detection/DetectPeople.h>

// topics
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

// opencv
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

// point cloud
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

// boost
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>

// external includes
#include "cob_vision_utils/GlobalDefines.h"


#include <sstream>
#include <string>
#include <vector>

#include <pluginlib/class_list_macros.h>

namespace ipa_PeopleDetector {
  
  
  //####################
  //#### node class ####
  class CobPeopleDetectionNodelet : public nodelet::Nodelet
  {
  protected:
    //message_filters::Subscriber<sensor_msgs::PointCloud2> shared_image_sub_; ///< Shared xyz image and color image topic
    image_transport::ImageTransport* it_;
    image_transport::SubscriberFilter people_segmentation_image_sub_; 
    image_transport::SubscriberFilter color_image_sub_; 
    message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_people_detection_msgs::PeopleDetectionArray, sensor_msgs::Image> >* sync_input_2_;
    message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_people_detection_msgs::PeopleDetectionArray, sensor_msgs::Image, sensor_msgs::Image> >* sync_input_3_;
    message_filters::Subscriber<cob_people_detection_msgs::PeopleDetectionArray> face_position_subscriber_; 
    ros::Publisher face_position_publisher_; 
    image_transport::Publisher people_detection_image_pub_; 
    ros::ServiceServer service_server_detect_people_; 
    
    
    ros::NodeHandle node_handle_; 
    
    std::vector<cob_people_detection_msgs::PeopleDetection> face_position_accumulator_; 
    boost::timed_mutex face_position_accumulator_mutex_; 
    std::vector<std::map<std::string, double> > face_identification_votes_; 
    
    // parameters
    bool display_; 
    bool use_people_segmentation_; 
    double face_redetection_time_; 
    double min_segmented_people_ratio_color_; 
    double min_segmented_people_ratio_range_; 
    double tracking_range_m_; 
    double face_identification_score_decay_rate_; 
    double min_face_identification_score_to_publish_; 
    bool fall_back_to_unknown_identification_; 
    
  public:
    
    CobPeopleDetectionNodelet()
    {
      it_ = 0;
      sync_input_2_ = 0;
      sync_input_3_ = 0;
    }
    
    ~CobPeopleDetectionNodelet()
    {
      if (it_ != 0) delete it_;
      if (sync_input_2_ != 0) delete sync_input_2_;
      if (sync_input_3_ != 0) delete sync_input_3_;
    }
    
    void onInit()
    {
      node_handle_ = getNodeHandle();
      
      // parameters
      std::cout << "\n---------------------------\nPeople Detection Parameters:\n---------------------------\n";
      node_handle_.param("display", display_, true);
      std::cout << "display = " << display_ << "\n";
      node_handle_.param("face_redetection_time", face_redetection_time_, 2.0);
      std::cout << "face_redetection_time = " << face_redetection_time_ << "\n";
      node_handle_.param("min_segmented_people_ratio_color", min_segmented_people_ratio_color_, 0.7);
      std::cout << "min_segmented_people_ratio_color = " << min_segmented_people_ratio_color_ << "\n";
      node_handle_.param("min_segmented_people_ratio_range", min_segmented_people_ratio_range_, 0.2);
      std::cout << "min_segmented_people_ratio_range = " << min_segmented_people_ratio_range_ << "\n";
      node_handle_.param("use_people_segmentation", use_people_segmentation_, true);
      std::cout << "use_people_segmentation = " << use_people_segmentation_ << "\n";
      node_handle_.param("tracking_range_m", tracking_range_m_, 0.3);
      std::cout << "tracking_range_m = " << tracking_range_m_ << "\n";
      node_handle_.param("face_identification_score_decay_rate", face_identification_score_decay_rate_, 0.9);
      std::cout << "face_identification_score_decay_rate = " << face_identification_score_decay_rate_ << "\n";
      node_handle_.param("min_face_identification_score_to_publish", min_face_identification_score_to_publish_, 0.9);
      std::cout << "min_face_identification_score_to_publish = " << min_face_identification_score_to_publish_ << "\n";
      node_handle_.param("fall_back_to_unknown_identification", fall_back_to_unknown_identification_, true);
      std::cout << "fall_back_to_unknown_identification = " << fall_back_to_unknown_identification_ << "\n";
      
      // subscribers
      it_ = new image_transport::ImageTransport(node_handle_);
      people_segmentation_image_sub_.subscribe(*it_, "people_segmentation_image", 1);
      if (display_==true) color_image_sub_.subscribe(*it_, "colorimage", 1);
      face_position_subscriber_.subscribe(node_handle_, "face_position_array_in", 1);
      
      // input synchronization
      sensor_msgs::Image::ConstPtr nullPtr;
      if (use_people_segmentation_ == true)
      {
        if (display_ == false)
        {
          sync_input_2_ = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_people_detection_msgs::PeopleDetectionArray, sensor_msgs::Image> >(2);
          sync_input_2_->connectInput(face_position_subscriber_, people_segmentation_image_sub_);
          sync_input_2_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, _2, nullPtr));
        }
        else
        {
          sync_input_3_ = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_people_detection_msgs::PeopleDetectionArray, sensor_msgs::Image, sensor_msgs::Image> >(3);
          sync_input_3_->connectInput(face_position_subscriber_, people_segmentation_image_sub_, color_image_sub_);
          sync_input_3_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, _2, _3));
        }
      }
      else
      {
        if (display_ == true)
        {
          sync_input_2_ = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_people_detection_msgs::PeopleDetectionArray, sensor_msgs::Image> >(2);
          sync_input_2_->connectInput(face_position_subscriber_, color_image_sub_);
          sync_input_2_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, nullPtr, _2));
        }
        else
        {
          face_position_subscriber_.registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, nullPtr, nullPtr));
        }
      }
      
      // services
      service_server_detect_people_ = node_handle_.advertiseService("detect_people", &CobPeopleDetectionNodelet::detectPeopleCallback, this);
      
      // publishers
      face_position_publisher_ = node_handle_.advertise<cob_people_detection_msgs::PeopleDetectionArray>("face_position_array", 1);
      people_detection_image_pub_ = it_->advertise("people_detection_image", 1);
      
      std::cout << "CobPeopleDetectionNodelet initialized.\n";
      
      //ros::spin(); not necessary with nodelets
    }
    
    
    unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image)
    {
      try
      {
        image_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8);
      }
      catch (cv_bridge::Exception& e)
      {
        ROS_ERROR("PeopleDetection: cv_bridge exception: %s", e.what());
        return ipa_Utils::RET_FAILED;
      }
      image = image_ptr->image;
      
      return ipa_Utils::RET_OK;
    }
    
    
    unsigned long copyDetection(const cob_people_detection_msgs::PeopleDetection& src, cob_people_detection_msgs::PeopleDetection& dest, bool update=false, unsigned int updateIndex=UINT_MAX)
    {
      // 2D image coordinates
      dest.mask.roi.x = src.mask.roi.x; dest.mask.roi.y = src.mask.roi.y;
      dest.mask.roi.width = src.mask.roi.width; dest.mask.roi.height = src.mask.roi.height;
      
      // 3D world coordinates
      const geometry_msgs::Point* pointIn = &(src.pose.pose.position);
      geometry_msgs::Point* pointOut = &(dest.pose.pose.position);
      pointOut->x = pointIn->x; pointOut->y = pointIn->y; pointOut->z = pointIn->z;
      
      // person ID
      if (update==true)
      {
        // update label history
        // if (src.label!="No face")
        // {
          if (face_identification_votes_[updateIndex].find(src.label) == face_identification_votes_[updateIndex].end())
            face_identification_votes_[updateIndex][src.label] = 1.0;
          else
            face_identification_votes_[updateIndex][src.label] += 1.0;
          // }
            
            // apply voting decay with time and find most voted label
            double max_score = 0;
            dest.label = src.label;
            for (std::map<std::string, double>::iterator face_identification_votes_it=face_identification_votes_[updateIndex].begin(); face_identification_votes_it!=face_identification_votes_[updateIndex].end(); face_identification_votes_it++)
            {
              face_identification_votes_it->second *= face_identification_score_decay_rate_;
              std::string label = face_identification_votes_it->first;
              if (((label!="Unknown" && fall_back_to_unknown_identification_==false) || fall_back_to_unknown_identification_==true)
                && label!="UnknownRange" /*&& label!="No face"*/ && face_identification_votes_it->second > max_score)
                {
                  max_score = face_identification_votes_it->second;
                  dest.label = label;
                }
            }
            
            // if the score for the assigned label is higher than the score for UnknownRange increase the score for UnknownRange to the label's score (allows smooth transition if only the range detection is available after recognition)
            if (face_identification_votes_[updateIndex][dest.label] > face_identification_votes_[updateIndex]["UnknownRange"])
              face_identification_votes_[updateIndex]["UnknownRange"] = face_identification_votes_[updateIndex][dest.label];
            if (fall_back_to_unknown_identification_==false)
            {
              if (face_identification_votes_[updateIndex]["Unknown"] > face_identification_votes_[updateIndex]["UnknownRange"])
                face_identification_votes_[updateIndex]["UnknownRange"] = face_identification_votes_[updateIndex]["Unknown"];
            }
      }
      else dest.label = src.label;
      
      if (dest.label=="UnknownRange") dest.label = "Unknown";
      
      // time stamp, detector (color or range)
      // if (update==true)
      // {
        // if (src.detector=="color") dest.detector = "color";
      // }
      // else dest.detector = src.detector;
      dest.detector = src.detector;
      
      dest.header.stamp = ros::Time::now();
      
      return ipa_Utils::RET_OK;
    }
    
    inline double abs(double x) { return ((x<0) ? -x : x); }
    
    
    double computeFacePositionDistance(const cob_people_detection_msgs::PeopleDetection& previous_detection, const cob_people_detection_msgs::PeopleDetection& current_detection)
    {
      const geometry_msgs::Point* point_1 = &(previous_detection.pose.pose.position);
      const geometry_msgs::Point* point_2 = &(current_detection.pose.pose.position);
      
      double dx = abs(point_1->x - point_2->x);
      double dy = abs(point_1->y - point_2->y);
      double dz = abs(point_1->z - point_2->z);
      
      // return a huge distance if the current face position is too far away from the recent
      if (dx>tracking_range_m_ || dy>tracking_range_m_ || dz>tracking_range_m_)
      {
        // new face position is too distant to recent position
        return DBL_MAX;
      }
      
      // return Euclidian distance if both faces are sufficiently close
      double dist = dx*dx+dy*dy+dz*dz;
      return dist;
    }
    
    
    unsigned long removeMultipleInstancesOfLabel()
    {
      // check this for each recognized face
      for (int i=0; i<(int)face_position_accumulator_.size(); i++)
      {
        // label of this detection
        std::string label = face_position_accumulator_[i].label;
        
        // check whether this label has multiple occurrences if it is a real name
        if (label!="Unknown" && label!="No face")
        {
          for (int j=0; j<(int)face_position_accumulator_.size(); j++)
          {
            if (j==i) continue; // do not check yourself

            if (face_position_accumulator_[j].label == label)
            {
              if (display_) std::cout << "face_identification_votes_[i][" << label << "] = " << face_identification_votes_[i][label] << " face_identification_votes_[j][" << label << "] = " << face_identification_votes_[j][label] << "\n";
                            
                            // correct this label to Unknown when some other instance has a higher score on this label
              if (face_identification_votes_[i][label] < face_identification_votes_[j][label])
              {
                face_position_accumulator_[i].label = "Unknown";
                // copy score to unknown if it is higher (this enables the display if either the unknown score or label's recognition score were high enough)
                if (face_identification_votes_[i][label] > face_identification_votes_[i]["Unknown"])
                  face_identification_votes_[i]["Unknown"] = face_identification_votes_[i][label];
              }
            }
          }
        }
      }
      
      return ipa_Utils::RET_OK;
    }
    
    
    unsigned long prepareFacePositionMessage(cob_people_detection_msgs::PeopleDetectionArray& face_position_msg_out)
    {
        // publish face positions
        std::vector<cob_people_detection_msgs::PeopleDetection> faces_to_publish;
        for (int i=0; i<(int)face_position_accumulator_.size(); i++)
        {
          if (display_) std::cout << "'UnknownRange' score: " << face_identification_votes_[i]["UnknownRange"] << " label '" << face_position_accumulator_[i].label << "' score: " << face_identification_votes_[i][face_position_accumulator_[i].label] << " - ";
                        if (face_identification_votes_[i][face_position_accumulator_[i].label]>min_face_identification_score_to_publish_ || face_identification_votes_[i]["UnknownRange"]>min_face_identification_score_to_publish_)
                        {
                                faces_to_publish.push_back(face_position_accumulator_[i]);
                                if (display_) std::cout << "published\n";
                        }
                        else if (display_) std::cout << "not published\n";
        }
        face_position_msg_out.detections = faces_to_publish;
        // hack: for WimiCare replace 'Unknown' by '0000'
        for (int i=0; i<(int)face_position_msg_out.detections.size(); i++)
        {
          if (face_position_msg_out.detections[i].label=="Unknown")
                  face_position_msg_out.detections[i].label = "0000";
        }
        face_position_msg_out.header.stamp = ros::Time::now();

        return ipa_Utils::RET_OK;
    }


    void inputCallback(const cob_people_detection_msgs::PeopleDetectionArray::ConstPtr& face_position_msg_in, const sensor_msgs::Image::ConstPtr& people_segmentation_image_msg, const sensor_msgs::Image::ConstPtr& color_image_msg)
    {
      // convert segmentation image to cv::Mat
      cv_bridge::CvImageConstPtr people_segmentation_image_ptr;
      cv::Mat people_segmentation_image;
      if (use_people_segmentation_ == true) convertColorImageMessageToMat(people_segmentation_image_msg, people_segmentation_image_ptr, people_segmentation_image);
      
      if (display_) std::cout << "detections: " << face_position_msg_in->detections.size() << "\n";
      
      // source image size
      std::stringstream ss;
      ss << face_position_msg_in->header.frame_id;
      int width, height;
      ss >> width;
      ss >> height;
      
      // delete old face positions in list
      ros::Duration timeSpan(face_redetection_time_);
      for (int i=0; i<(int)face_position_accumulator_.size(); i++)
      {
        if ((ros::Time::now()-face_position_accumulator_[i].header.stamp) > timeSpan)
        {
          face_position_accumulator_.erase(face_position_accumulator_.begin()+i);
          face_identification_votes_.erase(face_identification_votes_.begin()+i);
        }
      }
      if (display_) std::cout << "Old face positions deleted.\n";
      
      // secure this section with a mutex
      boost::timed_mutex::scoped_timed_lock lock(face_position_accumulator_mutex_, boost::posix_time::milliseconds(2000));
      if (lock.owns_lock() == false)
      {
        ROS_ERROR("cob_people_detection::CobPeopleDetectionNodelet: Mutex was not freed during response time.");
        return;
      }
      if (display_) std::cout << "Got mutex.\n";
      
      // verify face detections with people segmentation if enabled -> only accept detected faces if a person is segmented at that position
      std::vector<int> face_detection_indices; // index set for face_position_msg_in: contains those indices of detected faces which are supported by a person segmentation at the same location
      if (use_people_segmentation_ == true)
      {
        for(int i=0; i<(int)face_position_msg_in->detections.size(); i++)
        {
          const cob_people_detection_msgs::PeopleDetection* const det_in = &(face_position_msg_in->detections[i]);
          cv::Rect face;
          face.x = det_in->mask.roi.x;
          face.y = det_in->mask.roi.y;
          face.width = det_in->mask.roi.width;
          face.height = det_in->mask.roi.height;
          
          int numberBlackPixels = 0;
          //std::cout << "face: " << face.x << " " << face.y << " " << face.width << " " << face.height << "\n";
          for (int v=face.y; v<face.y+face.height; v++)
          {
            uchar* data = people_segmentation_image.ptr(v);
            for (int u=face.x; u<face.x+face.width; u++)
            {
              int index = 3*u;
              if (data[index]==0 && data[index+1]==0 && data[index+2]==0) numberBlackPixels++;
            }
          }
          int faceArea = face.height * face.width;
          double segmentedPeopleRatio = (double)(faceArea-numberBlackPixels)/(double)faceArea;
          
          // if (display_) std::cout << "ratio: " << segmentedPeopleRatio << "\n";
          if ((det_in->detector=="color" && segmentedPeopleRatio < min_segmented_people_ratio_color_) || (det_in->detector=="range" && segmentedPeopleRatio < min_segmented_people_ratio_range_))
          {
            // False detection
            if (display_) std::cout << "False detection\n";
          }
          else
          {
            // Push index of this detection into the list
            face_detection_indices.push_back(i);
          }
        }
                  if (display_) std::cout << "Verification with people segmentation done.\n";
      }
      else
      {
        for (unsigned int i=0; i<face_position_msg_in->detections.size(); i++)
          face_detection_indices.push_back(i);
      }
      // match current detections with previous detections
      // build distance matrix
      std::map<int, std::map<int, double> > distance_matrix; // 1. index = face_position_accumulator_ index of previous detections, 2. index = index of current detections, content = spatial distance between the indexed faces
      std::map<int, std::map<int, double> >::iterator distance_matrix_it;
      for (unsigned int previous_det=0; previous_det<face_position_accumulator_.size(); previous_det++)
      {
        std::map<int, double> distance_row;
        for (unsigned int i=0; i<face_detection_indices.size(); i++)
          distance_row[face_detection_indices[i]] = computeFacePositionDistance(face_position_accumulator_[previous_det], face_position_msg_in->detections[face_detection_indices[i]]);
        distance_matrix[previous_det] = distance_row;
      }
      if (display_) std::cout << "Distance matrix.\n";
      
      // find matching faces between previous and current detections
      unsigned int number_matchings = (face_position_accumulator_.size() < face_detection_indices.size()) ? face_position_accumulator_.size() : face_detection_indices.size();
      std::vector<bool> current_detection_has_matching(face_detection_indices.size(), false); // index set for face_detection_indices: contains the indices of face_detection_indices and whether these faces could be matched to previously found faces
      for (unsigned int m=0; m<number_matchings; m++)
      {
        // find minimum matching distance between any two elements of the distance_matrix
        double min_dist = DBL_MAX;
        int previous_min_index, current_min_index;
        for (distance_matrix_it=distance_matrix.begin(); distance_matrix_it!=distance_matrix.end(); distance_matrix_it++)
        {
          for (std::map<int, double>::iterator distance_row_it=distance_matrix_it->second.begin(); distance_row_it!=distance_matrix_it->second.end(); distance_row_it++)
          {
            if (distance_row_it->second < min_dist)
            {
              min_dist = distance_row_it->second;
              previous_min_index = distance_matrix_it->first;
              current_min_index = distance_row_it->first;
            }
          }
        }
        
        // if there is no matching pair of detections interrupt the search for matchings at this point
        if (min_dist == DBL_MAX) break;
                                                       
                                                       // instantiate the matching
        copyDetection(face_position_msg_in->detections[current_min_index], face_position_accumulator_[previous_min_index], true, previous_min_index);
        // mark the respective entry in face_detection_indices as labeled
        for (unsigned int i=0; i<face_detection_indices.size(); i++)
          if (face_detection_indices[i] == current_min_index)
            current_detection_has_matching[i] = true;
          
          // delete the row and column of the found matching so that both detections cannot be involved in any further matching again
            distance_matrix.erase(previous_min_index);
            for (distance_matrix_it=distance_matrix.begin(); distance_matrix_it!=distance_matrix.end(); distance_matrix_it++)
              distance_matrix_it->second.erase(current_min_index);
      }
      if (display_) std::cout << "Matches found.\n";
                                                       
                                                       // create new detections for the unmatched of the current detections if they originate from the color image
      for (unsigned int i=0; i<face_detection_indices.size(); i++)
      {
        if (current_detection_has_matching[i] == false)
        {
          const cob_people_detection_msgs::PeopleDetection* const det_in = &(face_position_msg_in->detections[face_detection_indices[i]]);
          if (det_in->detector=="color")
          {
            // save in accumulator
            if (display_) std::cout << "\n***** New detection *****\n\n";
            cob_people_detection_msgs::PeopleDetection det_out;
            copyDetection(*det_in, det_out, false);
            det_out.pose.header.frame_id = "head_cam3d_link";
            face_position_accumulator_.push_back(det_out);
            // remember label history
            std::map<std::string, double> new_identification_data;
            //new_identification_data["Unknown"] = 0.0;
            new_identification_data["UnknownRange"] = 0.0;
            new_identification_data[det_in->label] = 1.0;
            face_identification_votes_.push_back(new_identification_data);
          }
        }
      }
      if (display_) std::cout << "New detections.\n";
                                                       
                                                       // eliminate multiple instances of a label
      removeMultipleInstancesOfLabel();
      
      // publish face positions
      cob_people_detection_msgs::PeopleDetectionArray face_position_msg_out;
/*      std::vector<cob_people_detection_msgs::PeopleDetection> faces_to_publish;
      for (int i=0; i<(int)face_position_accumulator_.size(); i++)
      {
        if (display_) std::cout << "'UnknownRange' score: " << face_identification_votes_[i]["UnknownRange"] << " label '" << face_position_accumulator_[i].label << "' score: " << face_identification_votes_[i][face_position_accumulator_[i].label] << " - ";
                if (face_identification_votes_[i][face_position_accumulator_[i].label]>min_face_identification_score_to_publish_ || face_identification_votes_[i]["UnknownRange"]>min_face_identification_score_to_publish_)
                {
                        faces_to_publish.push_back(face_position_accumulator_[i]);
                        if (display_) std::cout << "published\n";
                }
                else if (display_) std::cout << "not published\n";
      }
      face_position_msg_out.detections = faces_to_publish;
      // hack: for WimiCare replace 'Unknown' by '0000'
      for (int i=0; i<(int)face_position_msg_out.detections.size(); i++)
      {
          if (face_position_msg_out.detections[i].label=="Unknown")
                  face_position_msg_out.detections[i].label = "0000";
      }
      face_position_msg_out.header.stamp = ros::Time::now();
      */
      prepareFacePositionMessage(face_position_msg_out);
      face_position_publisher_.publish(face_position_msg_out);
      
      // display
      if (display_ == true)
      {
        // convert image message to cv::Mat
        cv_bridge::CvImageConstPtr colorImagePtr;
        cv::Mat colorImage;
        convertColorImageMessageToMat(color_image_msg, colorImagePtr, colorImage);
        
        // display color image
        for(int i=0; i<(int)face_position_msg_out.detections.size(); i++)
        {
          cv::Rect face;
          cob_people_detection_msgs::Rect& faceRect = face_position_msg_out.detections[i].mask.roi;
          face.x = faceRect.x; face.width = faceRect.width;
          face.y = faceRect.y; face.height = faceRect.height;
          
          if (face_position_msg_out.detections[i].detector == "range")
            cv::rectangle(colorImage, cv::Point(face.x, face.y), cv::Point(face.x + face.width, face.y + face.height), CV_RGB(0, 0, 255), 2, 8, 0);
          else
            cv::rectangle(colorImage, cv::Point(face.x, face.y), cv::Point(face.x + face.width, face.y + face.height), CV_RGB(0, 255, 0), 2, 8, 0);
          
          if (face_position_msg_out.detections[i].label == "Unknown")
            // Distance to face class is too high
            cv::putText(colorImage, "Unknown", cv::Point(face.x,face.y+face.height+25), cv::FONT_HERSHEY_PLAIN, 2, CV_RGB( 255, 0, 0 ), 2);
          else if (face_position_msg_out.detections[i].label == "No face")
            // Distance to face space is too high
          cv::putText(colorImage, "No face", cv::Point(face.x,face.y+face.height+25), cv::FONT_HERSHEY_PLAIN, 2, CV_RGB( 255, 0, 0 ), 2);
          else
            // Face classified
            cv::putText(colorImage, face_position_msg_out.detections[i].label.c_str(), cv::Point(face.x,face.y+face.height+25), cv::FONT_HERSHEY_PLAIN, 2, CV_RGB( 0, 255, 0 ), 2);
        }
        // publish image
        cv_bridge::CvImage cv_ptr;
        cv_ptr.image = colorImage;
        cv_ptr.encoding = "bgr8";
        people_detection_image_pub_.publish(cv_ptr.toImageMsg());
      }
    }
    
    bool detectPeopleCallback(cob_people_detection::DetectPeople::Request &req, cob_people_detection::DetectPeople::Response &res)
    {
      // secure this section with a mutex
      boost::timed_mutex::scoped_timed_lock lock(face_position_accumulator_mutex_, boost::posix_time::milliseconds(2000));
      if (lock.owns_lock() == false)
      {
        ROS_ERROR("cob_people_detection::CobPeopleDetectionNodelet: Mutex was not freed during response time.");
        return false;
      }

      prepareFacePositionMessage(res.people_list);
      //res.people_list.detections = face_position_accumulator_;
      //res.people_list.header.stamp = ros::Time::now();
      return true;
    }
  };
  
};

PLUGINLIB_DECLARE_CLASS(ipa_PeopleDetector, CobPeopleDetectionNodelet, ipa_PeopleDetector::CobPeopleDetectionNodelet, nodelet::Nodelet);

#endif // _PEOPLE_DETECTION_

---

cob_people_detection
Author(s): Richard Bormann
autogenerated on Tue Mar 5 11:50:09 2013