Main MRPT website > C++ reference for MRPT 1.9.9
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 567 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. More...
 
virtual ~CLSLAM_RBPF_2DLASER ()
 Destructor. More...
 
void processOneLMH (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf)
 Main entry point from HMT-SLAM: process some actions & observations. More...
 
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. More...
 
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. More...
 

Protected Member Functions

void loadTPathBinFromPath (TPathBin &outBin, TMapPoseID2Pose3D *path=nullptr, mrpt::poses::CPose2D *newPose=nullptr)
 Fills out a "TPathBin" variable, given a path hypotesis and (if not set to nullptr) a new pose appended at the end, using the KLD params in "options". More...
 
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. More...
 

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". More...
 
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". More...
 

Protected Attributes

bool m_insertNewRobotPose
 For use within PF callback methods. More...
 
mrpt::safe_ptr< CHMTSLAMm_parent
 

Friends

class CLocalMetricHypothesis
 

Constructor & Destructor Documentation

◆ CLSLAM_RBPF_2DLASER()

CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER ( CHMTSLAM parent)

Constructor.

Definition at line 38 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

◆ ~CLSLAM_RBPF_2DLASER()

CLSLAM_RBPF_2DLASER::~CLSLAM_RBPF_2DLASER ( )
virtual

Destructor.

Definition at line 43 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

Member Function Documentation

◆ auxiliarComputeObservationLikelihood()

double 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".

Definition at line 672 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::maps::CMetricMap::computeObservationsLikelihood(), mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, and MRPT_UNUSED_PARAM.

Referenced by particlesEvaluator_AuxPFOptimal(), prediction_and_update_pfAuxiliaryPFOptimal(), and prediction_and_update_pfOptimalProposal().

◆ findTPathBinIntoSet()

int CLSLAM_RBPF_2DLASER::findTPathBinIntoSet ( CLSLAM_RBPF_2DLASER::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.

Definition at line 764 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::phi, mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x, and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y.

◆ loadTPathBinFromPath()

void CLSLAM_RBPF_2DLASER::loadTPathBinFromPath ( CLSLAM_RBPF_2DLASER::TPathBin outBin,
TMapPoseID2Pose3D path = nullptr,
mrpt::poses::CPose2D newPose = nullptr 
)
protected

◆ particlesEvaluator_AuxPFOptimal()

double 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

◆ prediction_and_update_pfAuxiliaryPFOptimal()

void 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.

The PF algorithm implementation for "optimal sampling for non-parametric observation models".

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Definition at line 229 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::poses::CPose2D::composeFrom(), mrpt::obs::CActionRobotMovement2D::computeFromOdometry(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::ESS(), mrpt::bayes::CParticleFilterCapable::fastDrawSample(), mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::gaussianModel, mrpt::obs::CActionCollection::getBestMovementEstimation(), mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose(), mrpt::random::getRandomGenerator(), mrpt::keep_max(), mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement, mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history, mrpt::hmtslam::CLocalMetricHypothesis::m_maxLikelihood, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawMaximumLikelihood, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDraws, mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawsIdx, mrpt::hmtslam::CLocalMetricHypothesis::m_parent, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_pfAuxiliaryPFOptimal_estimatedProb, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::math::maximum(), mrpt::math::mean(), min, mrpt::math::minimum(), mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdPHI, mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel::minStdXY, mrpt::obs::CActionRobotMovement2D::motionModelConfiguration, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::normalizeWeights(), particlesEvaluator_AuxPFOptimal(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::bayes::CParticleFilterCapable::prepareFastDrawSample(), mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSamples(), mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading, mrpt::system::CTicTac::Tac(), THROW_EXCEPTION, and mrpt::system::CTicTac::Tic().

◆ prediction_and_update_pfOptimalProposal()

void 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.

The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method)

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Definition at line 785 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, mrpt::slam::CICP::TConfigParams::ALFA, mrpt::slam::CMetricMapsAlignmentAlgorithm::Align(), ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose3D::composeFrom(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::DEG2RAD(), mrpt::slam::CICP::TConfigParams::doRANSAC, mrpt::poses::CPose3DPDFGaussian::drawSingleSample(), mrpt::obs::CActionCollection::getBestMovementEstimation(), mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose(), mrpt::maps::CPointsMap::insertionOptions, mrpt::obs::CSensoryFrame::insertObservationsInto(), mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement, mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w, mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints, mrpt::obs::CActionRobotMovement2D::motionModelConfiguration, MRPT_END, MRPT_START, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::normalizeWeights(), mrpt::slam::CICP::options, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::obs::CActionRobotMovement2D::rawOdometryIncrementReading, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::square(), mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, THROW_EXCEPTION, and mrpt::poses::CPose3D::yaw().

◆ processOneLMH()

void CLSLAM_RBPF_2DLASER::processOneLMH ( CLocalMetricHypothesis LMH,
const mrpt::obs::CActionCollection::Ptr act,
const mrpt::obs::CSensoryFrame::Ptr sf 
)
virtual

Friends And Related Function Documentation

◆ CLocalMetricHypothesis

friend class CLocalMetricHypothesis
friend

Definition at line 569 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 611 of file CHMTSLAM.h.

Referenced by processOneLMH().

◆ m_parent

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

Definition at line 522 of file CHMTSLAM.h.

Referenced by loadTPathBinFromPath(), and processOneLMH().




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019