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