leg_detector.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <ros/ros.h>
00035 
00036 #include <leg_detector/LegDetectorConfig.h>
00037 #include <leg_detector/laser_processor.h>
00038 #include <leg_detector/calc_leg_features.h>
00039 
00040 #include <opencv/cxcore.h>
00041 #include <opencv/cv.h>
00042 #include <opencv/ml.h>
00043 
00044 #include <people_msgs/PositionMeasurement.h>
00045 #include <people_msgs/PositionMeasurementArray.h>
00046 #include <sensor_msgs/LaserScan.h>
00047 
00048 #include <tf/transform_listener.h>
00049 #include <tf/message_filter.h>
00050 #include <message_filters/subscriber.h>
00051 
00052 #include <people_tracking_filter/tracker_kalman.h>
00053 #include <people_tracking_filter/state_pos_vel.h>
00054 #include <people_tracking_filter/rgb.h>
00055 #include <visualization_msgs/Marker.h>
00056 #include <dynamic_reconfigure/server.h>
00057 
00058 #include <algorithm>
00059 
00060 using namespace std;
00061 using namespace laser_processor;
00062 using namespace ros;
00063 using namespace tf;
00064 using namespace estimation;
00065 using namespace BFL;
00066 using namespace MatrixWrapper;
00067 
00068 
00069 static double no_observation_timeout_s = 0.5;
00070 static double max_second_leg_age_s     = 2.0;
00071 static double max_track_jump_m         = 1.0;
00072 static double max_meas_jump_m          = 0.75; // 1.0
00073 static double leg_pair_separation_m    = 1.0;
00074 static string fixed_frame              = "odom_combined";
00075 
00076 static double kal_p = 4, kal_q = .002, kal_r = 10;
00077 static bool use_filter = true;
00078 
00079 
00080 class SavedFeature
00081 {
00082 public:
00083   static int nextid;
00084   TransformListener& tfl_;
00085 
00086   BFL::StatePosVel sys_sigma_;
00087   TrackerKalman filter_;
00088 
00089   string id_;
00090   string object_id;
00091   ros::Time time_;
00092   ros::Time meas_time_;
00093 
00094   double reliability, p;
00095 
00096   Stamped<Point> position_;
00097   SavedFeature* other;
00098   float dist_to_person_;
00099 
00100   // one leg tracker
00101   SavedFeature(Stamped<Point> loc, TransformListener& tfl)
00102     : tfl_(tfl),
00103       sys_sigma_(Vector3(0.05, 0.05, 0.05), Vector3(1.0, 1.0, 1.0)),
00104       filter_("tracker_name", sys_sigma_),
00105       reliability(-1.), p(4)
00106   {
00107     char id[100];
00108     snprintf(id, 100, "legtrack%d", nextid++);
00109     id_ = std::string(id);
00110 
00111     object_id = "";
00112     time_ = loc.stamp_;
00113     meas_time_ = loc.stamp_;
00114     other = NULL;
00115 
00116     try
00117     {
00118       tfl_.transformPoint(fixed_frame, loc, loc);
00119     }
00120     catch (...)
00121     {
00122       ROS_WARN("TF exception spot 6.");
00123     }
00124     StampedTransform pose(Pose(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_);
00125     tfl_.setTransform(pose);
00126 
00127     StatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1), Vector3(0.0000001, 0.0000001, 0.0000001));
00128     filter_.initialize(loc, prior_sigma, time_.toSec());
00129 
00130     StatePosVel est;
00131     filter_.getEstimate(est);
00132 
00133     updatePosition();
00134   }
00135 
00136   void propagate(ros::Time time)
00137   {
00138     time_ = time;
00139 
00140     filter_.updatePrediction(time.toSec());
00141 
00142     updatePosition();
00143   }
00144 
00145   void update(Stamped<Point> loc, double probability)
00146   {
00147     StampedTransform pose(Pose(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_);
00148     tfl_.setTransform(pose);
00149 
00150     meas_time_ = loc.stamp_;
00151     time_ = meas_time_;
00152 
00153     SymmetricMatrix cov(3);
00154     cov = 0.0;
00155     cov(1, 1) = 0.0025;
00156     cov(2, 2) = 0.0025;
00157     cov(3, 3) = 0.0025;
00158 
00159     filter_.updateCorrection(loc, cov);
00160 
00161     updatePosition();
00162 
00163     if (reliability < 0 || !use_filter)
00164     {
00165       reliability = probability;
00166       p = kal_p;
00167     }
00168     else
00169     {
00170       p += kal_q;
00171       double k = p / (p + kal_r);
00172       reliability += k * (probability - reliability);
00173       p *= (1 - k);
00174     }
00175   }
00176 
00177   double getLifetime()
00178   {
00179     return filter_.getLifetime();
00180   }
00181 
00182   double getReliability()
00183   {
00184     return reliability;
00185   }
00186 
00187 private:
00188   void updatePosition()
00189   {
00190     StatePosVel est;
00191     filter_.getEstimate(est);
00192 
00193     position_[0] = est.pos_[0];
00194     position_[1] = est.pos_[1];
00195     position_[2] = est.pos_[2];
00196     position_.stamp_ = time_;
00197     position_.frame_id_ = fixed_frame;
00198     double nreliability = fmin(1.0, fmax(0.1, est.vel_.length() / 0.5));
00199     //reliability = fmax(reliability, nreliability);
00200   }
00201 };
00202 
00203 int SavedFeature::nextid = 0;
00204 
00205 
00206 
00207 
00208 class MatchedFeature
00209 {
00210 public:
00211   SampleSet* candidate_;
00212   SavedFeature* closest_;
00213   float distance_;
00214   double probability_;
00215 
00216   MatchedFeature(SampleSet* candidate, SavedFeature* closest, float distance, double probability)
00217     : candidate_(candidate)
00218     , closest_(closest)
00219     , distance_(distance)
00220     , probability_(probability)
00221   {}
00222 
00223   inline bool operator< (const MatchedFeature& b) const
00224   {
00225     return (distance_ <  b.distance_);
00226   }
00227 };
00228 
00229 int g_argc;
00230 char** g_argv;
00231 
00232 
00233 
00234 
00235 // actual legdetector node
00236 class LegDetector
00237 {
00238 public:
00239   NodeHandle nh_;
00240 
00241   TransformListener tfl_;
00242 
00243   ScanMask mask_;
00244 
00245   int mask_count_;
00246 
00247   CvRTrees forest;
00248 
00249   float connected_thresh_;
00250 
00251   int feat_count_;
00252 
00253   char save_[100];
00254 
00255   list<SavedFeature*> saved_features_;
00256   boost::mutex saved_mutex_;
00257 
00258   int feature_id_;
00259 
00260   bool use_seeds_;
00261   bool publish_legs_, publish_people_, publish_leg_markers_, publish_people_markers_;
00262   int next_p_id_;
00263   double leg_reliability_limit_;
00264   int min_points_per_group;
00265 
00266   ros::Publisher people_measurements_pub_;
00267   ros::Publisher leg_measurements_pub_;
00268   ros::Publisher markers_pub_;
00269 
00270   dynamic_reconfigure::Server<leg_detector::LegDetectorConfig> server_;
00271 
00272   message_filters::Subscriber<people_msgs::PositionMeasurement> people_sub_;
00273   message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub_;
00274   tf::MessageFilter<people_msgs::PositionMeasurement> people_notifier_;
00275   tf::MessageFilter<sensor_msgs::LaserScan> laser_notifier_;
00276 
00277   LegDetector(ros::NodeHandle nh) :
00278     nh_(nh),
00279     mask_count_(0),
00280     feat_count_(0),
00281     next_p_id_(0),
00282     people_sub_(nh_, "people_tracker_filter", 10),
00283     laser_sub_(nh_, "scan", 10),
00284     people_notifier_(people_sub_, tfl_, fixed_frame, 10),
00285     laser_notifier_(laser_sub_, tfl_, fixed_frame, 10)
00286   {
00287     if (g_argc > 1)
00288     {
00289       forest.load(g_argv[1]);
00290       feat_count_ = forest.get_active_var_mask()->cols;
00291       printf("Loaded forest with %d features: %s\n", feat_count_, g_argv[1]);
00292     }
00293     else
00294     {
00295       printf("Please provide a trained random forests classifier as an input.\n");
00296       shutdown();
00297     }
00298 
00299     nh_.param<bool>("use_seeds", use_seeds_, !true);
00300 
00301     // advertise topics
00302     leg_measurements_pub_ = nh_.advertise<people_msgs::PositionMeasurementArray>("leg_tracker_measurements", 0);
00303     people_measurements_pub_ = nh_.advertise<people_msgs::PositionMeasurementArray>("people_tracker_measurements", 0);
00304     markers_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 20);
00305 
00306     if (use_seeds_)
00307     {
00308       people_notifier_.registerCallback(boost::bind(&LegDetector::peopleCallback, this, _1));
00309       people_notifier_.setTolerance(ros::Duration(0.01));
00310     }
00311     laser_notifier_.registerCallback(boost::bind(&LegDetector::laserCallback, this, _1));
00312     laser_notifier_.setTolerance(ros::Duration(0.01));
00313 
00314     dynamic_reconfigure::Server<leg_detector::LegDetectorConfig>::CallbackType f;
00315     f = boost::bind(&LegDetector::configure, this, _1, _2);
00316     server_.setCallback(f);
00317 
00318     feature_id_ = 0;
00319   }
00320 
00321 
00322   ~LegDetector()
00323   {
00324   }
00325 
00326   void configure(leg_detector::LegDetectorConfig &config, uint32_t level)
00327   {
00328     connected_thresh_       = config.connection_threshold;
00329     min_points_per_group    = config.min_points_per_group;
00330     leg_reliability_limit_  = config.leg_reliability_limit;
00331     publish_legs_           = config.publish_legs;
00332     publish_people_         = config.publish_people;
00333     publish_leg_markers_    = config.publish_leg_markers;
00334     publish_people_markers_ = config.publish_people_markers;
00335 
00336     no_observation_timeout_s = config.no_observation_timeout;
00337     max_second_leg_age_s     = config.max_second_leg_age;
00338     max_track_jump_m         = config.max_track_jump;
00339     max_meas_jump_m          = config.max_meas_jump;
00340     leg_pair_separation_m    = config.leg_pair_separation;
00341     if (fixed_frame.compare(config.fixed_frame) != 0)
00342     {
00343       fixed_frame              = config.fixed_frame;
00344       laser_notifier_.setTargetFrame(fixed_frame);
00345       people_notifier_.setTargetFrame(fixed_frame);
00346     }
00347 
00348     kal_p                    = config.kalman_p;
00349     kal_q                    = config.kalman_q;
00350     kal_r                    = config.kalman_r;
00351     use_filter               = config.kalman_on == 1;
00352   }
00353 
00354   double distance(list<SavedFeature*>::iterator it1,  list<SavedFeature*>::iterator it2)
00355   {
00356     Stamped<Point> one = (*it1)->position_, two = (*it2)->position_;
00357     double dx = one[0] - two[0], dy = one[1] - two[1], dz = one[2] - two[2];
00358     return sqrt(dx * dx + dy * dy + dz * dz);
00359   }
00360 
00361   // Find the tracker that is closest to this person message
00362   // If a tracker was already assigned to a person, keep this assignment when the distance between them is not too large.
00363   void peopleCallback(const people_msgs::PositionMeasurement::ConstPtr& people_meas)
00364   {
00365     // If there are no legs, return.
00366     if (saved_features_.empty())
00367       return;
00368 
00369     Point pt;
00370     pointMsgToTF(people_meas->pos, pt);
00371     Stamped<Point> person_loc(pt, people_meas->header.stamp, people_meas->header.frame_id);
00372     person_loc[2] = 0.0; // Ignore the height of the person measurement.
00373     Stamped<Point> dest_loc(pt, people_meas->header.stamp, people_meas->header.frame_id); // Holder for all transformed pts.
00374 
00375     boost::mutex::scoped_lock lock(saved_mutex_);
00376 
00377     list<SavedFeature*>::iterator closest = saved_features_.end();
00378     list<SavedFeature*>::iterator closest1 = saved_features_.end();
00379     list<SavedFeature*>::iterator closest2 = saved_features_.end();
00380     float closest_dist = max_meas_jump_m;
00381     float closest_pair_dist = 2 * max_meas_jump_m;
00382 
00383     list<SavedFeature*>::iterator begin = saved_features_.begin();
00384     list<SavedFeature*>::iterator end = saved_features_.end();
00385     list<SavedFeature*>::iterator it1, it2;
00386 
00387     // If there's a pair of legs with the right label and within the max dist, return
00388     // If there's one leg with the right label and within the max dist,
00389     //   find a partner for it from the unlabeled legs whose tracks are reasonably new.
00390     //   If no partners exist, label just the one leg.
00391     // If there are no legs with the right label and within the max dist,
00392     //   find a pair of unlabeled legs and assign them the label.
00393     // If all of the above cases fail,
00394     //   find a new unlabeled leg and assign the label.
00395 
00396     // For each tracker, get the distance to this person.
00397     for (it1 = begin; it1 != end; ++it1)
00398     {
00399       try
00400       {
00401         tfl_.transformPoint((*it1)->id_, people_meas->header.stamp,
00402                             person_loc, fixed_frame, dest_loc);
00403         //ROS_INFO("Succesful leg transformation at spot 7");
00404       }
00405       catch (...)
00406       {
00407         ROS_WARN("TF exception spot 7.");
00408       }
00409       (*it1)->dist_to_person_ = dest_loc.length();
00410     }
00411 
00412     // Try to find one or two trackers with the same label and within the max distance of the person.
00413     cout << "Looking for two legs" << endl;
00414     it2 = end;
00415     for (it1 = begin; it1 != end; ++it1)
00416     {
00417       // If this leg belongs to the person...
00418       if ((*it1)->object_id == people_meas->object_id)
00419       {
00420         // and their distance is close enough...
00421         if ((*it1)->dist_to_person_ < max_meas_jump_m)
00422         {
00423           // if this is the first leg we've found, assign it to it2. Otherwise, leave it assigned to it1 and break.
00424           if (it2 == end)
00425             it2 = it1;
00426           else
00427             break;
00428         }
00429         // Otherwise, remove the tracker's label, it doesn't belong to this person.
00430         else
00431         {
00432           // the two trackers moved apart. This should not happen.
00433           (*it1)->object_id = "";
00434         }
00435       }
00436     }
00437     // If we found two legs with the right label and within the max distance, all is good, return.
00438     if (it1 != end && it2 != end)
00439     {
00440       cout << "Found matching pair. The second distance was " << (*it1)->dist_to_person_ << endl;
00441       return;
00442     }
00443 
00444 
00445 
00446     // If we only found one close leg with the right label, let's try to find a second leg that
00447     //   * doesn't yet have a label  (=valid precondition),
00448     //   * is within the max distance,
00449     //   * is less than max_second_leg_age_s old.
00450     cout << "Looking for one leg plus one new leg" << endl;
00451     float dist_between_legs, closest_dist_between_legs;
00452     if (it2 != end)
00453     {
00454       closest_dist = max_meas_jump_m;
00455       closest = saved_features_.end();
00456 
00457       for (it1 = begin; it1 != end; ++it1)
00458       {
00459         // Skip this leg track if:
00460         // - you're already using it.
00461         // - it already has an id.
00462         // - it's too old. Old unassigned trackers are unlikely to be the second leg in a pair.
00463         // - it's too far away from the person.
00464         if ((it1 == it2) || ((*it1)->object_id != "") || ((*it1)->getLifetime() > max_second_leg_age_s) || ((*it1)->dist_to_person_ >= closest_dist))
00465           continue;
00466 
00467         // Get the distance between the two legs
00468         try
00469         {
00470           tfl_.transformPoint((*it1)->id_, (*it2)->position_.stamp_, (*it2)->position_, fixed_frame, dest_loc);
00471         }
00472         catch (...)
00473         {
00474           ROS_WARN("TF exception getting distance between legs.");
00475         }
00476         dist_between_legs = dest_loc.length();
00477 
00478         // If this is the closest dist (and within range), and the legs are close together and unlabeled, mark it.
00479         if (dist_between_legs < leg_pair_separation_m)
00480         {
00481           closest = it1;
00482           closest_dist = (*it1)->dist_to_person_;
00483           closest_dist_between_legs = dist_between_legs;
00484         }
00485       }
00486       // If we found a close, unlabeled leg, set it's label.
00487       if (closest != end)
00488       {
00489         cout << "Replaced one leg with a distance of " << closest_dist << " and a distance between the legs of " << closest_dist_between_legs << endl;
00490         (*closest)->object_id = people_meas->object_id;
00491       }
00492       else
00493       {
00494         cout << "Returned one matched leg only" << endl;
00495       }
00496 
00497       // Regardless of whether we found a second leg, return.
00498       return;
00499     }
00500 
00501     cout << "Looking for a pair of new legs" << endl;
00502     // If we didn't find any legs with this person's label, try to find two unlabeled legs that are close together and close to the tracker.
00503     it1 = saved_features_.begin();
00504     it2 = saved_features_.begin();
00505     closest = saved_features_.end();
00506     closest1 = saved_features_.end();
00507     closest2 = saved_features_.end();
00508     closest_dist = max_meas_jump_m;
00509     closest_pair_dist = 2 * max_meas_jump_m;
00510     for (; it1 != end; ++it1)
00511     {
00512       // Only look at trackers without ids and that are not too far away.
00513       if ((*it1)->object_id != "" || (*it1)->dist_to_person_ >= max_meas_jump_m)
00514         continue;
00515 
00516       // Keep the single closest leg around in case none of the pairs work out.
00517       if ((*it1)->dist_to_person_ < closest_dist)
00518       {
00519         closest_dist = (*it1)->dist_to_person_;
00520         closest = it1;
00521       }
00522 
00523       // Find a second leg.
00524       it2 = it1;
00525       it2++;
00526       for (; it2 != end; ++it2)
00527       {
00528         // Only look at trackers without ids and that are not too far away.
00529         if ((*it2)->object_id != "" || (*it2)->dist_to_person_ >= max_meas_jump_m)
00530           continue;
00531 
00532         // Get the distance between the two legs
00533         try
00534         {
00535           tfl_.transformPoint((*it1)->id_, (*it2)->position_.stamp_, (*it2)->position_, fixed_frame, dest_loc);
00536         }
00537         catch (...)
00538         {
00539           ROS_WARN("TF exception getting distance between legs in spot 2.");
00540         }
00541         dist_between_legs = dest_loc.length();
00542 
00543         // Ensure that this pair of legs is the closest pair to the tracker, and that the distance between the legs isn't too large.
00544         if ((*it1)->dist_to_person_ + (*it2)->dist_to_person_ < closest_pair_dist && dist_between_legs < leg_pair_separation_m)
00545         {
00546           closest_pair_dist = (*it1)->dist_to_person_ + (*it2)->dist_to_person_;
00547           closest1 = it1;
00548           closest2 = it2;
00549           closest_dist_between_legs = dist_between_legs;
00550         }
00551       }
00552     }
00553     // Found a pair of legs.
00554     if (closest1 != end && closest2 != end)
00555     {
00556       (*closest1)->object_id = people_meas->object_id;
00557       (*closest2)->object_id = people_meas->object_id;
00558       cout << "Found a completely new pair with total distance " << closest_pair_dist << " and a distance between the legs of " << closest_dist_between_legs << endl;
00559       return;
00560     }
00561 
00562     cout << "Looking for just one leg" << endl;
00563     // No pair worked, try for just one leg.
00564     if (closest != end)
00565     {
00566       (*closest)->object_id = people_meas->object_id;
00567       cout << "Returned one new leg only" << endl;
00568       return;
00569     }
00570 
00571     cout << "Nothing matched" << endl;
00572   }
00573 
00574   void pairLegs()
00575   {
00576     // Deal With legs that already have ids
00577     list<SavedFeature*>::iterator begin = saved_features_.begin();
00578     list<SavedFeature*>::iterator end = saved_features_.end();
00579     list<SavedFeature*>::iterator leg1, leg2, best, it;
00580 
00581     for (leg1 = begin; leg1 != end; ++leg1)
00582     {
00583       // If this leg has no id, skip
00584       if ((*leg1)->object_id == "")
00585         continue;
00586 
00587       leg2 = end;
00588       best = end;
00589       double closest_dist = leg_pair_separation_m;
00590       for (it = begin; it != end; ++it)
00591       {
00592         if (it == leg1) continue;
00593 
00594         if ((*it)->object_id == (*leg1)->object_id)
00595         {
00596           leg2 = it;
00597           break;
00598         }
00599 
00600         if ((*it)->object_id != "")
00601           continue;
00602 
00603         double d = distance(it, leg1);
00604         if (((*it)->getLifetime() <= max_second_leg_age_s)
00605             && (d < closest_dist))
00606         {
00607           closest_dist = d;
00608           best = it;
00609         }
00610 
00611       }
00612 
00613       if (leg2 != end)
00614       {
00615         double dist_between_legs = distance(leg1, leg2);
00616         if (dist_between_legs > leg_pair_separation_m)
00617         {
00618           (*leg1)->object_id = "";
00619           (*leg1)->other = NULL;
00620           (*leg2)->object_id = "";
00621           (*leg2)->other = NULL;
00622         }
00623         else
00624         {
00625           (*leg1)->other = *leg2;
00626           (*leg2)->other = *leg1;
00627         }
00628       }
00629       else if (best != end)
00630       {
00631         (*best)->object_id = (*leg1)->object_id;
00632         (*leg1)->other = *best;
00633         (*best)->other = *leg1;
00634       }
00635     }
00636 
00637     // Attempt to pair up legs with no id
00638     for (;;)
00639     {
00640       list<SavedFeature*>::iterator best1 = end, best2 = end;
00641       double closest_dist = leg_pair_separation_m;
00642 
00643       for (leg1 = begin; leg1 != end; ++leg1)
00644       {
00645         // If this leg has an id or low reliability, skip
00646         if ((*leg1)->object_id != ""
00647             || (*leg1)->getReliability() < leg_reliability_limit_)
00648           continue;
00649 
00650         for (leg2 = begin; leg2 != end; ++leg2)
00651         {
00652           if (((*leg2)->object_id != "")
00653               || ((*leg2)->getReliability() < leg_reliability_limit_)
00654               || (leg1 == leg2)) continue;
00655           double d = distance(leg1, leg2);
00656           if (d < closest_dist)
00657           {
00658             best1 = leg1;
00659             best2 = leg2;
00660           }
00661         }
00662       }
00663 
00664       if (best1 != end)
00665       {
00666         char id[100];
00667         snprintf(id, 100, "Person%d", next_p_id_++);
00668         (*best1)->object_id = std::string(id);
00669         (*best2)->object_id = std::string(id);
00670         (*best1)->other = *best2;
00671         (*best2)->other = *best1;
00672       }
00673       else
00674       {
00675         break;
00676       }
00677     }
00678   }
00679 
00680   void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00681   {
00682     ScanProcessor processor(*scan, mask_);
00683 
00684     processor.splitConnected(connected_thresh_);
00685     processor.removeLessThan(5);
00686 
00687     CvMat* tmp_mat = cvCreateMat(1, feat_count_, CV_32FC1);
00688 
00689     // if no measurement matches to a tracker in the last <no_observation_timeout>  seconds: erase tracker
00690     ros::Time purge = scan->header.stamp + ros::Duration().fromSec(-no_observation_timeout_s);
00691     list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00692     while (sf_iter != saved_features_.end())
00693     {
00694       if ((*sf_iter)->meas_time_ < purge)
00695       {
00696         if ((*sf_iter)->other)
00697           (*sf_iter)->other->other = NULL;
00698         delete(*sf_iter);
00699         saved_features_.erase(sf_iter++);
00700       }
00701       else
00702         ++sf_iter;
00703     }
00704 
00705 
00706     // System update of trackers, and copy updated ones in propagate list
00707     list<SavedFeature*> propagated;
00708     for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00709          sf_iter != saved_features_.end();
00710          sf_iter++)
00711     {
00712       (*sf_iter)->propagate(scan->header.stamp);
00713       propagated.push_back(*sf_iter);
00714     }
00715 
00716 
00717     // Detection step: build up the set of "candidate" clusters
00718     // For each candidate, find the closest tracker (within threshold) and add to the match list
00719     // If no tracker is found, start a new one
00720     multiset<MatchedFeature> matches;
00721     for (list<SampleSet*>::iterator i = processor.getClusters().begin();
00722          i != processor.getClusters().end();
00723          i++)
00724     {
00725       vector<float> f = calcLegFeatures(*i, *scan);
00726 
00727       for (int k = 0; k < feat_count_; k++)
00728         tmp_mat->data.fl[k] = (float)(f[k]);
00729 
00730       float probability = forest.predict_prob(tmp_mat);
00731       Stamped<Point> loc((*i)->center(), scan->header.stamp, scan->header.frame_id);
00732       try
00733       {
00734         tfl_.transformPoint(fixed_frame, loc, loc);
00735       }
00736       catch (...)
00737       {
00738         ROS_WARN("TF exception spot 3.");
00739       }
00740 
00741       list<SavedFeature*>::iterator closest = propagated.end();
00742       float closest_dist = max_track_jump_m;
00743 
00744       for (list<SavedFeature*>::iterator pf_iter = propagated.begin();
00745            pf_iter != propagated.end();
00746            pf_iter++)
00747       {
00748         // find the closest distance between candidate and trackers
00749         float dist = loc.distance((*pf_iter)->position_);
00750         if (dist < closest_dist)
00751         {
00752           closest = pf_iter;
00753           closest_dist = dist;
00754         }
00755       }
00756       // Nothing close to it, start a new track
00757       if (closest == propagated.end())
00758       {
00759         list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(), new SavedFeature(loc, tfl_));
00760       }
00761       // Add the candidate, the tracker and the distance to a match list
00762       else
00763         matches.insert(MatchedFeature(*i, *closest, closest_dist, probability));
00764     }
00765 
00766     // loop through _sorted_ matches list
00767     // find the match with the shortest distance for each tracker
00768     while (matches.size() > 0)
00769     {
00770       multiset<MatchedFeature>::iterator matched_iter = matches.begin();
00771       bool found = false;
00772       list<SavedFeature*>::iterator pf_iter = propagated.begin();
00773       while (pf_iter != propagated.end())
00774       {
00775         // update the tracker with this candidate
00776         if (matched_iter->closest_ == *pf_iter)
00777         {
00778           // Transform candidate to fixed frame
00779           Stamped<Point> loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id);
00780           try
00781           {
00782             tfl_.transformPoint(fixed_frame, loc, loc);
00783           }
00784           catch (...)
00785           {
00786             ROS_WARN("TF exception spot 4.");
00787           }
00788 
00789           // Update the tracker with the candidate location
00790           matched_iter->closest_->update(loc, matched_iter->probability_);
00791 
00792           // remove this match and
00793           matches.erase(matched_iter);
00794           propagated.erase(pf_iter++);
00795           found = true;
00796           break;
00797         }
00798         // still looking for the tracker to update
00799         else
00800         {
00801           pf_iter++;
00802         }
00803       }
00804 
00805       // didn't find tracker to update, because it was deleted above
00806       // try to assign the candidate to another tracker
00807       if (!found)
00808       {
00809         Stamped<Point> loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id);
00810         try
00811         {
00812           tfl_.transformPoint(fixed_frame, loc, loc);
00813         }
00814         catch (...)
00815         {
00816           ROS_WARN("TF exception spot 5.");
00817         }
00818 
00819         list<SavedFeature*>::iterator closest = propagated.end();
00820         float closest_dist = max_track_jump_m;
00821 
00822         for (list<SavedFeature*>::iterator remain_iter = propagated.begin();
00823              remain_iter != propagated.end();
00824              remain_iter++)
00825         {
00826           float dist = loc.distance((*remain_iter)->position_);
00827           if (dist < closest_dist)
00828           {
00829             closest = remain_iter;
00830             closest_dist = dist;
00831           }
00832         }
00833 
00834         // no tracker is within a threshold of this candidate
00835         // so create a new tracker for this candidate
00836         if (closest == propagated.end())
00837           list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(), new SavedFeature(loc, tfl_));
00838         else
00839           matches.insert(MatchedFeature(matched_iter->candidate_, *closest, closest_dist, matched_iter->probability_));
00840         matches.erase(matched_iter);
00841       }
00842     }
00843 
00844     cvReleaseMat(&tmp_mat);
00845     tmp_mat = 0;
00846     if (!use_seeds_)
00847       pairLegs();
00848 
00849     // Publish Data!
00850     int i = 0;
00851     vector<people_msgs::PositionMeasurement> people;
00852     vector<people_msgs::PositionMeasurement> legs;
00853 
00854     for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00855          sf_iter != saved_features_.end();
00856          sf_iter++, i++)
00857     {
00858       // reliability
00859       double reliability = (*sf_iter)->getReliability();
00860 
00861       if ((*sf_iter)->getReliability() > leg_reliability_limit_
00862           && publish_legs_)
00863       {
00864         people_msgs::PositionMeasurement pos;
00865         pos.header.stamp = scan->header.stamp;
00866         pos.header.frame_id = fixed_frame;
00867         pos.name = "leg_detector";
00868         pos.object_id = (*sf_iter)->id_;
00869         pos.pos.x = (*sf_iter)->position_[0];
00870         pos.pos.y = (*sf_iter)->position_[1];
00871         pos.pos.z = (*sf_iter)->position_[2];
00872         pos.reliability = reliability;
00873         pos.covariance[0] = pow(0.3 / reliability, 2.0);
00874         pos.covariance[1] = 0.0;
00875         pos.covariance[2] = 0.0;
00876         pos.covariance[3] = 0.0;
00877         pos.covariance[4] = pow(0.3 / reliability, 2.0);
00878         pos.covariance[5] = 0.0;
00879         pos.covariance[6] = 0.0;
00880         pos.covariance[7] = 0.0;
00881         pos.covariance[8] = 10000.0;
00882         pos.initialization = 0;
00883         legs.push_back(pos);
00884 
00885       }
00886 
00887       if (publish_leg_markers_)
00888       {
00889         visualization_msgs::Marker m;
00890         m.header.stamp = (*sf_iter)->time_;
00891         m.header.frame_id = fixed_frame;
00892         m.ns = "LEGS";
00893         m.id = i;
00894         m.type = m.SPHERE;
00895         m.pose.position.x = (*sf_iter)->position_[0];
00896         m.pose.position.y = (*sf_iter)->position_[1];
00897         m.pose.position.z = (*sf_iter)->position_[2];
00898 
00899         m.scale.x = .1;
00900         m.scale.y = .1;
00901         m.scale.z = .1;
00902         m.color.a = 1;
00903         m.lifetime = ros::Duration(0.5);
00904         if ((*sf_iter)->object_id != "")
00905         {
00906           m.color.r = 1;
00907         }
00908         else
00909         {
00910           m.color.b = (*sf_iter)->getReliability();
00911         }
00912 
00913         markers_pub_.publish(m);
00914       }
00915 
00916       if (publish_people_ || publish_people_markers_)
00917       {
00918         SavedFeature* other = (*sf_iter)->other;
00919         if (other != NULL && other < (*sf_iter))
00920         {
00921           double dx = ((*sf_iter)->position_[0] + other->position_[0]) / 2,
00922                  dy = ((*sf_iter)->position_[1] + other->position_[1]) / 2,
00923                  dz = ((*sf_iter)->position_[2] + other->position_[2]) / 2;
00924 
00925           if (publish_people_)
00926           {
00927             reliability = reliability * other->reliability;
00928             people_msgs::PositionMeasurement pos;
00929             pos.header.stamp = (*sf_iter)->time_;
00930             pos.header.frame_id = fixed_frame;
00931             pos.name = (*sf_iter)->object_id;;
00932             pos.object_id = (*sf_iter)->id_ + "|" + other->id_;
00933             pos.pos.x = dx;
00934             pos.pos.y = dy;
00935             pos.pos.z = dz;
00936             pos.reliability = reliability;
00937             pos.covariance[0] = pow(0.3 / reliability, 2.0);
00938             pos.covariance[1] = 0.0;
00939             pos.covariance[2] = 0.0;
00940             pos.covariance[3] = 0.0;
00941             pos.covariance[4] = pow(0.3 / reliability, 2.0);
00942             pos.covariance[5] = 0.0;
00943             pos.covariance[6] = 0.0;
00944             pos.covariance[7] = 0.0;
00945             pos.covariance[8] = 10000.0;
00946             pos.initialization = 0;
00947             people.push_back(pos);
00948           }
00949 
00950           if (publish_people_markers_)
00951           {
00952             visualization_msgs::Marker m;
00953             m.header.stamp = (*sf_iter)->time_;
00954             m.header.frame_id = fixed_frame;
00955             m.ns = "PEOPLE";
00956             m.id = i;
00957             m.type = m.SPHERE;
00958             m.pose.position.x = dx;
00959             m.pose.position.y = dy;
00960             m.pose.position.z = dz;
00961             m.scale.x = .2;
00962             m.scale.y = .2;
00963             m.scale.z = .2;
00964             m.color.a = 1;
00965             m.color.g = 1;
00966             m.lifetime = ros::Duration(0.5);
00967 
00968             markers_pub_.publish(m);
00969           }
00970         }
00971       }
00972     }
00973 
00974     people_msgs::PositionMeasurementArray array;
00975     array.header.stamp = ros::Time::now();
00976     if (publish_legs_)
00977     {
00978       array.people = legs;
00979       leg_measurements_pub_.publish(array);
00980     }
00981     if (publish_people_)
00982     {
00983       array.people = people;
00984       people_measurements_pub_.publish(array);
00985     }
00986   }
00987 };
00988 
00989 int main(int argc, char **argv)
00990 {
00991   ros::init(argc, argv, "leg_detector");
00992   g_argc = argc;
00993   g_argv = argv;
00994   ros::NodeHandle nh;
00995   LegDetector ld(nh);
00996   ros::spin();
00997 
00998   return 0;
00999 }
01000 


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Apr 13 2017 02:41:52