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_perception_msgs/DetectionArray.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 <opencv2/opencv.hpp>
#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/people_detector.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_; 
#if CV_MAJOR_VERSION == 2
        cv::SVM person_classifier_; 
#else
// OpenCV 3
        cv::ml::SVM person_classifier_; 
#endif

        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_