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