#include <mrpt_localization_core.h>
Public Types | |
enum | PFStates { NA, INIT, RUN, IDLE } |
Public Member Functions | |
void | init () |
void | observation (CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry) |
PFLocalizationCore () | |
~PFLocalizationCore () | |
Protected Attributes | |
float | init_PDF_max_x |
float | init_PDF_max_y |
float | init_PDF_min_x |
any cell | |
float | init_PDF_min_y |
bool | init_PDF_mode |
int | initial_particle_count_ |
number of particles for initialization | |
mrpt::poses::CPosePDFGaussian | initial_pose_ |
initial posed used in initializeFilter() | |
CMultiMetricMap | metric_map_ |
map | |
CActionRobotMovement2D::TMotionModelOptions | motion_model_default_options_ |
used if there are is not odom | |
CActionRobotMovement2D::TMotionModelOptions | motion_model_options_ |
used with odom value motion noise | |
mrpt::poses::CPose2D | odom_last_observation_ |
correct time | |
mrpt::slam::CMonteCarloLocalization2D | pdf_ |
the filter | |
mrpt::bayes::CParticleFilter | pf_ |
common interface for particle filters | |
mrpt::bayes::CParticleFilter::TParticleFilterStats | pf_stats_ |
filter statistics | |
PFStates | state_ |
updates | |
mrpt::utils::CTicTac | tictac_ |
timer to measure performance | |
mrpt::system::TTimeStamp | time_last_update_ |
time of the last update | |
size_t | update_counter_ |
bool | use_motion_model_default_options_ |
used default odom_params | |
Private Member Functions | |
void | initializeFilter () |
void | updateFilter (CActionCollection::Ptr _action, CSensoryFrame::Ptr _sf) |
Definition at line 56 of file mrpt_localization_core.h.
Definition at line 61 of file mrpt_localization_core.h.
Definition at line 45 of file mrpt_localization_core.cpp.
Definition at line 44 of file mrpt_localization_core.cpp.
void PFLocalizationCore::init | ( | ) |
Initializes the parameter with common values to acive a working filter out of the box
Reimplemented in PFLocalizationNode, and PFLocalization.
Definition at line 46 of file mrpt_localization_core.cpp.
void PFLocalizationCore::initializeFilter | ( | ) | [private] |
Initializes the filter at pose PFLocalizationCore::initial_pose_ with PFLocalizationCore::initial_particle_count_ it is called by the PFLocalizationCore::updateFilter if the state_ == INIT
Definition at line 60 of file mrpt_localization_core.cpp.
void PFLocalizationCore::observation | ( | CSensoryFrame::Ptr | _sf, |
CObservationOdometry::Ptr | _odometry | ||
) |
preprocesses an observation and calls the update PFLocalizationCore::updateFilter If the odom data is null the function will assume a dummy odometry distribution around the last pose
_sf | sensor observation |
_odometry | the odom data can also be NULL |
Definition at line 99 of file mrpt_localization_core.cpp.
void PFLocalizationCore::updateFilter | ( | CActionCollection::Ptr | _action, |
CSensoryFrame::Ptr | _sf | ||
) | [private] |
Definition at line 89 of file mrpt_localization_core.cpp.
float PFLocalizationCore::init_PDF_max_x [protected] |
Definition at line 114 of file mrpt_localization_core.h.
float PFLocalizationCore::init_PDF_max_y [protected] |
Definition at line 116 of file mrpt_localization_core.h.
float PFLocalizationCore::init_PDF_min_x [protected] |
float PFLocalizationCore::init_PDF_min_y [protected] |
Definition at line 115 of file mrpt_localization_core.h.
bool PFLocalizationCore::init_PDF_mode [protected] |
Initial PDF mode: 0 for free space cells, 1 for
Definition at line 111 of file mrpt_localization_core.h.
int PFLocalizationCore::initial_particle_count_ [protected] |
number of particles for initialization
Definition at line 102 of file mrpt_localization_core.h.
mrpt::poses::CPosePDFGaussian PFLocalizationCore::initial_pose_ [protected] |
initial posed used in initializeFilter()
Definition at line 101 of file mrpt_localization_core.h.
CMultiMetricMap PFLocalizationCore::metric_map_ [protected] |
map
Definition at line 94 of file mrpt_localization_core.h.
CActionRobotMovement2D::TMotionModelOptions PFLocalizationCore::motion_model_default_options_ [protected] |
used if there are is not odom
Definition at line 91 of file mrpt_localization_core.h.
CActionRobotMovement2D::TMotionModelOptions PFLocalizationCore::motion_model_options_ [protected] |
used with odom value motion noise
Definition at line 93 of file mrpt_localization_core.h.
mrpt::poses::CPose2D PFLocalizationCore::odom_last_observation_ [protected] |
mrpt::slam::CMonteCarloLocalization2D PFLocalizationCore::pdf_ [protected] |
the filter
Definition at line 99 of file mrpt_localization_core.h.
mrpt::bayes::CParticleFilter PFLocalizationCore::pf_ [protected] |
common interface for particle filters
Definition at line 96 of file mrpt_localization_core.h.
mrpt::bayes::CParticleFilter::TParticleFilterStats PFLocalizationCore::pf_stats_ [protected] |
filter statistics
Definition at line 98 of file mrpt_localization_core.h.
PFStates PFLocalizationCore::state_ [protected] |
updates
filter states to perform things like init on the
Definition at line 107 of file mrpt_localization_core.h.
mrpt::utils::CTicTac PFLocalizationCore::tictac_ [protected] |
timer to measure performance
Definition at line 104 of file mrpt_localization_core.h.
mrpt::system::TTimeStamp PFLocalizationCore::time_last_update_ [protected] |
time of the last update
Definition at line 103 of file mrpt_localization_core.h.
size_t PFLocalizationCore::update_counter_ [protected] |
internal counter to count the number of filter
Definition at line 105 of file mrpt_localization_core.h.
bool PFLocalizationCore::use_motion_model_default_options_ [protected] |
used default odom_params
Definition at line 89 of file mrpt_localization_core.h.