52 TrackerParticle::TrackerParticle(
const string& name,
unsigned int num_particles,
const StatePosVel& sysnoise):
54 prior_(num_particles),
58 tracker_initialized_(false),
59 num_particles_(num_particles)
74 cout <<
"Initializing tracker with " <<
num_particles_ <<
" particles, with covariance " 75 << sigma <<
" around " << mu << endl;
81 prior_.ListOfSamplesSet(prior_samples);
116 assert(cov.columns() == 3);
147 est.pos.x = tmp.
pos_[0];
148 est.pos.
y = tmp.
pos_[1];
149 est.pos.
z = tmp.
pos_[2];
const std::string & getName() const
return the name of the tracker
virtual double getLifetime() const
return the lifetime of the tracker
virtual void getEstimate(BFL::StatePosVel &est) const
get filter posterior
Class representing state with pos and vel.
virtual ~TrackerParticle()
destructor
virtual void initialize(const BFL::StatePosVel &mu, const BFL::StatePosVel &sigma, const double time)
initialize tracker
BFL::BootstrapFilter< BFL::StatePosVel, tf::Vector3 > * filter_
virtual bool updatePrediction(const double time)
update tracker
virtual bool updateCorrection(const tf::Vector3 &meas, const MatrixWrapper::SymmetricMatrix &cov)
Class representing gaussian pos_vel.
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool tracker_initialized_
TFSIMD_FORCE_INLINE const tfScalar & y() const
unsigned int num_particles_
MatrixWrapper::Matrix getHistogramVel(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
void getParticleCloud(const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const
MatrixWrapper::Matrix getHistogramPos(const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const
Get histogram from certain area.
BFL::MeasModelPos meas_model_
BFL::SysModelPosVel sys_model_
virtual double getTime() const
return the time of the tracker
bool SampleFrom(vector< Sample< StatePosVel > > &list_samples, const int num_samples, int method=DEFAULT, void *args=NULL) const
Class representing a posvel mcpdf.