36 #include <leg_detector/LegDetectorConfig.h> 40 #include <opencv/cxcore.h> 41 #include <opencv/cv.h> 42 #include <opencv/ml.h> 44 #include <people_msgs/PositionMeasurement.h> 45 #include <people_msgs/PositionMeasurementArray.h> 46 #include <sensor_msgs/LaserScan.h> 55 #include <visualization_msgs/Marker.h> 56 #include <dynamic_reconfigure/server.h> 104 filter_(
"tracker_name", sys_sigma_),
105 reliability(-1.), p(4)
108 snprintf(
id, 100,
"legtrack%d", nextid++);
109 id_ = std::string(
id);
153 SymmetricMatrix cov(3);
165 reliability = probability;
171 double k = p / (p +
kal_r);
172 reliability += k * (probability - reliability);
193 position_[0] = est.
pos_[0];
194 position_[1] = est.
pos_[1];
195 position_[2] = est.
pos_[2];
198 double nreliability = fmin(1.0, fmax(0.1, est.
vel_.
length() / 0.5));
217 : candidate_(candidate)
219 , distance_(distance)
220 , probability_(probability)
271 dynamic_reconfigure::Server<leg_detector::LegDetectorConfig>
server_;
283 people_sub_(nh_,
"people_tracker_filter", 10),
284 laser_sub_(nh_,
"scan", 10),
285 people_notifier_(people_sub_, tfl_,
fixed_frame, 10),
290 forest = cv::ml::RTrees::create();
291 cv::String feature_file = cv::String(
g_argv[1]);
292 forest = cv::ml::StatModel::load<cv::ml::RTrees>(feature_file);
293 feat_count_ = forest->getVarCount();
294 printf(
"Loaded forest with %d features: %s\n", feat_count_,
g_argv[1]);
298 printf(
"Please provide a trained random forests classifier as an input.\n");
302 nh_.
param<
bool>(
"use_seeds", use_seeds_, !
true);
305 leg_measurements_pub_ = nh_.
advertise<people_msgs::PositionMeasurementArray>(
"leg_tracker_measurements", 0);
306 people_measurements_pub_ = nh_.
advertise<people_msgs::PositionMeasurementArray>(
"people_tracker_measurements", 0);
307 markers_pub_ = nh_.
advertise<visualization_msgs::Marker>(
"visualization_marker", 20);
317 dynamic_reconfigure::Server<leg_detector::LegDetectorConfig>::CallbackType
f;
319 server_.setCallback(f);
329 void configure(leg_detector::LegDetectorConfig &config, uint32_t level)
331 connected_thresh_ = config.connection_threshold;
332 min_points_per_group = config.min_points_per_group;
333 leg_reliability_limit_ = config.leg_reliability_limit;
334 publish_legs_ = config.publish_legs;
335 publish_people_ = config.publish_people;
336 publish_leg_markers_ = config.publish_leg_markers;
337 publish_people_markers_ = config.publish_people_markers;
351 kal_p = config.kalman_p;
352 kal_q = config.kalman_q;
353 kal_r = config.kalman_r;
357 double distance(list<SavedFeature*>::iterator it1, list<SavedFeature*>::iterator it2)
360 double dx = one[0] - two[0], dy = one[1] - two[1], dz = one[2] - two[2];
361 return sqrt(dx * dx + dy * dy + dz * dz);
366 void peopleCallback(
const people_msgs::PositionMeasurement::ConstPtr& people_meas)
369 if (saved_features_.empty())
373 pointMsgToTF(people_meas->pos, pt);
374 Stamped<Point> person_loc(pt, people_meas->header.stamp, people_meas->header.frame_id);
376 Stamped<Point> dest_loc(pt, people_meas->header.stamp, people_meas->header.frame_id);
378 boost::mutex::scoped_lock lock(saved_mutex_);
380 list<SavedFeature*>::iterator closest = saved_features_.end();
381 list<SavedFeature*>::iterator closest1 = saved_features_.end();
382 list<SavedFeature*>::iterator closest2 = saved_features_.end();
386 list<SavedFeature*>::iterator begin = saved_features_.begin();
387 list<SavedFeature*>::iterator end = saved_features_.end();
388 list<SavedFeature*>::iterator it1, it2;
400 for (it1 = begin; it1 != end; ++it1)
412 (*it1)->dist_to_person_ = dest_loc.length();
416 cout <<
"Looking for two legs" << endl;
418 for (it1 = begin; it1 != end; ++it1)
421 if ((*it1)->object_id == people_meas->object_id)
436 (*it1)->object_id =
"";
441 if (it1 != end && it2 != end)
443 cout <<
"Found matching pair. The second distance was " << (*it1)->dist_to_person_ << endl;
453 cout <<
"Looking for one leg plus one new leg" << endl;
454 float dist_between_legs, closest_dist_between_legs;
458 closest = saved_features_.end();
460 for (it1 = begin; it1 != end; ++it1)
467 if ((it1 == it2) || ((*it1)->object_id !=
"") || ((*it1)->getLifetime() >
max_second_leg_age_s) || ((*it1)->dist_to_person_ >= closest_dist))
477 ROS_WARN(
"TF exception getting distance between legs.");
479 dist_between_legs = dest_loc.length();
485 closest_dist = (*it1)->dist_to_person_;
486 closest_dist_between_legs = dist_between_legs;
492 cout <<
"Replaced one leg with a distance of " << closest_dist <<
" and a distance between the legs of " << closest_dist_between_legs << endl;
493 (*closest)->object_id = people_meas->object_id;
497 cout <<
"Returned one matched leg only" << endl;
504 cout <<
"Looking for a pair of new legs" << endl;
506 it1 = saved_features_.begin();
507 it2 = saved_features_.begin();
508 closest = saved_features_.end();
509 closest1 = saved_features_.end();
510 closest2 = saved_features_.end();
513 for (; it1 != end; ++it1)
516 if ((*it1)->object_id !=
"" || (*it1)->dist_to_person_ >=
max_meas_jump_m)
520 if ((*it1)->dist_to_person_ < closest_dist)
522 closest_dist = (*it1)->dist_to_person_;
529 for (; it2 != end; ++it2)
532 if ((*it2)->object_id !=
"" || (*it2)->dist_to_person_ >=
max_meas_jump_m)
542 ROS_WARN(
"TF exception getting distance between legs in spot 2.");
544 dist_between_legs = dest_loc.length();
547 if ((*it1)->dist_to_person_ + (*it2)->dist_to_person_ < closest_pair_dist && dist_between_legs <
leg_pair_separation_m)
549 closest_pair_dist = (*it1)->dist_to_person_ + (*it2)->dist_to_person_;
552 closest_dist_between_legs = dist_between_legs;
557 if (closest1 != end && closest2 != end)
559 (*closest1)->object_id = people_meas->object_id;
560 (*closest2)->object_id = people_meas->object_id;
561 cout <<
"Found a completely new pair with total distance " << closest_pair_dist <<
" and a distance between the legs of " << closest_dist_between_legs << endl;
565 cout <<
"Looking for just one leg" << endl;
569 (*closest)->object_id = people_meas->object_id;
570 cout <<
"Returned one new leg only" << endl;
574 cout <<
"Nothing matched" << endl;
580 list<SavedFeature*>::iterator begin = saved_features_.begin();
581 list<SavedFeature*>::iterator end = saved_features_.end();
582 list<SavedFeature*>::iterator leg1, leg2, best, it;
584 for (leg1 = begin; leg1 != end; ++leg1)
587 if ((*leg1)->object_id ==
"")
593 for (it = begin; it != end; ++it)
595 if (it == leg1)
continue;
597 if ((*it)->object_id == (*leg1)->object_id)
603 if ((*it)->object_id !=
"")
608 && (d < closest_dist))
618 double dist_between_legs =
distance(leg1, leg2);
621 (*leg1)->object_id =
"";
622 (*leg1)->other = NULL;
623 (*leg2)->object_id =
"";
624 (*leg2)->other = NULL;
628 (*leg1)->other = *leg2;
629 (*leg2)->other = *leg1;
632 else if (best != end)
634 (*best)->object_id = (*leg1)->object_id;
635 (*leg1)->other = *best;
636 (*best)->other = *leg1;
643 list<SavedFeature*>::iterator best1 = end, best2 = end;
646 for (leg1 = begin; leg1 != end; ++leg1)
649 if ((*leg1)->object_id !=
"" 650 || (*leg1)->getReliability() < leg_reliability_limit_)
653 for (leg2 = begin; leg2 != end; ++leg2)
655 if (((*leg2)->object_id !=
"")
656 || ((*leg2)->getReliability() < leg_reliability_limit_)
657 || (leg1 == leg2))
continue;
659 if (d < closest_dist)
670 snprintf(
id, 100,
"Person%d", next_p_id_++);
671 (*best1)->object_id = std::string(
id);
672 (*best2)->object_id = std::string(
id);
673 (*best1)->other = *best2;
674 (*best2)->other = *best1;
690 cv::Mat tmp_mat = cv::Mat(1, feat_count_, CV_32FC1);
694 list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
695 while (sf_iter != saved_features_.end())
697 if ((*sf_iter)->meas_time_ < purge)
699 if ((*sf_iter)->other)
700 (*sf_iter)->other->other = NULL;
702 saved_features_.erase(sf_iter++);
710 list<SavedFeature*> propagated;
711 for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
712 sf_iter != saved_features_.end();
715 (*sf_iter)->propagate(scan->header.stamp);
716 propagated.push_back(*sf_iter);
723 multiset<MatchedFeature> matches;
724 for (list<SampleSet*>::iterator i = processor.
getClusters().begin();
730 for (
int k = 0; k < feat_count_; k++)
731 tmp_mat.data[k] = (
float)(f[k]);
736 float probability = 0.5 -
737 forest->predict(tmp_mat, cv::noArray(), cv::ml::RTrees::PREDICT_SUM) /
738 forest->getRoots().size();
739 Stamped<Point> loc((*i)->center(), scan->header.stamp, scan->header.frame_id);
749 list<SavedFeature*>::iterator closest = propagated.end();
752 for (list<SavedFeature*>::iterator pf_iter = propagated.begin();
753 pf_iter != propagated.end();
757 float dist = loc.distance((*pf_iter)->position_);
758 if (dist < closest_dist)
765 if (closest == propagated.end())
767 list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(),
new SavedFeature(loc, tfl_));
771 matches.insert(
MatchedFeature(*i, *closest, closest_dist, probability));
776 while (matches.size() > 0)
778 multiset<MatchedFeature>::iterator matched_iter = matches.begin();
780 list<SavedFeature*>::iterator pf_iter = propagated.begin();
781 while (pf_iter != propagated.end())
784 if (matched_iter->closest_ == *pf_iter)
787 Stamped<Point> loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id);
798 matched_iter->closest_->update(loc, matched_iter->probability_);
801 matches.erase(matched_iter);
802 propagated.erase(pf_iter++);
817 Stamped<Point> loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id);
827 list<SavedFeature*>::iterator closest = propagated.end();
830 for (list<SavedFeature*>::iterator remain_iter = propagated.begin();
831 remain_iter != propagated.end();
834 float dist = loc.distance((*remain_iter)->position_);
835 if (dist < closest_dist)
837 closest = remain_iter;
844 if (closest == propagated.end())
845 list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(),
new SavedFeature(loc, tfl_));
847 matches.insert(
MatchedFeature(matched_iter->candidate_, *closest, closest_dist, matched_iter->probability_));
848 matches.erase(matched_iter);
857 vector<people_msgs::PositionMeasurement> people;
858 vector<people_msgs::PositionMeasurement> legs;
860 for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
861 sf_iter != saved_features_.end();
865 double reliability = (*sf_iter)->getReliability();
867 if ((*sf_iter)->getReliability() > leg_reliability_limit_
870 people_msgs::PositionMeasurement pos;
871 pos.header.stamp = scan->header.stamp;
873 pos.name =
"leg_detector";
874 pos.object_id = (*sf_iter)->id_;
875 pos.pos.x = (*sf_iter)->position_[0];
876 pos.pos.y = (*sf_iter)->position_[1];
877 pos.pos.z = (*sf_iter)->position_[2];
878 pos.reliability = reliability;
879 pos.covariance[0] = pow(0.3 / reliability, 2.0);
880 pos.covariance[1] = 0.0;
881 pos.covariance[2] = 0.0;
882 pos.covariance[3] = 0.0;
883 pos.covariance[4] = pow(0.3 / reliability, 2.0);
884 pos.covariance[5] = 0.0;
885 pos.covariance[6] = 0.0;
886 pos.covariance[7] = 0.0;
887 pos.covariance[8] = 10000.0;
888 pos.initialization = 0;
893 if (publish_leg_markers_)
895 visualization_msgs::Marker m;
896 m.header.stamp = (*sf_iter)->time_;
901 m.pose.position.x = (*sf_iter)->position_[0];
902 m.pose.position.y = (*sf_iter)->position_[1];
903 m.pose.position.z = (*sf_iter)->position_[2];
910 if ((*sf_iter)->object_id !=
"")
916 m.color.b = (*sf_iter)->getReliability();
922 if (publish_people_ || publish_people_markers_)
925 if (other != NULL && other < (*sf_iter))
927 double dx = ((*sf_iter)->position_[0] + other->
position_[0]) / 2,
928 dy = ((*sf_iter)->position_[1] + other->
position_[1]) / 2,
929 dz = ((*sf_iter)->position_[2] + other->
position_[2]) / 2;
934 people_msgs::PositionMeasurement pos;
935 pos.header.stamp = (*sf_iter)->time_;
937 pos.name = (*sf_iter)->object_id;;
938 pos.object_id = (*sf_iter)->id_ +
"|" + other->
id_;
942 pos.reliability = reliability;
943 pos.covariance[0] = pow(0.3 / reliability, 2.0);
944 pos.covariance[1] = 0.0;
945 pos.covariance[2] = 0.0;
946 pos.covariance[3] = 0.0;
947 pos.covariance[4] = pow(0.3 / reliability, 2.0);
948 pos.covariance[5] = 0.0;
949 pos.covariance[6] = 0.0;
950 pos.covariance[7] = 0.0;
951 pos.covariance[8] = 10000.0;
952 pos.initialization = 0;
953 people.push_back(pos);
956 if (publish_people_markers_)
958 visualization_msgs::Marker m;
959 m.header.stamp = (*sf_iter)->time_;
964 m.pose.position.x = dx;
965 m.pose.position.y = dy;
966 m.pose.position.z = dz;
980 people_msgs::PositionMeasurementArray array;
985 leg_measurements_pub_.
publish(array);
989 array.people = people;
990 people_measurements_pub_.
publish(array);
995 int main(
int argc,
char **argv)
Stamped< Point > position_
virtual void getEstimate(BFL::StatePosVel &est) const
static double max_second_leg_age_s
double leg_reliability_limit_
void publish(const boost::shared_ptr< M > &message) const
void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
ros::Publisher leg_measurements_pub_
cv::Ptr< cv::ml::RTrees > forest
void configure(leg_detector::LegDetectorConfig &config, uint32_t level)
virtual double getLifetime() const
void splitConnected(float thresh)
dynamic_reconfigure::Server< leg_detector::LegDetectorConfig > server_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool publish_people_markers_
message_filters::Subscriber< sensor_msgs::LaserScan > laser_sub_
BFL::StatePosVel sys_sigma_
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
message_filters::Subscriber< people_msgs::PositionMeasurement > people_sub_
std::vector< float > calcLegFeatures(laser_processor::SampleSet *cluster, const sensor_msgs::LaserScan &scan)
list< SavedFeature * > saved_features_
ROSCPP_DECL void spin(Spinner &spinner)
tf::MessageFilter< sensor_msgs::LaserScan > laser_notifier_
ros::Publisher people_measurements_pub_
static double max_track_jump_m
A namespace containing the laser processor helper classes.
boost::mutex saved_mutex_
int main(int argc, char **argv)
Duration & fromSec(double t)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void update(Stamped< Point > loc, double probability)
static double max_meas_jump_m
An ordered set of Samples.
void propagate(ros::Time time)
double distance(list< SavedFeature * >::iterator it1, list< SavedFeature * >::iterator it2)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void initialize(const BFL::StatePosVel &mu, const BFL::StatePosVel &sigma, const double time)
static string fixed_frame
static double leg_pair_separation_m
tf::MessageFilter< people_msgs::PositionMeasurement > people_notifier_
std::list< SampleSet * > & getClusters()
void peopleCallback(const people_msgs::PositionMeasurement::ConstPtr &people_meas)
LegDetector(ros::NodeHandle nh)
void removeLessThan(uint32_t num)
ros::Publisher markers_pub_
ROSCPP_DECL void shutdown()
void setTargetFrame(const std::string &target_frame)
virtual bool updateCorrection(const tf::Vector3 &meas, const MatrixWrapper::SymmetricMatrix &cov)
A mask for filtering out Samples based on range.
SavedFeature(Stamped< Point > loc, TransformListener &tfl)
void setTolerance(const ros::Duration &tolerance)
MatchedFeature(SampleSet *candidate, SavedFeature *closest, float distance, double probability)
static double no_observation_timeout_s
virtual bool updatePrediction(const double time)
TFSIMD_FORCE_INLINE tfScalar length() const
Connection registerCallback(const C &callback)