#include <mrpt_localization_core.h>
Public Types | |
enum | PFStates { NA, INIT, RUN } |
Public Member Functions | |
void | init () |
void | observation (mrpt::slam::CSensoryFramePtr _sf, mrpt::slam::CObservationOdometryPtr _odometry) |
PFLocalizationCore () | |
~PFLocalizationCore () | |
Protected Attributes | |
int | initialParticleCount_ |
initilial posed used in initializeFilter() | |
mrpt::poses::CPosePDFGaussian | initialPose_ |
fhe filter | |
mrpt::slam::CMultiMetricMap | metric_map_ |
used with odom value motion noise | |
mrpt::slam::CActionRobotMovement2D::TMotionModelOptions | motion_model_default_options_ |
used default odom_params | |
mrpt::slam::CActionRobotMovement2D::TMotionModelOptions | motion_model_options_ |
used if there are is not odom | |
mrpt::poses::CPose2D | odomLastObservation_ |
filter states to perform things like init on the corret time | |
mrpt::slam::CMonteCarloLocalization2D | pdf_ |
filter statists | |
mrpt::bayes::CParticleFilter | pf_ |
map | |
mrpt::bayes::CParticleFilter::TParticleFilterStats | pf_stats_ |
common interface for particle filters | |
PFStates | state_ |
internal counter to count the number of filter updates | |
mrpt::utils::CTicTac | tictac_ |
time of the last update | |
mrpt::system::TTimeStamp | timeLastUpdate_ |
number of particles for initilization | |
size_t | update_counter_ |
timer to measure performance | |
bool | use_motion_model_default_options_ |
Private Member Functions | |
void | initializeFilter () |
pose at the last oversvation | |
void | updateFilter (mrpt::slam::CActionCollectionPtr _action, mrpt::slam::CSensoryFramePtr _sf) |
Private Attributes | |
MRPT_VIRTUAL_LOG_MACROS |
Definition at line 52 of file mrpt_localization_core.h.
Definition at line 55 of file mrpt_localization_core.h.
Definition at line 45 of file mrpt_localization_core.cpp.
Definition at line 41 of file mrpt_localization_core.cpp.
void PFLocalizationCore::init | ( | ) |
initilizes the parameter with common values to acive a working filter out of the box
Reimplemented in PFLocalizationNode, and PFLocalization.
Definition at line 50 of file mrpt_localization_core.cpp.
void PFLocalizationCore::initializeFilter | ( | ) | [private] |
pose at the last oversvation
Initilizes the filter at pose PFLocalizationCore::initialPose_ with PFLocalizationCore::initialParticleCount_ it is called by the PFLocalizationCore::updateFilter if the state_ == INIT
Definition at line 61 of file mrpt_localization_core.cpp.
void PFLocalizationCore::observation | ( | mrpt::slam::CSensoryFramePtr | _sf, |
mrpt::slam::CObservationOdometryPtr | _odometry | ||
) |
preprocesses an obseration and calles the update PFLocalizationCore::updateFilter If the odom data is null the function will assume a dummy odometry distribution around the last pose
_sf | sonsor observation |
_odometry | the odom data can also be NULL |
Definition at line 85 of file mrpt_localization_core.cpp.
void PFLocalizationCore::updateFilter | ( | mrpt::slam::CActionCollectionPtr | _action, |
mrpt::slam::CSensoryFramePtr | _sf | ||
) | [private] |
Definition at line 77 of file mrpt_localization_core.cpp.
int PFLocalizationCore::initialParticleCount_ [protected] |
initilial posed used in initializeFilter()
Definition at line 79 of file mrpt_localization_core.h.
mrpt::poses::CPosePDFGaussian PFLocalizationCore::initialPose_ [protected] |
fhe filter
Definition at line 78 of file mrpt_localization_core.h.
mrpt::slam::CMultiMetricMap PFLocalizationCore::metric_map_ [protected] |
used with odom value motion noise
Definition at line 74 of file mrpt_localization_core.h.
mrpt::slam::CActionRobotMovement2D::TMotionModelOptions PFLocalizationCore::motion_model_default_options_ [protected] |
used default odom_params
Definition at line 72 of file mrpt_localization_core.h.
mrpt::slam::CActionRobotMovement2D::TMotionModelOptions PFLocalizationCore::motion_model_options_ [protected] |
used if there are is not odom
Definition at line 73 of file mrpt_localization_core.h.
Definition at line 53 of file mrpt_localization_core.h.
mrpt::poses::CPose2D PFLocalizationCore::odomLastObservation_ [protected] |
filter states to perform things like init on the corret time
Definition at line 84 of file mrpt_localization_core.h.
mrpt::slam::CMonteCarloLocalization2D PFLocalizationCore::pdf_ [protected] |
filter statists
Definition at line 77 of file mrpt_localization_core.h.
mrpt::bayes::CParticleFilter PFLocalizationCore::pf_ [protected] |
map
Definition at line 75 of file mrpt_localization_core.h.
mrpt::bayes::CParticleFilter::TParticleFilterStats PFLocalizationCore::pf_stats_ [protected] |
common interface for particle filters
Definition at line 76 of file mrpt_localization_core.h.
PFStates PFLocalizationCore::state_ [protected] |
internal counter to count the number of filter updates
Definition at line 83 of file mrpt_localization_core.h.
mrpt::utils::CTicTac PFLocalizationCore::tictac_ [protected] |
time of the last update
Definition at line 81 of file mrpt_localization_core.h.
mrpt::system::TTimeStamp PFLocalizationCore::timeLastUpdate_ [protected] |
number of particles for initilization
Definition at line 80 of file mrpt_localization_core.h.
size_t PFLocalizationCore::update_counter_ [protected] |
timer to measure performance
Definition at line 82 of file mrpt_localization_core.h.
bool PFLocalizationCore::use_motion_model_default_options_ [protected] |
Definition at line 71 of file mrpt_localization_core.h.