Go to the documentation of this file.00001
00060 #ifndef _DETECTION_TRACKER_
00061 #define _DETECTION_TRACKER_
00062
00063
00064
00065
00066
00067
00068
00069
00070 #include <ros/ros.h>
00071 #include <ros/package.h>
00072
00073
00074 #include <sensor_msgs/Image.h>
00075
00076 #include <cob_perception_msgs/DetectionArray.h>
00077
00078
00079
00080
00081
00082 #include <message_filters/subscriber.h>
00083 #include <message_filters/synchronizer.h>
00084 #include <message_filters/sync_policies/approximate_time.h>
00085 #include <image_transport/image_transport.h>
00086 #include <image_transport/subscriber_filter.h>
00087
00088
00089 #include <opencv2/opencv.hpp>
00090 #include <cv_bridge/cv_bridge.h>
00091 #include <sensor_msgs/image_encodings.h>
00092
00093
00094 #include <boost/bind.hpp>
00095 #include <boost/thread/mutex.hpp>
00096
00097
00098 #include "cob_vision_utils/GlobalDefines.h"
00099
00100 #include <sstream>
00101 #include <string>
00102 #include <vector>
00103
00104 namespace ipa_PeopleDetector
00105 {
00106
00107 class DetectionTrackerNode
00108 {
00109 protected:
00110 image_transport::ImageTransport* it_;
00111 image_transport::SubscriberFilter people_segmentation_image_sub_;
00112
00113 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >* sync_input_2_;
00114 message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_position_subscriber_;
00115 ros::Publisher face_position_publisher_;
00116
00117 ros::NodeHandle node_handle_;
00118
00119 std::vector<cob_perception_msgs::Detection> face_position_accumulator_;
00120 boost::timed_mutex face_position_accumulator_mutex_;
00121 std::vector<std::map<std::string, double> > face_identification_votes_;
00122
00123
00124 bool debug_;
00125 bool use_people_segmentation_;
00126 double face_redetection_time_;
00127 ros::Duration publish_currently_not_visible_detections_timespan_;
00128 double min_segmented_people_ratio_face_;
00129 double min_segmented_people_ratio_head_;
00130 double tracking_range_m_;
00131 double face_identification_score_decay_rate_;
00132 double min_face_identification_score_to_publish_;
00133 bool fall_back_to_unknown_identification_;
00134 bool display_timing_;
00135
00136 bool rosbag_mode_;
00137
00138 public:
00139
00140 DetectionTrackerNode(ros::NodeHandle nh);
00141
00142 ~DetectionTrackerNode();
00143
00145 unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image);
00146
00153 unsigned long copyDetection(const cob_perception_msgs::Detection& src, cob_perception_msgs::Detection& dest, bool update = false,
00154 unsigned int updateIndex = UINT_MAX);
00155
00159 double computeFacePositionDistanceTrackingRange(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection);
00160
00163 double computeFacePositionDistance(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection);
00164
00167 unsigned long removeMultipleInstancesOfLabel();
00168
00169 unsigned long prepareFacePositionMessage(cob_perception_msgs::DetectionArray& face_position_msg_out, ros::Time image_recording_time, std::string frame_id);
00170
00172 void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_position_msg_in, const sensor_msgs::Image::ConstPtr& people_segmentation_image_msg);
00173
00174 };
00175
00176 }
00177 ;
00178
00179 #endif // _DETECTION_TRACKER_