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:

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< CHMTSLAM >	m_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

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 709 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 >::x(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y.

◆ 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 578 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().

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

◆ 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

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

References ASSERT_, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::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, min, MRPT_END, MRPT_START, NODE_ANNOTATION_REF_POSEID, POSEID_INVALID, mrpt::math::wrapToPi(), and mrpt::poses::CPose3D::yaw().

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().

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