Main MRPT website > C++ reference for MRPT 1.4.0
List of all members | Classes | Public Member Functions | Protected Member Functions | Static Protected Member Functions | Protected Attributes | Friends
mrpt::hmtslam::CLSLAM_RBPF_2DLASER Class Reference

Detailed Description

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 512 of file CHMTSLAM.h.

#include <mrpt/hmtslam/CHMTSLAM.h>

Inheritance diagram for mrpt::hmtslam::CLSLAM_RBPF_2DLASER:
Inheritance graph

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 mrpt::obs::CActionCollectionPtr &act, const mrpt::obs::CSensoryFramePtr &sf)
 Main entry point from HMT-SLAM: process some actions & observations.
 
void prediction_and_update_pfAuxiliaryPFOptimal (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
 The PF algorithm implementation.
 
void prediction_and_update_pfOptimalProposal (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
 The PF algorithm implementation.
 

Protected Member Functions

void loadTPathBinFromPath (TPathBin &outBin, TMapPoseID2Pose3D *path=NULL, mrpt::poses::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 mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
 Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
 
static double auxiliarComputeObservationLikelihood (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame *observation, const mrpt::poses::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.
 
mrpt::utils::safe_ptr< CHMTSLAMm_parent
 

Friends

class CLocalMetricHypothesis
 

Constructor & Destructor Documentation

◆ CLSLAM_RBPF_2DLASER()

mrpt::hmtslam::CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER ( CHMTSLAM parent)

Constructor.

◆ ~CLSLAM_RBPF_2DLASER()

virtual mrpt::hmtslam::CLSLAM_RBPF_2DLASER::~CLSLAM_RBPF_2DLASER ( )
virtual

Destructor.

Member Function Documentation

◆ auxiliarComputeObservationLikelihood()

static double mrpt::hmtslam::CLSLAM_RBPF_2DLASER::auxiliarComputeObservationLikelihood ( const mrpt::bayes::CParticleFilter::TParticleFilterOptions PF_options,
const mrpt::bayes::CParticleFilterCapable obj,
size_t  particleIndexForMap,
const mrpt::obs::CSensoryFrame observation,
const mrpt::poses::CPose2D x 
)
staticprotected

Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".

◆ findTPathBinIntoSet()

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.

◆ loadTPathBinFromPath()

void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::loadTPathBinFromPath ( TPathBin outBin,
TMapPoseID2Pose3D path = NULL,
mrpt::poses::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".

◆ particlesEvaluator_AuxPFOptimal()

static double mrpt::hmtslam::CLSLAM_RBPF_2DLASER::particlesEvaluator_AuxPFOptimal ( const mrpt::bayes::CParticleFilter::TParticleFilterOptions PF_options,
const mrpt::bayes::CParticleFilterCapable obj,
size_t  index,
const void *  action,
const void *  observation 
)
staticprotected

Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".

◆ prediction_and_update_pfAuxiliaryPFOptimal()

void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal ( CLocalMetricHypothesis LMH,
const mrpt::obs::CActionCollection action,
const mrpt::obs::CSensoryFrame observation,
const bayes::CParticleFilter::TParticleFilterOptions PF_options 
)
virtual

The PF algorithm implementation.


Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

◆ prediction_and_update_pfOptimalProposal()

void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal ( CLocalMetricHypothesis LMH,
const mrpt::obs::CActionCollection action,
const mrpt::obs::CSensoryFrame observation,
const bayes::CParticleFilter::TParticleFilterOptions PF_options 
)
virtual

The PF algorithm implementation.


Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

◆ processOneLMH()

void mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH ( CLocalMetricHypothesis LMH,
const mrpt::obs::CActionCollectionPtr act,
const mrpt::obs::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.

Parameters
LMHThe local metric hypothesis which must be updated by this SLAM algorithm.
actThe action to process (or NULL).
sfThe observations to process (or NULL).

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Friends And Related Function Documentation

◆ CLocalMetricHypothesis

friend class CLocalMetricHypothesis
friend

Definition at line 514 of file CHMTSLAM.h.

Member Data Documentation

◆ m_insertNewRobotPose

bool mrpt::hmtslam::CLSLAM_RBPF_2DLASER::m_insertNewRobotPose
protected

For use within PF callback methods.

Definition at line 554 of file CHMTSLAM.h.

◆ m_parent

mrpt::utils::safe_ptr<CHMTSLAM> mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent
protectedinherited

Definition at line 465 of file CHMTSLAM.h.




Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Fri Jan 20 00:13:14 UTC 2023