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
00038 #ifndef __DETECTOR_PARTICLE__
00039 #define __DETECTOR_PARTICLE__
00040
00041 #include "tracker.h"
00042
00043
00044 #include <filter/bootstrapfilter.h>
00045 #include "mcpdf_vector.h"
00046 #include "measmodel_vector.h"
00047 #include "sysmodel_vector.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 DetectorParticle
00062 {
00063 public:
00065 DetectorParticle(unsigned int num_particles);
00066
00068 ~DetectorParticle();
00069
00071 void initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time);
00072
00074 bool isInitialized() const {return detector_initialized_;};
00075
00077 double getQuality() const {return quality_;};
00078
00080 bool updatePrediction(const double dt);
00081 bool updateCorrection(const tf::Vector3& meas,
00082 const MatrixWrapper::SymmetricMatrix& cov,
00083 const double time);
00084
00086 void getEstimate(tf::Vector3& est) const;
00087 void getEstimate(srs_msgs::PositionMeasurement& est) const;
00088
00089
00090 void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const;
00091
00093 MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00094
00095 private:
00096
00097 BFL::MCPdfVector prior_;
00098 BFL::BootstrapFilter<tf::Vector3, tf::Vector3>* filter_;
00099 BFL::SysModelVector sys_model_;
00100 BFL::MeasModelVector meas_model_;
00101
00102
00103 bool detector_initialized_;
00104 double filter_time_, quality_;
00105 unsigned int num_particles_;
00106
00107
00108 };
00109
00110 };
00111
00112 #endif