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 "cob_people_tracking_filter/tracker_particle.h"
00038 #include "cob_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 if (filter_) delete filter_;
00067 };
00068
00069
00070
00071 void TrackerParticle::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
00072 {
00073 cout << "Initializing tracker with " << num_particles_ << " particles, with covariance "
00074 << sigma << " around " << mu << endl;
00075
00076
00077 GaussianPosVel gauss_pos_vel(mu, sigma);
00078 vector<Sample<StatePosVel> > prior_samples(num_particles_);
00079 gauss_pos_vel.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL);
00080 prior_.ListOfSamplesSet(prior_samples);
00081 filter_ = new BootstrapFilter<StatePosVel, tf::Vector3>(&prior_, &prior_, 0, num_particles_/4.0);
00082
00083
00084 tracker_initialized_ = true;
00085 quality_ = 1;
00086 filter_time_ = time;
00087 init_time_ = time;
00088 }
00089
00090
00091
00092
00093
00094 bool TrackerParticle::updatePrediction(const double time)
00095 {
00096 bool res = true;
00097 if (time > filter_time_){
00098
00099 sys_model_.SetDt(time - filter_time_);
00100 filter_time_ = time;
00101
00102
00103 res = filter_->Update(&sys_model_);
00104 if (!res) quality_ = 0;
00105 }
00106 return res;
00107 };
00108
00109
00110
00111
00112 bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
00113 {
00114 assert(cov.columns() == 3);
00115
00116
00117 ((MeasPdfPos*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov);
00118
00119
00120 bool res = filter_->Update(&meas_model_, meas);
00121 if (!res) quality_ = 0;
00122
00123 return res;
00124 };
00125
00126
00127
00128 void TrackerParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00129 {
00130 ((MCPdfPosVel*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud);
00131 };
00132
00133
00134
00135 void TrackerParticle::getEstimate(StatePosVel& est) const
00136 {
00137 est = ((MCPdfPosVel*)(filter_->PostGet()))->ExpectedValueGet();
00138 };
00139
00140
00141 void TrackerParticle::getEstimate(cob_perception_msgs::PositionMeasurement& est) const
00142 {
00143 StatePosVel tmp = filter_->PostGet()->ExpectedValueGet();
00144
00145 est.pos.x = tmp.pos_[0];
00146 est.pos.y = tmp.pos_[1];
00147 est.pos.z = tmp.pos_[2];
00148
00149 est.header.stamp.fromSec( filter_time_ );
00150 est.object_id = getName();
00151 }
00152
00153
00154
00155
00156
00158 Matrix TrackerParticle::getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00159 {
00160 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramPos(min, max, step);
00161 };
00162
00163
00164 Matrix TrackerParticle::getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00165 {
00166 return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramVel(min, max, step);
00167 };
00168
00169
00170 double TrackerParticle::getLifetime() const
00171 {
00172 if (tracker_initialized_)
00173 return filter_time_ - init_time_;
00174 else
00175 return 0;
00176 }
00177
00178
00179 double TrackerParticle::getTime() const
00180 {
00181 if (tracker_initialized_)
00182 return filter_time_;
00183 else
00184 return 0;
00185 }
00186 };
00187
00188
00189