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
00038 #include "srs_people_tracking_filter/tracker_particle.h"
00039 #include "srs_people_tracking_filter/gaussian_pos_vel.h"
00040
00041 using namespace MatrixWrapper;
00042 using namespace BFL;
00043 using namespace tf;
00044 using namespace std;
00045 using namespace ros;
00046
00047
00048
00049
00050 namespace estimation
00051 {
00052
00053 TrackerParticle::TrackerParticle(const string& name, unsigned int num_particles, const StatePosVel& sysnoise):
00054 Tracker(name),
00055 prior_(num_particles),
00056 filter_(NULL),
00057 sys_model_(sysnoise),
00058 meas_model_(tf::Vector3(0.1,0.1,0.1)),
00059 tracker_initialized_(false),
00060 num_particles_(num_particles)
00061 {};
00062
00063
00064
00065
00066 TrackerParticle::~TrackerParticle(){
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 sys_model_.SetDt(time - filter_time_);
00101 filter_time_ = time;
00102
00103
00104 res = filter_->Update(&sys_model_);
00105 if (!res) quality_ = 0;
00106 }
00107 return res;
00108 };
00109
00110
00111
00112
00113 bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
00114 {
00115 assert(cov.columns() == 3);
00116
00117
00118 ((MeasPdfPos*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov);
00119
00120
00121 bool res = filter_->Update(&meas_model_, meas);
00122 if (!res) quality_ = 0;
00123
00124 return res;
00125 };
00126
00127
00128
00129 void TrackerParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00130 {
00131 ((MCPdfPosVel*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud);
00132 };
00133
00134
00135
00136 void TrackerParticle::getEstimate(StatePosVel& est) const
00137 {
00138 est = ((MCPdfPosVel*)(filter_->PostGet()))->ExpectedValueGet();
00139 };
00140
00141
00142 void TrackerParticle::getEstimate(srs_msgs::PositionMeasurement& est) const
00143 {
00144 StatePosVel tmp = filter_->PostGet()->ExpectedValueGet();
00145
00146 est.pos.x = tmp.pos_[0];
00147 est.pos.y = tmp.pos_[1];
00148 est.pos.z = tmp.pos_[2];
00149
00150 est.header.stamp.fromSec( filter_time_ );
00151 est.object_id = getName();
00152 }
00153
00154
00155
00156
00157
00159 Matrix TrackerParticle::getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00160 {
00161 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramPos(min, max, step);
00162 };
00163
00164
00165 Matrix TrackerParticle::getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00166 {
00167 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramVel(min, max, step);
00168 };
00169
00170
00171 double TrackerParticle::getLifetime() const
00172 {
00173 if (tracker_initialized_)
00174 return filter_time_ - init_time_;
00175 else
00176 return 0;
00177 }
00178
00179
00180 double TrackerParticle::getTime() const
00181 {
00182 if (tracker_initialized_)
00183 return filter_time_;
00184 else
00185 return 0;
00186 }
00187 };
00188
00189
00190