#include <tracker_particle.h>
Public Member Functions | |
| virtual void | getEstimate (BFL::StatePosVel &est) const |
| get filter posterior | |
| virtual void | getEstimate (people_msgs::PositionMeasurement &est) const |
| MatrixWrapper::Matrix | getHistogramPos (const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const |
| Get histogram from certain area. | |
| MatrixWrapper::Matrix | getHistogramVel (const tf::Vector3 &min, const tf::Vector3 &max, const tf::Vector3 &step) const |
| virtual double | getLifetime () const |
| return the lifetime of the tracker | |
| void | getParticleCloud (const tf::Vector3 &step, double threshold, sensor_msgs::PointCloud &cloud) const |
| virtual double | getQuality () const |
| return measure for tracker quality: 0=bad 1=good | |
| virtual double | getTime () const |
| return the time of the tracker | |
| virtual void | initialize (const BFL::StatePosVel &mu, const BFL::StatePosVel &sigma, const double time) |
| initialize tracker | |
| virtual bool | isInitialized () const |
| return if tracker was initialized | |
| TrackerParticle (const std::string &name, unsigned int num_particles, const BFL::StatePosVel &sysnoise) | |
| constructor | |
| virtual bool | updateCorrection (const tf::Vector3 &meas, const MatrixWrapper::SymmetricMatrix &cov) |
| virtual bool | updatePrediction (const double time) |
| update tracker | |
| virtual | ~TrackerParticle () |
| destructor | |
Private Attributes | |
| BFL::BootstrapFilter < BFL::StatePosVel, tf::Vector3 > * | filter_ |
| double | filter_time_ |
| double | init_time_ |
| BFL::MeasModelPos | meas_model_ |
| unsigned int | num_particles_ |
| BFL::MCPdfPosVel | prior_ |
| double | quality_ |
| BFL::SysModelPosVel | sys_model_ |
| bool | tracker_initialized_ |
Definition at line 61 of file tracker_particle.h.
| estimation::TrackerParticle::TrackerParticle | ( | const std::string & | name, |
| unsigned int | num_particles, | ||
| const BFL::StatePosVel & | sysnoise | ||
| ) |
constructor
Definition at line 52 of file tracker_particle.cpp.
| estimation::TrackerParticle::~TrackerParticle | ( | ) | [virtual] |
destructor
Definition at line 65 of file tracker_particle.cpp.
| void estimation::TrackerParticle::getEstimate | ( | BFL::StatePosVel & | est | ) | const [virtual] |
get filter posterior
Implements estimation::Tracker.
Definition at line 135 of file tracker_particle.cpp.
| void estimation::TrackerParticle::getEstimate | ( | people_msgs::PositionMeasurement & | est | ) | const [virtual] |
Implements estimation::Tracker.
Definition at line 141 of file tracker_particle.cpp.
| Matrix estimation::TrackerParticle::getHistogramPos | ( | const tf::Vector3 & | min, |
| const tf::Vector3 & | max, | ||
| const tf::Vector3 & | step | ||
| ) | const |
Get histogram from certain area.
Definition at line 158 of file tracker_particle.cpp.
| Matrix estimation::TrackerParticle::getHistogramVel | ( | const tf::Vector3 & | min, |
| const tf::Vector3 & | max, | ||
| const tf::Vector3 & | step | ||
| ) | const |
Definition at line 164 of file tracker_particle.cpp.
| double estimation::TrackerParticle::getLifetime | ( | ) | const [virtual] |
return the lifetime of the tracker
Implements estimation::Tracker.
Definition at line 170 of file tracker_particle.cpp.
| void estimation::TrackerParticle::getParticleCloud | ( | const tf::Vector3 & | step, |
| double | threshold, | ||
| sensor_msgs::PointCloud & | cloud | ||
| ) | const |
Definition at line 128 of file tracker_particle.cpp.
| virtual double estimation::TrackerParticle::getQuality | ( | ) | const [inline, virtual] |
return measure for tracker quality: 0=bad 1=good
Implements estimation::Tracker.
Definition at line 77 of file tracker_particle.h.
| double estimation::TrackerParticle::getTime | ( | ) | const [virtual] |
return the time of the tracker
Implements estimation::Tracker.
Definition at line 179 of file tracker_particle.cpp.
| void estimation::TrackerParticle::initialize | ( | const BFL::StatePosVel & | mu, |
| const BFL::StatePosVel & | sigma, | ||
| const double | time | ||
| ) | [virtual] |
initialize tracker
Implements estimation::Tracker.
Definition at line 71 of file tracker_particle.cpp.
| virtual bool estimation::TrackerParticle::isInitialized | ( | ) | const [inline, virtual] |
return if tracker was initialized
Implements estimation::Tracker.
Definition at line 74 of file tracker_particle.h.
| bool estimation::TrackerParticle::updateCorrection | ( | const tf::Vector3 & | meas, |
| const MatrixWrapper::SymmetricMatrix & | cov | ||
| ) | [virtual] |
Implements estimation::Tracker.
Definition at line 112 of file tracker_particle.cpp.
| bool estimation::TrackerParticle::updatePrediction | ( | const double | time | ) | [virtual] |
BFL::BootstrapFilter<BFL::StatePosVel, tf::Vector3>* estimation::TrackerParticle::filter_ [private] |
Definition at line 104 of file tracker_particle.h.
double estimation::TrackerParticle::filter_time_ [private] |
Definition at line 110 of file tracker_particle.h.
double estimation::TrackerParticle::init_time_ [private] |
Definition at line 110 of file tracker_particle.h.
Definition at line 106 of file tracker_particle.h.
unsigned int estimation::TrackerParticle::num_particles_ [private] |
Definition at line 111 of file tracker_particle.h.
Definition at line 103 of file tracker_particle.h.
double estimation::TrackerParticle::quality_ [private] |
Definition at line 110 of file tracker_particle.h.
Definition at line 105 of file tracker_particle.h.
bool estimation::TrackerParticle::tracker_initialized_ [private] |
Definition at line 109 of file tracker_particle.h.