Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
This class also implements particle filtering for robot localization. See the MRPT application "app/pf-localization" for an example of usage.
Definition at line 55 of file CMonteCarloLocalization2D.h.
#include <mrpt/slam/CMonteCarloLocalization2D.h>
Public Member Functions | |
CMonteCarloLocalization2D (size_t M=1) | |
Constructor. | |
virtual | ~CMonteCarloLocalization2D () |
Destructor. | |
void | resetUniformFreeSpace (COccupancyGridMap2D *theMap, const double freeCellsThreshold=0.7, const int particlesCount=-1, const double x_min=-1e10f, const double x_max=1e10f, const double y_min=-1e10f, const double y_max=1e10f, const double phi_min=-M_PI, const double phi_max=M_PI) |
Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map. | |
void | prediction_and_update_pfStandardProposal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
Update the m_particles, predicting the posterior of robot pose and map after a movement command. | |
void | prediction_and_update_pfAuxiliaryPFStandard (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
Update the m_particles, predicting the posterior of robot pose and map after a movement command. | |
void | prediction_and_update_pfAuxiliaryPFOptimal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
Update the m_particles, predicting the posterior of robot pose and map after a movement command. | |
Virtual methods that the PF_implementations assume exist. | |
const TPose3D * | getLastPose (const size_t i) const |
Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). | |
void | PF_SLAM_implementation_custom_update_particle_with_new_pose (CParticleDataContent *particleData, const TPose3D &newPose) const |
void | PF_SLAM_implementation_replaceByNewParticleSet (CParticleList &old_particles, const std::vector< TPose3D > &newParticles, const vector_double &newParticlesWeight, const std::vector< size_t > &newParticlesDerivedFromIdx) const |
double | PF_SLAM_computeObservationLikelihoodForParticle (const CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const CSensoryFrame &observation, const CPose3D &x) const |
Evaluate the observation likelihood for one particle at a given location. | |
Public Attributes | |
TMonteCarloLocalizationParams | options |
MCL parameters. |
mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D | ( | size_t | M = 1 | ) |
Constructor.
M | The number of m_particles. |
virtual mrpt::slam::CMonteCarloLocalization2D::~CMonteCarloLocalization2D | ( | ) | [virtual] |
Destructor.
const TPose3D* mrpt::slam::CMonteCarloLocalization2D::getLastPose | ( | const size_t | i | ) | const [virtual] |
Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).
Implements mrpt::slam::PF_implementation< CPose2D, CMonteCarloLocalization2D >.
double mrpt::slam::CMonteCarloLocalization2D::PF_SLAM_computeObservationLikelihoodForParticle | ( | const CParticleFilter::TParticleFilterOptions & | PF_options, |
const size_t | particleIndexForMap, | ||
const CSensoryFrame & | observation, | ||
const CPose3D & | x | ||
) | const [virtual] |
Evaluate the observation likelihood for one particle at a given location.
Implements mrpt::slam::PF_implementation< CPose2D, CMonteCarloLocalization2D >.
void mrpt::slam::CMonteCarloLocalization2D::PF_SLAM_implementation_custom_update_particle_with_new_pose | ( | CParticleDataContent * | particleData, |
const TPose3D & | newPose | ||
) | const [virtual] |
void mrpt::slam::CMonteCarloLocalization2D::PF_SLAM_implementation_replaceByNewParticleSet | ( | CParticleList & | old_particles, |
const std::vector< TPose3D > & | newParticles, | ||
const vector_double & | newParticlesWeight, | ||
const std::vector< size_t > & | newParticlesDerivedFromIdx | ||
) | const |
void mrpt::slam::CMonteCarloLocalization2D::prediction_and_update_pfAuxiliaryPFOptimal | ( | const mrpt::slam::CActionCollection * | action, |
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [virtual] |
Update the m_particles, predicting the posterior of robot pose and map after a movement command.
This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed CSensoryFrame:
Action | This is a pointer to CActionCollection, containing the pose change the robot has been commanded. |
observation | This must be a pointer to a CSensoryFrame object, with robot sensed observations. |
Reimplemented from mrpt::bayes::CParticleFilterCapable.
void mrpt::slam::CMonteCarloLocalization2D::prediction_and_update_pfAuxiliaryPFStandard | ( | const mrpt::slam::CActionCollection * | action, |
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [virtual] |
Update the m_particles, predicting the posterior of robot pose and map after a movement command.
This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed CSensoryFrame:
Action | This is a pointer to CActionCollection, containing the pose change the robot has been commanded. |
observation | This must be a pointer to a CSensoryFrame object, with robot sensed observations. |
Reimplemented from mrpt::bayes::CParticleFilterCapable.
void mrpt::slam::CMonteCarloLocalization2D::prediction_and_update_pfStandardProposal | ( | const mrpt::slam::CActionCollection * | action, |
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [virtual] |
Update the m_particles, predicting the posterior of robot pose and map after a movement command.
This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed CSensoryFrame:
action | This is a pointer to CActionCollection, containing the pose change the robot has been commanded. |
observation | This must be a pointer to a CSensoryFrame object, with robot sensed observations. |
Reimplemented from mrpt::bayes::CParticleFilterCapable.
void mrpt::slam::CMonteCarloLocalization2D::resetUniformFreeSpace | ( | COccupancyGridMap2D * | theMap, |
const double | freeCellsThreshold = 0.7 , |
||
const int | particlesCount = -1 , |
||
const double | x_min = -1e10f , |
||
const double | x_max = 1e10f , |
||
const double | y_min = -1e10f , |
||
const double | y_max = 1e10f , |
||
const double | phi_min = -M_PI , |
||
const double | phi_max = M_PI |
||
) |
Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map.
Orientation is randomly generated in the whole 2*PI range.
theMap | The occupancy grid map |
freeCellsThreshold | The minimum free-probability to consider a cell as empty (default is 0.7) |
particlesCount | If set to -1 the number of m_particles remains unchanged. |
x_min | The limits of the area to look for free cells. |
x_max | The limits of the area to look for free cells. |
y_min | The limits of the area to look for free cells. |
y_max | The limits of the area to look for free cells. |
phi_min | The limits of the area to look for free cells. |
phi_max | The limits of the area to look for free cells. |
std::exception | On any error (no free cell found in map, map=NULL, etc...) |
MCL parameters.
Definition at line 62 of file CMonteCarloLocalization2D.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |