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 __DETECTOR_PARTICLE__
00038 #define __DETECTOR_PARTICLE__
00039
00040 #include "tracker.h"
00041
00042
00043 #include <filter/bootstrapfilter.h>
00044 #include "mcpdf_vector.h"
00045 #include "measmodel_vector.h"
00046 #include "sysmodel_vector.h"
00047
00048
00049 #include <tf/tf.h>
00050
00051
00052 #include <sensor_msgs/PointCloud.h>
00053
00054
00055 #include <fstream>
00056
00057 namespace estimation
00058 {
00059
00060 class DetectorParticle
00061 {
00062 public:
00064 DetectorParticle(unsigned int num_particles);
00065
00067 ~DetectorParticle();
00068
00070 void initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time);
00071
00073 bool isInitialized() const
00074 {
00075 return detector_initialized_;
00076 };
00077
00079 double getQuality() const
00080 {
00081 return quality_;
00082 };
00083
00085 bool updatePrediction(const double dt);
00086 bool updateCorrection(const tf::Vector3& meas,
00087 const MatrixWrapper::SymmetricMatrix& cov,
00088 const double time);
00089
00091 void getEstimate(tf::Vector3& est) const;
00092 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 getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00099
00100 private:
00101
00102 BFL::MCPdfVector prior_;
00103 BFL::BootstrapFilter<tf::Vector3, tf::Vector3>* filter_;
00104 BFL::SysModelVector sys_model_;
00105 BFL::MeasModelVector meas_model_;
00106
00107
00108 bool detector_initialized_;
00109 double filter_time_, quality_;
00110 unsigned int num_particles_;
00111
00112
00113 };
00114
00115 };
00116
00117 #endif