Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "people_tracking_filter/tracker_particle.h"
00038 #include "people_tracking_filter/gaussian_pos_vel.h"
00039
00040 using namespace MatrixWrapper;
00041 using namespace BFL;
00042 using namespace tf;
00043 using namespace std;
00044 using namespace ros;
00045
00046
00047
00048
00049 namespace estimation
00050 {
00051
00052 TrackerParticle::TrackerParticle(const string& name, unsigned int num_particles, const StatePosVel& sysnoise):
00053 Tracker(name),
00054 prior_(num_particles),
00055 filter_(NULL),
00056 sys_model_(sysnoise),
00057 meas_model_(tf::Vector3(0.1, 0.1, 0.1)),
00058 tracker_initialized_(false),
00059 num_particles_(num_particles)
00060 {};
00061
00062
00063
00064
00065 TrackerParticle::~TrackerParticle()
00066 {
00067 if (filter_) delete filter_;
00068 };
00069
00070
00071
00072 void TrackerParticle::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
00073 {
00074 cout << "Initializing tracker with " << num_particles_ << " particles, with covariance "
00075 << sigma << " around " << mu << endl;
00076
00077
00078 GaussianPosVel gauss_pos_vel(mu, sigma);
00079 vector<Sample<StatePosVel> > prior_samples(num_particles_);
00080 gauss_pos_vel.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL);
00081 prior_.ListOfSamplesSet(prior_samples);
00082 filter_ = new BootstrapFilter<StatePosVel, tf::Vector3>(&prior_, &prior_, 0, num_particles_ / 4.0);
00083
00084
00085 tracker_initialized_ = true;
00086 quality_ = 1;
00087 filter_time_ = time;
00088 init_time_ = time;
00089 }
00090
00091
00092
00093
00094
00095 bool TrackerParticle::updatePrediction(const double time)
00096 {
00097 bool res = true;
00098 if (time > filter_time_)
00099 {
00100
00101 sys_model_.SetDt(time - filter_time_);
00102 filter_time_ = time;
00103
00104
00105 res = filter_->Update(&sys_model_);
00106 if (!res) quality_ = 0;
00107 }
00108 return res;
00109 };
00110
00111
00112
00113
00114 bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
00115 {
00116 assert(cov.columns() == 3);
00117
00118
00119 ((MeasPdfPos*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov);
00120
00121
00122 bool res = filter_->Update(&meas_model_, meas);
00123 if (!res) quality_ = 0;
00124
00125 return res;
00126 };
00127
00128
00129
00130 void TrackerParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00131 {
00132 ((MCPdfPosVel*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud);
00133 };
00134
00135
00136
00137 void TrackerParticle::getEstimate(StatePosVel& est) const
00138 {
00139 est = ((MCPdfPosVel*)(filter_->PostGet()))->ExpectedValueGet();
00140 };
00141
00142
00143 void TrackerParticle::getEstimate(people_msgs::PositionMeasurement& est) const
00144 {
00145 StatePosVel tmp = filter_->PostGet()->ExpectedValueGet();
00146
00147 est.pos.x = tmp.pos_[0];
00148 est.pos.y = tmp.pos_[1];
00149 est.pos.z = tmp.pos_[2];
00150
00151 est.header.stamp.fromSec(filter_time_);
00152 est.object_id = getName();
00153 }
00154
00155
00156
00157
00158
00160 Matrix TrackerParticle::getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00161 {
00162 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramPos(min, max, step);
00163 };
00164
00165
00166 Matrix TrackerParticle::getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00167 {
00168 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramVel(min, max, step);
00169 };
00170
00171
00172 double TrackerParticle::getLifetime() const
00173 {
00174 if (tracker_initialized_)
00175 return filter_time_ - init_time_;
00176 else
00177 return 0;
00178 }
00179
00180
00181 double TrackerParticle::getTime() const
00182 {
00183 if (tracker_initialized_)
00184 return filter_time_;
00185 else
00186 return 0;
00187 }
00188 };
00189
00190
00191