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
00075 {
00076 return tracker_initialized_;
00077 };
00078
00080 virtual double getQuality() const
00081 {
00082 return quality_;
00083 };
00084
00086 virtual double getLifetime() const;
00087
00089 virtual double getTime() const;
00090
00092 virtual bool updatePrediction(const double time);
00093 virtual bool updateCorrection(const tf::Vector3& meas,
00094 const MatrixWrapper::SymmetricMatrix& cov);
00095
00097 virtual void getEstimate(BFL::StatePosVel& est) const;
00098 virtual void getEstimate(people_msgs::PositionMeasurement& est) const;
00099
00100
00101 void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const;
00102
00104 MatrixWrapper::Matrix getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00105 MatrixWrapper::Matrix getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00106
00107 private:
00108
00109 BFL::MCPdfPosVel prior_;
00110 BFL::BootstrapFilter<BFL::StatePosVel, tf::Vector3>* filter_;
00111 BFL::SysModelPosVel sys_model_;
00112 BFL::MeasModelPos meas_model_;
00113
00114
00115 bool tracker_initialized_;
00116 double init_time_, filter_time_, quality_;
00117 unsigned int num_particles_;
00118
00119
00120 };
00121
00122 };
00123
00124 #endif