43 using MatrixWrapper::Matrix;
47 prior_(num_particles),
51 detector_initialized_(false),
52 num_particles_(num_particles)
64 std::cout <<
"Initializing detector with " <<
num_particles_ <<
" particles, with uniform size " 65 << size <<
" around " << mu << std::endl;
68 std::vector<BFL::Sample<tf::Vector3> > prior_samples(
num_particles_);
96 assert(cov.columns() == 3);
132 est.header.frame_id =
"base_link";
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
virtual MCPdf< StateVar > * PostGet()
bool ListOfSamplesSet(const vector< WeightedSample< T > > &list_of_samples)
bool detector_initialized_
unsigned int num_particles_
void getEstimate(tf::Vector3 &est) const
get filter posterior
DetectorParticle(unsigned int num_particles)
constructor
TFSIMD_FORCE_INLINE const tfScalar & z() const
BFL::BootstrapFilter< tf::Vector3, tf::Vector3 > * filter_
TFSIMD_FORCE_INLINE Vector3()
TFSIMD_FORCE_INLINE const tfScalar & y() const
Class representing a vector mcpdf.
BFL::MeasModelVector meas_model_
bool updatePrediction(const double dt)
update detector
MatrixWrapper::Matrix getHistogram(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get histogram from certain area.
ConditionalPdf< MeasVar, StateVar > * MeasurementPdfGet()
~DetectorParticle()
destructor
virtual bool Update(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
bool updateCorrection(const tf::Vector3 &meas, const MatrixWrapper::SymmetricMatrix &cov, const double time)
void initialize(const tf::Vector3 &mu, const tf::Vector3 &size, const double time)
initialize detector
BFL::SysModelVector sys_model_