#include <detection_tracker_node.h>
Public Member Functions | |
double | computeFacePositionDistance (const cob_perception_msgs::Detection &previous_detection, const cob_perception_msgs::Detection ¤t_detection) |
double | computeFacePositionDistanceTrackingRange (const cob_perception_msgs::Detection &previous_detection, const cob_perception_msgs::Detection ¤t_detection) |
unsigned long | convertColorImageMessageToMat (const sensor_msgs::Image::ConstPtr &image_msg, cv_bridge::CvImageConstPtr &image_ptr, cv::Mat &image) |
Converts a color image message to cv::Mat format. | |
unsigned long | copyDetection (const cob_perception_msgs::Detection &src, cob_perception_msgs::Detection &dest, bool update=false, unsigned int updateIndex=UINT_MAX) |
DetectionTrackerNode (ros::NodeHandle nh) | |
< true if data from a rosbag is used, to ignore deprecated timestamps | |
void | inputCallback (const cob_perception_msgs::DetectionArray::ConstPtr &face_position_msg_in, const sensor_msgs::Image::ConstPtr &people_segmentation_image_msg) |
checks the detected faces from the input topic against the people segmentation and outputs faces if both are positive | |
unsigned long | prepareFacePositionMessage (cob_perception_msgs::DetectionArray &face_position_msg_out, ros::Time image_recording_time) |
unsigned long | removeMultipleInstancesOfLabel () |
~DetectionTrackerNode () | |
Protected Attributes | |
bool | debug_ |
enables some debug outputs | |
bool | display_timing_ |
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 | |
std::vector< std::map < std::string, double > > | face_identification_votes_ |
collects votes for all names (map index) ever assigned to each detection (vector index) in face_position_accumulator_ | |
std::vector < cob_perception_msgs::Detection > | face_position_accumulator_ |
accumulates face positions over time | |
boost::timed_mutex | face_position_accumulator_mutex_ |
secures write and read operations to face_position_accumulator_ | |
ros::Publisher | face_position_publisher_ |
publisher for the positions of the detected faces | |
message_filters::Subscriber < cob_perception_msgs::DetectionArray > | face_position_subscriber_ |
receives the face messages from the face detector | |
double | face_redetection_time_ |
timespan during which a face is preserved in the list of tracked faces although it is currently not visible | |
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 | |
image_transport::ImageTransport * | it_ |
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)) | |
double | min_segmented_people_ratio_face_ |
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) | |
double | min_segmented_people_ratio_head_ |
the minimum area inside the head rectangle found in the depth image that has to be covered with positive people segmentation results (from openni_tracker) | |
ros::NodeHandle | node_handle_ |
ROS node handle. | |
image_transport::SubscriberFilter | people_segmentation_image_sub_ |
Color camera image topic. | |
ros::Duration | publish_currently_not_visible_detections_timespan_ |
timespan during which a currently not visible face, which is though preserved in the list of tracked faces, is published as detection (in [0, face_redetection_time]) | |
bool | rosbag_mode_ |
message_filters::Synchronizer < message_filters::sync_policies::ApproximateTime < cob_perception_msgs::DetectionArray, sensor_msgs::Image > > * | sync_input_2_ |
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 | |
bool | use_people_segmentation_ |
enables the combination of face detections with the openni people segmentation |
Definition at line 108 of file detection_tracker_node.h.
< true if data from a rosbag is used, to ignore deprecated timestamps
Definition at line 109 of file detection_tracker_node.cpp.
Definition at line 168 of file detection_tracker_node.cpp.
double DetectionTrackerNode::computeFacePositionDistance | ( | const cob_perception_msgs::Detection & | previous_detection, |
const cob_perception_msgs::Detection & | current_detection | ||
) |
Computes the Euclidean distance of a recent faces detection to a current face detection.
Definition at line 300 of file detection_tracker_node.cpp.
double DetectionTrackerNode::computeFacePositionDistanceTrackingRange | ( | const cob_perception_msgs::Detection & | previous_detection, |
const cob_perception_msgs::Detection & | current_detection | ||
) |
Computes the Euclidean distance of a recent faces detection to a current face detection. If the current face detection is outside the neighborhood of the previous detection, DBL_MAX is returned.
Computes the Euclidean distance of a recent faces detection to a current face detection. If the current face detection is outside the neighborhood of the previous detection, DBL_MAX is returned.
Definition at line 278 of file detection_tracker_node.cpp.
unsigned long DetectionTrackerNode::convertColorImageMessageToMat | ( | const sensor_msgs::Image::ConstPtr & | image_msg, |
cv_bridge::CvImageConstPtr & | image_ptr, | ||
cv::Mat & | image | ||
) |
Converts a color image message to cv::Mat format.
Definition at line 177 of file detection_tracker_node.cpp.
unsigned long DetectionTrackerNode::copyDetection | ( | const cob_perception_msgs::Detection & | src, |
cob_perception_msgs::Detection & | dest, | ||
bool | update = false , |
||
unsigned int | updateIndex = UINT_MAX |
||
) |
Copies the data from src to dest.
src | The new data which shall be copied into dest |
dst | The new data src is copied into dest |
update | If update is true, dest must contain the data which shall be updated |
updateIndex | The index in face_identification_votes_ corresponding to the previous detection dest. Only necessary if update is true. |
Definition at line 198 of file detection_tracker_node.cpp.
void DetectionTrackerNode::inputCallback | ( | const cob_perception_msgs::DetectionArray::ConstPtr & | face_position_msg_in, |
const sensor_msgs::Image::ConstPtr & | people_segmentation_image_msg | ||
) |
checks the detected faces from the input topic against the people segmentation and outputs faces if both are positive
Definition at line 386 of file detection_tracker_node.cpp.
unsigned long DetectionTrackerNode::prepareFacePositionMessage | ( | cob_perception_msgs::DetectionArray & | face_position_msg_out, |
ros::Time | image_recording_time | ||
) |
Definition at line 353 of file detection_tracker_node.cpp.
unsigned long DetectionTrackerNode::removeMultipleInstancesOfLabel | ( | ) |
Removes multiple instances of a label by renaming the detections with lower score to Unknown.
Definition at line 315 of file detection_tracker_node.cpp.
bool ipa_PeopleDetector::DetectionTrackerNode::debug_ [protected] |
enables some debug outputs
Definition at line 125 of file detection_tracker_node.h.
bool ipa_PeopleDetector::DetectionTrackerNode::display_timing_ [protected] |
Definition at line 135 of file detection_tracker_node.h.
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
Definition at line 132 of file detection_tracker_node.h.
std::vector<std::map<std::string, double> > ipa_PeopleDetector::DetectionTrackerNode::face_identification_votes_ [protected] |
collects votes for all names (map index) ever assigned to each detection (vector index) in face_position_accumulator_
Definition at line 122 of file detection_tracker_node.h.
std::vector<cob_perception_msgs::Detection> ipa_PeopleDetector::DetectionTrackerNode::face_position_accumulator_ [protected] |
accumulates face positions over time
Definition at line 120 of file detection_tracker_node.h.
boost::timed_mutex ipa_PeopleDetector::DetectionTrackerNode::face_position_accumulator_mutex_ [protected] |
secures write and read operations to face_position_accumulator_
Definition at line 121 of file detection_tracker_node.h.
publisher for the positions of the detected faces
Definition at line 116 of file detection_tracker_node.h.
message_filters::Subscriber<cob_perception_msgs::DetectionArray> ipa_PeopleDetector::DetectionTrackerNode::face_position_subscriber_ [protected] |
receives the face messages from the face detector
Definition at line 115 of file detection_tracker_node.h.
double ipa_PeopleDetector::DetectionTrackerNode::face_redetection_time_ [protected] |
timespan during which a face is preserved in the list of tracked faces although it is currently not visible
Definition at line 127 of file detection_tracker_node.h.
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
Definition at line 134 of file detection_tracker_node.h.
Definition at line 111 of file detection_tracker_node.h.
double ipa_PeopleDetector::DetectionTrackerNode::min_face_identification_score_to_publish_ [protected] |
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))
Definition at line 133 of file detection_tracker_node.h.
double ipa_PeopleDetector::DetectionTrackerNode::min_segmented_people_ratio_face_ [protected] |
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)
Definition at line 129 of file detection_tracker_node.h.
double ipa_PeopleDetector::DetectionTrackerNode::min_segmented_people_ratio_head_ [protected] |
the minimum area inside the head rectangle found in the depth image that has to be covered with positive people segmentation results (from openni_tracker)
Definition at line 130 of file detection_tracker_node.h.
ROS node handle.
Definition at line 118 of file detection_tracker_node.h.
image_transport::SubscriberFilter ipa_PeopleDetector::DetectionTrackerNode::people_segmentation_image_sub_ [protected] |
Color camera image topic.
Definition at line 112 of file detection_tracker_node.h.
ros::Duration ipa_PeopleDetector::DetectionTrackerNode::publish_currently_not_visible_detections_timespan_ [protected] |
timespan during which a currently not visible face, which is though preserved in the list of tracked faces, is published as detection (in [0, face_redetection_time])
Definition at line 128 of file detection_tracker_node.h.
bool ipa_PeopleDetector::DetectionTrackerNode::rosbag_mode_ [protected] |
Definition at line 137 of file detection_tracker_node.h.
message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >* ipa_PeopleDetector::DetectionTrackerNode::sync_input_2_ [protected] |
Definition at line 114 of file detection_tracker_node.h.
double ipa_PeopleDetector::DetectionTrackerNode::tracking_range_m_ [protected] |
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
Definition at line 131 of file detection_tracker_node.h.
bool ipa_PeopleDetector::DetectionTrackerNode::use_people_segmentation_ [protected] |
enables the combination of face detections with the openni people segmentation
Definition at line 126 of file detection_tracker_node.h.