00001 #include "people_tracking_mht_alg.h" 00002 00003 PeopleTrackingMhtAlgorithm::PeopleTrackingMhtAlgorithm(void) 00004 { 00005 myPeopleTrackingMht = new Cmht(); 00006 } 00007 00008 PeopleTrackingMhtAlgorithm::~PeopleTrackingMhtAlgorithm(void) 00009 { 00010 delete myPeopleTrackingMht; 00011 } 00012 00013 void PeopleTrackingMhtAlgorithm::config_update(Config& new_cfg, uint32_t level) 00014 { 00015 this->lock(); 00016 00017 // save the current configuration 00018 this->config_=new_cfg; 00019 myPeopleTrackingMht->set_threshold_distance( new_cfg.association_threshold); 00020 myPeopleTrackingMht->set_threshold_confirmation_track( new_cfg.confirmation_threshold); 00021 myPeopleTrackingMht->set_threshold_no_deteccion( new_cfg.deletion_threshold); 00022 myPeopleTrackingMht->set_laser_Pd( new_cfg.laser_probability); 00023 myPeopleTrackingMht->set_laser_beta_ft( new_cfg.laser_false_alarm); 00024 myPeopleTrackingMht->set_laser_beta_nt( new_cfg.laser_new_track); 00025 myPeopleTrackingMht->set_laser_beta_no_detection( new_cfg.laser_no_detection); 00026 myPeopleTrackingMht->set_laser_beta_track_no_confirmed( new_cfg.laser_no_confirmed); 00027 myPeopleTrackingMht->set_increment_iteration_track_no_confirmed(new_cfg.laser_no_confirmed_iteration); 00028 this->unlock(); 00029 } 00030 00031 // PeopleTrackingMhtAlgorithm Public API 00032 00033 bool PeopleTrackingMhtAlgorithm::iteration(const std::vector<Sdetection> &detections, std::vector<Strack> &tracks) 00034 { 00035 //ROS_INFO("PeopleTrackingMhtAlgorithm::iteration"); 00036 std::vector<Sdetection> myDetections= detections; 00037 //ROS_INFO("PeopleTrackingMhtAlgorithm::iteration %d", myDetections.size()); 00038 myPeopleTrackingMht->mht_update(myDetections); 00039 //ROS_INFO("PeopleTrackingMhtAlgorithm::iteration: mht_update done! Detections sent!"); 00040 tracks = myPeopleTrackingMht->get_persons_mht(); 00041 //ROS_INFO("PeopleTrackingMhtAlgorithm::iteration: get_persons_mht done! %d Tracks received!", tracks.size()); 00042 return true; 00043 }