$search

face_detection.h

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:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * 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 _FACE_DETECTION_
#define _FACE_DETECTION_


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

// standard includes
//--

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

#include <actionlib/server/simple_action_server.h>

// ROS message includes
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
//#include <std_msgs/Float32MultiArray.h>
#include <cob_people_detection_msgs/PeopleDetectionArray.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>

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

// actions
#include <actionlib/server/simple_action_server.h>
#include <cob_people_detection/TrainContinuousAction.h>
#include <cob_people_detection/TrainCaptureSampleAction.h>
#include <cob_people_detection/RecognizeAction.h>
#include <cob_people_detection/LoadAction.h>
#include <cob_people_detection/SaveAction.h>
#include <cob_people_detection/ShowAction.h>

// opencv
#include <opencv/cv.h>
#include <opencv/ml.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>

// tiny xml
#include "tinyxml.h"

// boost
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/convenience.hpp"
#include "boost/filesystem/path.hpp"
namespace fs = boost::filesystem;

// external includes
#ifdef __LINUX__
#else
        #include "cob_vision_ipa_utils/MathUtils.h"
        #include "cob_sensor_fusion/ColoredPointCloudSequence.h"
#endif

#include "cob_people_detection/PeopleDetector.h"

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

namespace ipa_PeopleDetector {

typedef actionlib::SimpleActionServer<cob_people_detection::TrainContinuousAction> TrainContinuousServer;
typedef actionlib::SimpleActionServer<cob_people_detection::TrainCaptureSampleAction> TrainCaptureSampleServer;
typedef actionlib::SimpleActionServer<cob_people_detection::RecognizeAction> RecognizeServer;
typedef actionlib::SimpleActionServer<cob_people_detection::LoadAction> LoadServer;
typedef actionlib::SimpleActionServer<cob_people_detection::SaveAction> SaveServer;
typedef actionlib::SimpleActionServer<cob_people_detection::ShowAction> ShowServer;


//####################
//#### node class ####
class CobFaceDetectionNodelet : public nodelet::Nodelet
{
protected:
        message_filters::Subscriber<sensor_msgs::PointCloud2> shared_image_sub_;        
        image_transport::ImageTransport* it_;
        image_transport::SubscriberFilter color_camera_image_sub_;      
        image_transport::Publisher face_detection_image_pub_;   
        message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> >* sync_pointcloud_;
        message_filters::Connection sync_pointcloud_callback_connection_;
        ros::Publisher face_position_publisher_;                

        ros::NodeHandle node_handle_;                           

#ifdef __LINUX__
        cv::Mat color_image_;   
        cv::Mat range_image_;   
#else
        ipa_SensorFusion::ColoredPointCloudPtr colored_pc_; 
#endif

        std::string directory_;                                         
        bool run_pca_;                                                          
        int filename_;                                                          

        std::vector<cv::Mat> face_images_;                      
        std::vector<std::string> id_;                           
        std::vector<std::string> id_unique_;            

        int n_eigens_;                                                          
        std::vector<cv::Mat> eigen_vectors_;            
        cv::Mat eigen_val_mat_;                                         
        cv::Mat avg_image_;                                                     
        cv::Mat projected_train_face_mat_;                      
        cv::Mat face_class_avg_projections_;            
        cv::SVM person_classifier_;                                     

        PeopleDetector* people_detector_;                       
        int threshold_;                                                         
        int threshold_fs_;                                                      
        std::vector<cv::Rect> color_faces_;                     
        std::vector<cv::Rect> range_faces_;                     
        std::set<size_t> range_face_indices_with_color_face_detection_; 

        // Services
        ros::ServiceServer recognize_service_server_; 

        // Actions
        TrainContinuousServer* train_continuous_server_;
        TrainCaptureSampleServer* train_capture_sample_server_;
        RecognizeServer* recognize_server_;
        LoadServer* load_server_;
        SaveServer* save_server_;
        ShowServer* show_server_;
        bool occupied_by_action_;                                       
        bool recognize_server_running_;                         
        bool train_continuous_server_running_;          
        bool capture_training_face_;                            
        int number_training_images_captured_;           
//      bool turn_off_recognition_;                                     ///< is set true on quit request during recognition mode
        bool do_recognition_;                                           
        bool display_;                                                          

        std::string current_training_id_;                       
        boost::timed_mutex action_mutex_;                       

        // constants
        static const double FACE_SIZE_MIN_M = 0.12;
        static const double FACE_SIZE_MAX_M = 0.35;
        static const double MAX_FACE_Z_M = 8.0;

        // parameters
        double face_size_min_m_;                
        double face_size_max_m_;                
        double max_face_z_m_;                   
        bool fill_unassigned_depth_values_;     
        bool reason_about_3dface_size_;         

public:

        CobFaceDetectionNodelet();

        ~CobFaceDetectionNodelet();

        void onInit();

        unsigned long init();

        unsigned long detectFaces(cv::Mat& xyz_image, cv::Mat& color_image);

        unsigned long recognizeFace(cv::Mat& color_image, std::vector<int>& index);

        unsigned long addFace(cv::Mat& image, std::string id);

        unsigned long PCA();

        unsigned long saveAllData();

        unsigned long saveTrainingData();

        unsigned long saveRecognizerData();

        unsigned long loadAllData();

        unsigned long loadTrainingData(bool runPCA);

        unsigned long loadRecognizerData();

        unsigned long getEigenface(cv::Mat& eigenface, int index);

        unsigned long showAVGImage(cv::Mat& avgImage);

        unsigned long saveRangeTrainImages(cv::Mat& xyz_image);

        unsigned long getMeasurement(const sensor_msgs::PointCloud2::ConstPtr& shared_image_msg, const sensor_msgs::Image::ConstPtr& color_image_msg);

        unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& color_image_msg, cv_bridge::CvImageConstPtr& color_image_ptr, cv::Mat& color_image);

        unsigned long convertPclMessageToMat(const sensor_msgs::PointCloud2::ConstPtr& shared_image_msg, cv::Mat& depth_image);

        std::string getLabel(int index);

        void recognizeCallback(const sensor_msgs::PointCloud2::ConstPtr& shared_image_msg, const sensor_msgs::Image::ConstPtr& color_image_msg);

        
        bool recognizeServiceServerCallback(cob_people_detection::Recognition::Request &req, cob_people_detection::Recognition::Response &res);
        
        void recognizeServerCallback(const cob_people_detection::RecognizeGoalConstPtr& goal);


        void trainContinuousCallback(const sensor_msgs::PointCloud2::ConstPtr& shared_image_msg, const sensor_msgs::Image::ConstPtr& color_image_msg);

        void trainContinuousServerCallback(const cob_people_detection::TrainContinuousGoalConstPtr& goal);

        void trainCaptureSampleServerCallback(const cob_people_detection::TrainCaptureSampleGoalConstPtr& goal);

        void loadServerCallback(const cob_people_detection::LoadGoalConstPtr& goal);

        void saveServerCallback(const cob_people_detection::SaveGoalConstPtr& goal);

        void showServerCallback(const cob_people_detection::ShowGoalConstPtr& goal);

        unsigned long loadParameters(const char* iniFileName);
};

};

#endif // _FACE_DETECTION_

---

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