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

#include <mrpt/hmtslam/CHMTSLAM.h>

Inheritance diagram for mrpt::hmtslam::CLSLAM_RBPF_2DLASER:

Classes

struct  TPathBin
 Auxiliary structure. More...
 

Public Member Functions

 CLSLAM_RBPF_2DLASER (CHMTSLAM *parent)
 Constructor. More...
 
 ~CLSLAM_RBPF_2DLASER () override
 Destructor. More...
 
void processOneLMH (CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf) override
 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) override
 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) override
 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 37 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

◆ ~CLSLAM_RBPF_2DLASER()

CLSLAM_RBPF_2DLASER::~CLSLAM_RBPF_2DLASER ( )
overridedefault

Destructor.

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 664 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::maps::CMetricMap::computeObservationsLikelihood(), and MRPT_UNUSED_PARAM.

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

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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 755 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

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

Definition at line 700 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent, mrpt::poses::CPose2D::phi(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::phi, mrpt::round(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y.

Here is the call graph for this function:

◆ 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

Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".

Definition at line 571 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References ASSERT_, auxiliarComputeObservationLikelihood(), mrpt::math::maximum(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples.

Referenced by prediction_and_update_pfAuxiliaryPFOptimal().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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 
)
overridevirtual

The PF algorithm implementation.

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

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Definition at line 222 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(), 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::math::CVectorDynamic< T >::resize(), mrpt::system::CTicTac::Tac(), THROW_EXCEPTION, and mrpt::system::CTicTac::Tic().

Here is the call graph for this function:

◆ 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 
)
overridevirtual

The PF algorithm implementation.

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

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Definition at line 776 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::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::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::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, and THROW_EXCEPTION.

Here is the call graph for this function:

◆ processOneLMH()

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

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 nullptr).
sfThe observations to process (or nullptr).

Implements mrpt::hmtslam::CLSLAMAlgorithmBase.

Definition at line 58 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References ASSERT_, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::distanceTo(), mrpt::bayes::CParticleFilter::executeOn(), mrpt::hmtslam::CHMTSLAM::generatePoseID(), mrpt::hmtslam::CLocalMetricHypothesis::getMeans(), mrpt::system::LVL_INFO, mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_ID, m_insertNewRobotPose, mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships, mrpt::bayes::CParticleFilter::m_options, mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner, mrpt::hmtslam::CLocalMetricHypothesis::m_SFs, MRPT_END, MRPT_START, NODE_ANNOTATION_REF_POSEID, POSEID_INVALID, mrpt::math::wrapToPi(), and mrpt::poses::CPose3D::yaw().

Here is the call graph for this function:

Friends And Related Function Documentation

◆ CLocalMetricHypothesis

friend class CLocalMetricHypothesis
friend

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

Referenced by processOneLMH().

◆ m_parent

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

Definition at line 521 of file CHMTSLAM.h.

Referenced by loadTPathBinFromPath(), and processOneLMH().




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019