00001 00060 #ifndef _FACE_CAPTURE_NODE_ 00061 #define _FACE_CAPTURE_NODE_ 00062 00063 // standard includes 00064 #include <sstream> 00065 #include <string> 00066 #include <vector> 00067 00068 // ROS includes 00069 #include <ros/ros.h> 00070 #include <ros/package.h> 00071 00072 // ROS message includes 00073 #include <sensor_msgs/Image.h> 00074 //#include <cob_perception_msgs/DetectionArray.h> 00075 #include <cob_perception_msgs/ColorDepthImageArray.h> 00076 00077 // topics 00078 #include <message_filters/subscriber.h> 00079 #include <message_filters/synchronizer.h> 00080 #include <message_filters/sync_policies/approximate_time.h> 00081 #include <image_transport/image_transport.h> 00082 #include <image_transport/subscriber_filter.h> 00083 00084 // actions 00085 #include <actionlib/server/simple_action_server.h> 00086 #include <cob_people_detection/addDataAction.h> 00087 #include <cob_people_detection/updateDataAction.h> 00088 #include <cob_people_detection/deleteDataAction.h> 00089 00090 // services 00091 #include <cob_people_detection/captureImage.h> 00092 #include <cob_people_detection/finishRecording.h> 00093 00094 // opencv 00095 #include <opencv2/opencv.hpp> 00096 #include <cv_bridge/cv_bridge.h> 00097 #include <sensor_msgs/image_encodings.h> 00098 00099 // boost 00100 #include <boost/bind.hpp> 00101 #include <boost/thread/mutex.hpp> 00102 00103 // external includes 00104 #include "cob_vision_utils/GlobalDefines.h" 00105 00106 #include "cob_people_detection/face_recognizer.h" 00107 00108 namespace ipa_PeopleDetector 00109 { 00110 00111 typedef actionlib::SimpleActionServer<cob_people_detection::addDataAction> AddDataServer; 00112 typedef actionlib::SimpleActionServer<cob_people_detection::updateDataAction> UpdateDataServer; 00113 typedef actionlib::SimpleActionServer<cob_people_detection::deleteDataAction> DeleteDataServer; 00114 00115 class FaceCaptureNode 00116 { 00117 protected: 00118 00119 ros::NodeHandle node_handle_; 00120 00121 // mutex 00122 boost::mutex active_action_mutex_; 00123 00124 // face recognizer trainer 00125 FaceRecognizer face_recognizer_trainer_; 00126 std::vector<cv::Mat> face_images_; 00127 std::vector<cv::Mat> face_depthmaps_; 00128 std::string current_label_; 00129 bool capture_image_; 00130 int number_captured_images_; 00131 bool finish_image_capture_; 00132 enum CaptureMode 00133 { 00134 MANUAL = 0, CONTINUOUS 00135 }; 00136 enum UpdateMode 00137 { 00138 BY_INDEX = 1, BY_LABEL 00139 }; 00140 //enum DeleteMode {BY_INDEX=1, BY_LABEL}; 00141 00142 image_transport::ImageTransport* it_; 00143 // image_transport::SubscriberFilter people_segmentation_image_sub_; ///< Color camera image topic 00144 // message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, cob_perception_msgs::ColorDepthImageArray, sensor_msgs::Image> >* sync_input_3_; 00145 // message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_recognition_subscriber_; ///< receives the face messages from the detection tracker 00146 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::ColorDepthImageArray, sensor_msgs::Image> >* sync_input_2_; 00147 message_filters::Subscriber<cob_perception_msgs::ColorDepthImageArray> face_detection_subscriber_; 00148 image_transport::SubscriberFilter color_image_sub_; 00149 00150 // actions 00151 AddDataServer* add_data_server_; 00152 UpdateDataServer* update_data_server_; 00153 DeleteDataServer* delete_data_server_; 00154 00155 // services 00156 ros::ServiceServer service_server_capture_image_; 00157 ros::ServiceServer service_server_finish_recording_; 00158 00159 // parameters 00160 std::string data_directory_; 00161 // bool display_; ///< if on, several debug outputs are activated 00162 // bool use_people_segmentation_; ///< enables the combination of face detections with the openni people segmentation 00163 // double face_redetection_time_; ///< timespan during which a face is preserved in the list of tracked faces although it is currently not visible 00164 // double min_segmented_people_ratio_color_; ///< the minimum area inside the face rectangle found in the color image that has to be covered with positive people segmentation results (from openni_tracker) 00165 // double min_segmented_people_ratio_range_; ///< the minimum area inside the face rectangle found in the range image that has to be covered with positive people segmentation results (from openni_tracker) 00166 // double tracking_range_m_; ///< maximum tracking manhattan distance for a face (in meters), i.e. a face can move this distance between two images and can still be tracked 00167 // double face_identification_score_decay_rate_; ///< face identification score decay rate (0 < x < 1), i.e. the score for each label at a certain detection location is multiplied by this factor 00168 // double min_face_identification_score_to_publish_; ///< minimum face identification score to publish (0 <= x < max_score), i.e. this score must be exceeded by a label at a detection location before the person detection is published (higher values increase robustness against short misdetections, but consider the maximum possible score max_score w.r.t. the face_identification_score_decay_rate: new_score = (old_score+1)*face_identification_score_decay_rate --> max_score = face_identification_score_decay_rate/(1-face_identification_score_decay_rate)) 00169 // bool fall_back_to_unknown_identification_; ///< if this is true, the unknown label will be assigned for the identification of a person if it has the highest score, otherwise, the last detection of a name will display as label even if there has been a detection of Unknown recently for that face 00170 00171 00172 void addDataServerCallback(const cob_people_detection::addDataGoalConstPtr& goal); 00173 00175 void inputCallback(const cob_perception_msgs::ColorDepthImageArray::ConstPtr& face_detection_msg);//, const sensor_msgs::Image::ConstPtr& color_image_msg); 00176 00178 unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image); 00179 unsigned long convertDepthImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image); 00180 00181 bool captureImageCallback(cob_people_detection::captureImage::Request &req, cob_people_detection::captureImage::Response &res); 00182 00183 bool finishRecordingCallback(cob_people_detection::finishRecording::Request &req, cob_people_detection::finishRecording::Response &res); 00184 00185 void updateDataServerCallback(const cob_people_detection::updateDataGoalConstPtr& goal); 00186 00187 void deleteDataServerCallback(const cob_people_detection::deleteDataGoalConstPtr& goal); 00188 00189 public: 00190 00191 FaceCaptureNode(ros::NodeHandle nh); 00192 ~FaceCaptureNode(); 00193 }; 00194 00195 } 00196 ; 00197 00198 #endif // _FACE_CAPTURE_NODE_