53 DetectorParticle::DetectorParticle(
unsigned int num_particles):
54 prior_(num_particles),
58 detector_initialized_(false),
59 num_particles_(num_particles)
74 cout <<
"Initializing detector with " <<
num_particles_ <<
" particles, with uniform size " 75 << size <<
" around " << mu << endl;
80 prior_.ListOfSamplesSet(prior_samples);
110 assert(cov.columns() == 3);
149 est.header.frame_id =
"base_link";
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
bool detector_initialized_
unsigned int num_particles_
void getEstimate(tf::Vector3 &est) const
get filter posterior
TFSIMD_FORCE_INLINE const tfScalar & z() const
BFL::BootstrapFilter< tf::Vector3, tf::Vector3 > * filter_
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.
~DetectorParticle()
destructor
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_