Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
This class is used internally in mrpt::slam::CHMTSLAM
Definition at line 562 of file CHMTSLAM.h.
#include <mrpt/hmtslam/CHMTSLAM.h>
Classes | |
struct | TPathBin |
Auxiliary structure. More... | |
Public Member Functions | |
CLSLAM_RBPF_2DLASER (CHMTSLAM *parent) | |
Constructor. | |
virtual | ~CLSLAM_RBPF_2DLASER () |
Destructor. | |
void | processOneLMH (CLocalMetricHypothesis *LMH, const CActionCollectionPtr &act, const CSensoryFramePtr &sf) |
Main entry point from HMT-SLAM: process some actions & observations. | |
void | prediction_and_update_pfAuxiliaryPFOptimal (CLocalMetricHypothesis *LMH, const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
void | prediction_and_update_pfOptimalProposal (CLocalMetricHypothesis *LMH, const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
Protected Member Functions | |
void | loadTPathBinFromPath (TPathBin &outBin, std::map< TPoseID, CPose3D > *path=NULL, CPose2D *newPose=NULL) |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options". | |
int | findTPathBinIntoSet (TPathBin &desiredBin, std::deque< TPathBin > &theSet) |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found. | |
Static Protected Member Functions | |
static double | particlesEvaluator_AuxPFOptimal (const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation) |
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal". | |
static double | auxiliarComputeObservationLikelihood (const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t particleIndexForMap, const CSensoryFrame *observation, const CPose2D *x) |
Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options". | |
Protected Attributes | |
bool | m_insertNewRobotPose |
For use within PF callback methods. | |
Friends | |
class HMTSLAM_IMPEXP | CLocalMetricHypothesis |
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER | ( | CHMTSLAM * | parent | ) |
Constructor.
virtual mrpt::hmtslam::CLSLAM_RBPF_2DLASER::~CLSLAM_RBPF_2DLASER | ( | ) | [virtual] |
Destructor.
static double mrpt::hmtslam::CLSLAM_RBPF_2DLASER::auxiliarComputeObservationLikelihood | ( | const bayes::CParticleFilter::TParticleFilterOptions & | PF_options, |
const CParticleFilterCapable * | obj, | ||
size_t | particleIndexForMap, | ||
const CSensoryFrame * | observation, | ||
const CPose2D * | x | ||
) | [static, protected] |
Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".
int mrpt::hmtslam::CLSLAM_RBPF_2DLASER::findTPathBinIntoSet | ( | TPathBin & | desiredBin, |
std::deque< TPathBin > & | theSet | ||
) | [protected] |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found.
void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::loadTPathBinFromPath | ( | TPathBin & | outBin, |
std::map< TPoseID, CPose3D > * | path = NULL , |
||
CPose2D * | newPose = NULL |
||
) | [protected] |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
static double mrpt::hmtslam::CLSLAM_RBPF_2DLASER::particlesEvaluator_AuxPFOptimal | ( | const bayes::CParticleFilter::TParticleFilterOptions & | PF_options, |
const CParticleFilterCapable * | obj, | ||
size_t | index, | ||
const void * | action, | ||
const void * | observation | ||
) | [static, protected] |
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal | ( | CLocalMetricHypothesis * | LMH, |
const mrpt::slam::CActionCollection * | action, | ||
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [virtual] |
The PF algorithm implementation.
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal | ( | CLocalMetricHypothesis * | LMH, |
const mrpt::slam::CActionCollection * | action, | ||
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [virtual] |
The PF algorithm implementation.
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH | ( | CLocalMetricHypothesis * | LMH, |
const CActionCollectionPtr & | act, | ||
const CSensoryFramePtr & | sf | ||
) | [virtual] |
Main entry point from HMT-SLAM: process some actions & observations.
The passed action/observation will be deleted, so a copy must be made if necessary. This method must be in charge of updating the robot pose estimates and also to update the map when required.
LMH | The local metric hypothesis which must be updated by this SLAM algorithm. |
act | The action to process (or NULL). |
sf | The observations to process (or NULL). |
Implements mrpt::hmtslam::CLSLAMAlgorithmBase.
friend class HMTSLAM_IMPEXP CLocalMetricHypothesis [friend] |
Reimplemented from mrpt::hmtslam::CLSLAMAlgorithmBase.
Definition at line 564 of file CHMTSLAM.h.
bool mrpt::hmtslam::CLSLAM_RBPF_2DLASER::m_insertNewRobotPose [protected] |
For use within PF callback methods.
Definition at line 604 of file CHMTSLAM.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |