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/detector_particle.h"
00038 #include "people_tracking_filter/uniform_vector.h"
00039
00040 using namespace MatrixWrapper;
00041 using namespace BFL;
00042 using namespace tf;
00043 using namespace std;
00044 using namespace ros;
00045 using namespace geometry_msgs;
00046
00047
00048
00049
00050 namespace estimation
00051 {
00052
00053 DetectorParticle::DetectorParticle(unsigned int num_particles):
00054 prior_(num_particles),
00055 filter_(NULL),
00056 sys_model_(tf::Vector3(0.1, 0.1, 0.1)),
00057 meas_model_(tf::Vector3(0.1, 0.1, 0.1)),
00058 detector_initialized_(false),
00059 num_particles_(num_particles)
00060 {}
00061
00062
00063
00064
00065 DetectorParticle::~DetectorParticle()
00066 {
00067 if (filter_) delete filter_;
00068 }
00069
00070
00071
00072 void DetectorParticle::initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time)
00073 {
00074 cout << "Initializing detector with " << num_particles_ << " particles, with uniform size "
00075 << size << " around " << mu << endl;
00076
00077 UniformVector uniform_vector(mu, size);
00078 vector<Sample<tf::Vector3> > prior_samples(num_particles_);
00079 uniform_vector.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL);
00080 prior_.ListOfSamplesSet(prior_samples);
00081 filter_ = new BootstrapFilter<tf::Vector3, tf::Vector3>(&prior_, &prior_, 0, num_particles_ / 4.0);
00082
00083
00084 detector_initialized_ = true;
00085 quality_ = 1;
00086 filter_time_ = time;
00087 }
00088
00089
00090
00091
00092
00093 bool DetectorParticle::updatePrediction(const double dt)
00094 {
00095
00096 sys_model_.SetDt(dt);
00097
00098
00099 bool res = filter_->Update(&sys_model_);
00100 if (!res) quality_ = 0;
00101
00102 return res;
00103 }
00104
00105
00106
00107
00108 bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov, const double time)
00109 {
00110 assert(cov.columns() == 3);
00111
00112
00113 filter_time_ = time;
00114
00115
00116 ((MeasPdfVector*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov);
00117
00118
00119 bool res = filter_->Update(&meas_model_, meas);
00120 if (!res) quality_ = 0;
00121
00122 return res;
00123 }
00124
00125
00126
00127 void DetectorParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const
00128 {
00129 ((MCPdfVector*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud);
00130 }
00131
00132
00133
00134 void DetectorParticle::getEstimate(tf::Vector3& est) const
00135 {
00136 est = ((MCPdfVector*)(filter_->PostGet()))->ExpectedValueGet();
00137 }
00138
00139
00140 void DetectorParticle::getEstimate(people_msgs::PositionMeasurement& est) const
00141 {
00142 tf::Vector3 tmp = filter_->PostGet()->ExpectedValueGet();
00143
00144 est.pos.x = tmp[0];
00145 est.pos.y = tmp[1];
00146 est.pos.z = tmp[2];
00147
00148 est.header.stamp.fromSec(filter_time_);
00149 est.header.frame_id = "base_link";
00150 }
00151
00152
00153
00154
00155
00157 Matrix DetectorParticle::getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const
00158 {
00159 return ((MCPdfVector*)(filter_->PostGet()))->getHistogram(min, max, step);
00160 }
00161
00162
00163 };
00164
00165
00166