Detailed Description

An implementation of rejection sampling for generating 2D robot pose from range-only measurements within a landmarks (beacons) map.

Before calling the method "rejectionSampling" to generate the samples, you must call "setParams". It is assumed a planar scenario, where the robot is at a fixed height (default=0).

See also: bayes::CRejectionSamplingCapable

Definition at line 34 of file CRejectionSamplingRangeOnlyLocalization.h.

#include <mrpt/slam/CRejectionSamplingRangeOnlyLocalization.h>

Inheritance diagram for mrpt::slam::CRejectionSamplingRangeOnlyLocalization:

Classes
struct	TDataPerBeacon
	Data for each beacon observation with a correspondence with the map. More...

Public Types
using	TParticle = CProbabilityParticle< mrpt::poses::CPose2D, mrpt::bayes::particle_storage_mode::POINTER >

Public Member Functions
	CRejectionSamplingRangeOnlyLocalization ()
	Constructor. More...

	~CRejectionSamplingRangeOnlyLocalization () override=default
	Destructor. More...

bool	setParams (const mrpt::maps::CLandmarksMap &beaconsMap, const mrpt::obs::CObservationBeaconRanges &observation, float sigmaRanges, const mrpt::poses::CPose2D &oldPose, float robot_z=0, bool autoCheckAngleRanges=true)
	The parameters used in the generation of random samples: More...

void	rejectionSampling (size_t desiredSamples, std::vector< TParticle > &outSamples, size_t timeoutTrials=1000)
	Generates a set of N independent samples via rejection sampling. More...

Protected Member Functions
void	RS_drawFromProposal (mrpt::poses::CPose2D &outSample) override
	Generates one sample, drawing from some proposal distribution. More...

double	RS_observationLikelihood (const mrpt::poses::CPose2D &x) override
	Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]). More...

Protected Attributes
float	m_z_robot {0}
	Z coordinate of the robot. More...

float	m_sigmaRanges {0.10f}

mrpt::poses::CPose2D	m_oldPose

size_t	m_drawIndex {0}
	The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood) More...

std::deque< TDataPerBeacon >	m_dataPerBeacon
	Data for each beacon observation with a correspondence with the map. More...

Member Typedef Documentation

◆ TParticle

using mrpt::bayes::CRejectionSamplingCapable< mrpt::poses::CPose2D , mrpt::bayes::particle_storage_mode::POINTER >::TParticle = CProbabilityParticle<mrpt::poses::CPose2D , mrpt::bayes::particle_storage_mode::POINTER >

inherited

Definition at line 32 of file CRejectionSamplingCapable.h.

Constructor & Destructor Documentation

◆ CRejectionSamplingRangeOnlyLocalization()

CRejectionSamplingRangeOnlyLocalization::CRejectionSamplingRangeOnlyLocalization ( )

Constructor.

Definition at line 31 of file CRejectionSamplingRangeOnlyLocalization.cpp.

◆ ~CRejectionSamplingRangeOnlyLocalization()

mrpt::slam::CRejectionSamplingRangeOnlyLocalization::~CRejectionSamplingRangeOnlyLocalization ( )

overridedefault

Destructor.

Member Function Documentation

◆ rejectionSampling()

void mrpt::bayes::CRejectionSamplingCapable< mrpt::poses::CPose2D , mrpt::bayes::particle_storage_mode::POINTER >::rejectionSampling	(	size_t	desiredSamples,
		std::vector< TParticle > &	outSamples,
		size_t	timeoutTrials = `1000`
	)

inlineinherited

Generates a set of N independent samples via rejection sampling.

Parameters

desiredSamples	The number of desired samples to generate
outSamples	The output samples.
timeoutTrials	The maximum number of rejection trials for each generated sample (i.e. the maximum number of iterations). This can be used to set a limit to the time complexity of the algorithm for difficult probability densities. All will have equal importance weights (a property of rejection sampling), although those samples generated at timeout will have a different importance weights.

Definition at line 48 of file CRejectionSamplingCapable.h.

◆ RS_drawFromProposal()

void CRejectionSamplingRangeOnlyLocalization::RS_drawFromProposal ( mrpt::poses::CPose2D & outSample )

overrideprotectedvirtual

Generates one sample, drawing from some proposal distribution.

Implements mrpt::bayes::CRejectionSamplingCapable< mrpt::poses::CPose2D >.

Definition at line 41 of file CRejectionSamplingRangeOnlyLocalization.cpp.

References ASSERT_, mrpt::random::CRandomGenerator::drawGaussian1D(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::random::getRandomGenerator(), m_dataPerBeacon, m_drawIndex, m_oldPose, m_sigmaRanges, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), R, THROW_EXCEPTION, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().

Here is the call graph for this function:

◆ RS_observationLikelihood()

double CRejectionSamplingRangeOnlyLocalization::RS_observationLikelihood ( const mrpt::poses::CPose2D & x )

overrideprotectedvirtual

Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]).

Implements mrpt::bayes::CRejectionSamplingCapable< mrpt::poses::CPose2D >.

Definition at line 85 of file CRejectionSamplingRangeOnlyLocalization.cpp.

References mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::distanceTo(), m_dataPerBeacon, m_drawIndex, m_sigmaRanges, MRPT_END, MRPT_START, and mrpt::square().

Here is the call graph for this function:

◆ setParams()

bool CRejectionSamplingRangeOnlyLocalization::setParams	(	const mrpt::maps::CLandmarksMap &	beaconsMap,
		const mrpt::obs::CObservationBeaconRanges &	observation,
		float	sigmaRanges,
		const mrpt::poses::CPose2D &	oldPose,
		float	robot_z = `0`,
		bool	autoCheckAngleRanges = `true`
	)

The parameters used in the generation of random samples:

Parameters

beaconsMap	The map containing the N beacons (indexed by their "beacon ID"s). Only the mean 3D position of the beacons is used, the covariance is ignored.
observation	An observation with, at least ONE range measurement.
sigmaRanges	The standard deviation of the "range measurement noise".
robot_z	The height of the robot on the floor (default=0). Note that the beacon sensor on the robot may be at a different height, according to data within the observation object.
autoCheckAngleRanges	Whether to make a simple check for potential good angles from the beacons to generate samples (disable to speed-up the preparation vs. making slower the drawn). This method fills out the member "m_dataPerBeacon".

Returns: true if at least ONE beacon has been successfully loaded, false otherwise. In this case do not call "rejectionSampling" or an exception will be launch, since there is no information to generate samples.

Definition at line 120 of file CRejectionSamplingRangeOnlyLocalization.cpp.

References ASSERT_, mrpt::containers::CDynamicGrid< T >::cellByPos(), mrpt::opengl::internal::data, mrpt::containers::CDynamicGrid< T >::fill(), mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getByBeaconID(), mrpt::maps::CLandmarksMap::landmarks, m_dataPerBeacon, m_drawIndex, m_oldPose, M_PIf, m_sigmaRanges, m_z_robot, MRPT_END, MRPT_START, mrpt::maps::CLandmark::pose_mean, R, mrpt::obs::CObservationBeaconRanges::sensedData, mrpt::square(), mrpt::math::TPoint3D_data< T >::x, mrpt::math::TPoint3D_data< T >::y, and mrpt::math::TPoint3D_data< T >::z.