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 #ifndef __TRACKER_PARTICLE__
00038 #define __TRACKER_PARTICLE__
00039
00040 #include "tracker.h"
00041
00042
00043 #include <filter/bootstrapfilter.h>
00044 #include "state_pos_vel.h"
00045 #include "mcpdf_pos_vel.h"
00046 #include "sysmodel_pos_vel.h"
00047 #include "measmodel_pos.h"
00048
00049
00050 #include <tf/tf.h>
00051
00052
00053 #include <sensor_msgs/PointCloud.h>
00054
00055
00056 #include <fstream>
00057
00058 namespace estimation
00059 {
00060
00061 class TrackerParticle: public Tracker
00062 {
00063 public:
00065 TrackerParticle(const std::string& name, unsigned int num_particles, const BFL::StatePosVel& sysnoise);
00066
00068 virtual ~TrackerParticle();
00069
00071 virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time);
00072
00074 virtual bool isInitialized() const {return tracker_initialized_;};
00075
00077 virtual double getQuality() const {return quality_;};
00078
00080 virtual double getLifetime() const;
00081
00083 virtual double getTime() const;
00084
00086 virtual bool updatePrediction(const double time);
00087 virtual bool updateCorrection(const tf::Vector3& meas,
00088 const MatrixWrapper::SymmetricMatrix& cov);
00089
00091 virtual void getEstimate(BFL::StatePosVel& est) const;
00092 virtual void getEstimate(people_msgs::PositionMeasurement& est) const;
00093
00094
00095 void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const;
00096
00098 MatrixWrapper::Matrix getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00099 MatrixWrapper::Matrix getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00100
00101 private:
00102
00103 BFL::MCPdfPosVel prior_;
00104 BFL::BootstrapFilter<BFL::StatePosVel, tf::Vector3>* filter_;
00105 BFL::SysModelPosVel sys_model_;
00106 BFL::MeasModelPos meas_model_;
00107
00108
00109 bool tracker_initialized_;
00110 double init_time_, filter_time_, quality_;
00111 unsigned int num_particles_;
00112
00113
00114 };
00115
00116 };
00117
00118 #endif