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< CHMTSLAM >	m_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 662 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::maps::CMetricMap::computeObservationsLikelihood().

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 753 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 698 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, 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.

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 774 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

LMH	The local metric hypothesis which must be updated by this SLAM algorithm.
act	The action to process (or nullptr).
sf	The 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().

Detailed Description

Classes

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Friends

Constructor & Destructor Documentation

◆ CLSLAM_RBPF_2DLASER()

◆ ~CLSLAM_RBPF_2DLASER()

Member Function Documentation

◆ auxiliarComputeObservationLikelihood()

◆ findTPathBinIntoSet()

◆ loadTPathBinFromPath()

◆ particlesEvaluator_AuxPFOptimal()

◆ prediction_and_update_pfAuxiliaryPFOptimal()

◆ prediction_and_update_pfOptimalProposal()

◆ processOneLMH()

Friends And Related Function Documentation

◆ CLocalMetricHypothesis

Member Data Documentation

◆ m_insertNewRobotPose

◆ m_parent