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 {return detector_initialized_;};
00074
00076 double getQuality() const {return quality_;};
00077
00079 bool updatePrediction(const double dt);
00080 bool updateCorrection(const tf::Vector3& meas,
00081 const MatrixWrapper::SymmetricMatrix& cov,
00082 const double time);
00083
00085 void getEstimate(tf::Vector3& est) const;
00086 void getEstimate(cob_perception_msgs::PositionMeasurement& est) const;
00087
00088
00089 void getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const;
00090
00092 MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const;
00093
00094 private:
00095
00096 BFL::MCPdfVector prior_;
00097 BFL::BootstrapFilter<tf::Vector3, tf::Vector3>* filter_;
00098 BFL::SysModelVector sys_model_;
00099 BFL::MeasModelVector meas_model_;
00100
00101
00102 bool detector_initialized_;
00103 double filter_time_, quality_;
00104 unsigned int num_particles_;
00105
00106
00107 };
00108
00109 };
00110
00111 #endif