Detailed Description

An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose probability density function (pdf).

This class keeps an internal state which speeds up the sequential generation of samples. It can manage any kind of pose PDF.

Use with CPoseRandomSampler::setPosePDF, then CPoseRandomSampler::drawSample to draw values.

Notice that you can pass a 2D or 3D pose PDF, then ask for a 2D or 3D sample. This class always returns the kind of sample you ask it for, but will skip missing terms or fill out with zeroes as required. Specifically, when sampling 3D poses from a 2D pose pdf, this class will be smart enough to draw only the 3 required dimensions, avoiding a waste of time with the other 3 missing components.

See also: CPosePDF, CPose3DPDF

Definition at line 42 of file CPoseRandomSampler.h.

#include <mrpt/poses/CPoseRandomSampler.h>

Public Member Functions
	CPoseRandomSampler ()
	Default constructor. More...

	CPoseRandomSampler (const CPoseRandomSampler &o)

CPoseRandomSampler &	operator= (const CPoseRandomSampler &o)

	CPoseRandomSampler (CPoseRandomSampler &&o)

CPoseRandomSampler &	operator= (CPoseRandomSampler &&o)

void	setPosePDF (const CPosePDF &pdf)
	This method must be called to select the PDF from which to draw samples. More...

void	setPosePDF (const CPose3DPDF &pdf)
	This method must be called to select the PDF from which to draw samples. More...

CPose2D &	drawSample (CPose2D &p) const
	Generate a new sample from the selected PDF. More...

CPose3D &	drawSample (CPose3D &p) const
	Generate a new sample from the selected PDF. More...

bool	isPrepared () const
	Return true if samples can be generated, which only requires a previous call to setPosePDF. More...

CPose2D &	getSamplingMean2D (CPose2D &out_mean) const
	If the object has been loaded with setPosePDF this method returns the 2D pose mean samples will be drawn around. More...

CPose3D &	getSamplingMean3D (CPose3D &out_mean) const
	If the object has been loaded with setPosePDF this method returns the 3D pose mean samples will be drawn around. More...

void	getOriginalPDFCov2D (mrpt::math::CMatrixDouble33 &cov3x3) const
	Retrieves the 3x3 covariance of the original PDF in $[ x ~ y ~ \phi ]$ . More...

void	getOriginalPDFCov2D (mrpt::math::CMatrixDouble &cov3x3) const
	Retrieves the 3x3 covariance of the original PDF in $[ x ~ y ~ \phi ]$ . More...

void	getOriginalPDFCov3D (mrpt::math::CMatrixDouble66 &cov6x6) const
	Retrieves the 6x6 covariance of the original PDF in . More...

void	getOriginalPDFCov3D (mrpt::math::CMatrixDouble &cov6x6) const
	Retrieves the 6x6 covariance of the original PDF in . More...

Protected Member Functions
void	clear ()
	Clear internal pdf. More...

void	do_sample_2D (CPose2D &p) const
	Used internally: sample from m_pdf2D. More...

void	do_sample_3D (CPose3D &p) const
	Used internally: sample from m_pdf3D. More...

Protected Attributes
CPosePDF::Ptr	m_pdf2D
	A local copy of the PDF. More...

CPose3DPDF::Ptr	m_pdf3D
	A local copy of the PDF. More...

mrpt::math::CMatrixDouble33	m_fastdraw_gauss_Z3

mrpt::math::CMatrixDouble66	m_fastdraw_gauss_Z6

CPose2D	m_fastdraw_gauss_M_2D

CPose3D	m_fastdraw_gauss_M_3D

Constructor & Destructor Documentation

◆ CPoseRandomSampler() [1/3]

CPoseRandomSampler::CPoseRandomSampler ( )

default

Default constructor.

◆ CPoseRandomSampler() [2/3]

CPoseRandomSampler::CPoseRandomSampler ( const CPoseRandomSampler & o )

Definition at line 33 of file CPoseRandomSampler.cpp.

◆ CPoseRandomSampler() [3/3]

CPoseRandomSampler::CPoseRandomSampler ( CPoseRandomSampler && o )

Definition at line 52 of file CPoseRandomSampler.cpp.

References m_fastdraw_gauss_M_2D, m_fastdraw_gauss_M_3D, m_fastdraw_gauss_Z3, m_fastdraw_gauss_Z6, m_pdf2D, and m_pdf3D.

Member Function Documentation

◆ clear()

void CPoseRandomSampler::clear ( )

protected

Clear internal pdf.

Definition at line 94 of file CPoseRandomSampler.cpp.

References m_pdf2D, and m_pdf3D.

Referenced by operator=(), and setPosePDF().

Here is the caller graph for this function:

◆ do_sample_2D()

void CPoseRandomSampler::do_sample_2D ( CPose2D & p ) const

protected

Used internally: sample from m_pdf2D.

Definition at line 235 of file CPoseRandomSampler.cpp.

References ASSERT_, mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::poses::CPosePDFParticles::drawSingleSample(), mrpt::random::getRandomGenerator(), IS_CLASS, m_fastdraw_gauss_M_2D, m_fastdraw_gauss_Z3, m_pdf2D, MRPT_END, MRPT_START, mrpt::poses::CPose2D::normalizePhi(), mrpt::poses::CPose2D::phi(), mrpt::math::MatrixVectorBase< Scalar, Derived >::setZero(), THROW_EXCEPTION, THROW_EXCEPTION_FMT, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().

Referenced by drawSample().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ do_sample_3D()

void CPoseRandomSampler::do_sample_3D ( CPose3D & p ) const

protected

Used internally: sample from m_pdf3D.

Definition at line 285 of file CPoseRandomSampler.cpp.

References ASSERT_, mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::poses::CPose3DPDFParticles::drawSingleSample(), mrpt::random::getRandomGenerator(), IS_CLASS, m_fastdraw_gauss_M_3D, m_fastdraw_gauss_Z6, m_pdf3D, MRPT_END, MRPT_START, mrpt::poses::CPose3D::pitch(), mrpt::poses::CPose3D::roll(), mrpt::poses::CPose3D::setFromValues(), mrpt::math::MatrixVectorBase< Scalar, Derived >::setZero(), THROW_EXCEPTION, THROW_EXCEPTION_FMT, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y(), and mrpt::poses::CPose3D::yaw().

Referenced by drawSample().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ drawSample() [1/2]

CPose2D & CPoseRandomSampler::drawSample ( CPose2D & p ) const

Generate a new sample from the selected PDF.

Returns: A reference to the same object passed as argument.

See also: setPosePDF

Definition at line 185 of file CPoseRandomSampler.cpp.

References do_sample_2D(), do_sample_3D(), m_pdf2D, m_pdf3D, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), THROW_EXCEPTION, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y(), and mrpt::poses::CPose3D::yaw().

Referenced by mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ drawSample() [2/2]

CPose3D & CPoseRandomSampler::drawSample ( CPose3D & p ) const

Generate a new sample from the selected PDF.

Returns: A reference to the same object passed as argument.

See also: setPosePDF

Definition at line 211 of file CPoseRandomSampler.cpp.

References do_sample_2D(), do_sample_3D(), m_pdf2D, m_pdf3D, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), mrpt::poses::CPose3D::setFromValues(), THROW_EXCEPTION, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().

Here is the call graph for this function:

◆ getOriginalPDFCov2D() [1/2]

void CPoseRandomSampler::getOriginalPDFCov2D ( mrpt::math::CMatrixDouble33 & cov3x3 ) const

Retrieves the 3x3 covariance of the original PDF in $[ x ~ y ~ \phi ]$ .

Definition at line 342 of file CPoseRandomSampler.cpp.

References ASSERT_, mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::poses::CPosePDFGaussian::cov, isPrepared(), m_pdf2D, m_pdf3D, MRPT_END, and MRPT_START.

Referenced by getOriginalPDFCov2D(), and mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getOriginalPDFCov2D() [2/2]

void CPoseRandomSampler::getOriginalPDFCov2D ( mrpt::math::CMatrixDouble & cov3x3 ) const

Retrieves the 3x3 covariance of the original PDF in $[ x ~ y ~ \phi ]$ .

Definition at line 420 of file CPoseRandomSampler.cpp.

References getOriginalPDFCov2D().

Here is the call graph for this function:

◆ getOriginalPDFCov3D() [1/2]

void CPoseRandomSampler::getOriginalPDFCov3D ( mrpt::math::CMatrixDouble66 & cov6x6 ) const

Retrieves the 6x6 covariance of the original PDF in $ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] $ .

Definition at line 366 of file CPoseRandomSampler.cpp.

References ASSERT_, mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DPDFGaussian::cov, isPrepared(), m_pdf2D, m_pdf3D, MRPT_END, and MRPT_START.

Referenced by getOriginalPDFCov3D().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getOriginalPDFCov3D() [2/2]

void CPoseRandomSampler::getOriginalPDFCov3D ( mrpt::math::CMatrixDouble & cov6x6 ) const

Retrieves the 6x6 covariance of the original PDF in $ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] $ .

Definition at line 428 of file CPoseRandomSampler.cpp.

References getOriginalPDFCov3D().

Here is the call graph for this function:

◆ getSamplingMean2D()

CPose2D & CPoseRandomSampler::getSamplingMean2D ( CPose2D & out_mean ) const

If the object has been loaded with setPosePDF this method returns the 2D pose mean samples will be drawn around.

Returns: A reference to the argument

Definition at line 389 of file CPoseRandomSampler.cpp.

References ASSERT_, isPrepared(), m_fastdraw_gauss_M_2D, m_fastdraw_gauss_M_3D, m_pdf2D, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ getSamplingMean3D()

CPose3D & CPoseRandomSampler::getSamplingMean3D ( CPose3D & out_mean ) const

If the object has been loaded with setPosePDF this method returns the 3D pose mean samples will be drawn around.

Returns: A reference to the argument

Definition at line 406 of file CPoseRandomSampler.cpp.

References ASSERT_, isPrepared(), m_fastdraw_gauss_M_2D, m_fastdraw_gauss_M_3D, m_pdf3D, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ isPrepared()

bool CPoseRandomSampler::isPrepared ( ) const

Return true if samples can be generated, which only requires a previous call to setPosePDF.

Definition at line 338 of file CPoseRandomSampler.cpp.

References m_pdf2D, and m_pdf3D.

Referenced by getOriginalPDFCov2D(), getOriginalPDFCov3D(), getSamplingMean2D(), getSamplingMean3D(), and mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal().

Here is the caller graph for this function:

◆ operator=() [1/2]

CPoseRandomSampler & CPoseRandomSampler::operator= ( const CPoseRandomSampler & o )

Definition at line 38 of file CPoseRandomSampler.cpp.

References m_fastdraw_gauss_M_2D, m_fastdraw_gauss_M_3D, m_fastdraw_gauss_Z3, m_fastdraw_gauss_Z6, m_pdf2D, and m_pdf3D.

◆ operator=() [2/2]

CPoseRandomSampler & CPoseRandomSampler::operator= ( CPoseRandomSampler && o )

Definition at line 70 of file CPoseRandomSampler.cpp.

References clear(), m_fastdraw_gauss_M_2D, m_fastdraw_gauss_M_3D, m_fastdraw_gauss_Z3, m_fastdraw_gauss_Z6, m_pdf2D, and m_pdf3D.

Here is the call graph for this function:

◆ setPosePDF() [1/2]

void CPoseRandomSampler::setPosePDF ( const CPosePDF & pdf )

This method must be called to select the PDF from which to draw samples.

See also: drawSample

Definition at line 103 of file CPoseRandomSampler.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), clear(), mrpt::rtti::CObject::clone(), mrpt::math::cov(), mrpt::math::MatrixBase< Scalar, Derived >::eig_symmetric(), IS_CLASS, m_fastdraw_gauss_M_2D, m_fastdraw_gauss_Z3, m_pdf2D, mrpt::math::MatrixBase< Scalar, Derived >::matProductOf_AB(), MRPT_END, MRPT_START, mrpt::math::MatrixBase< Scalar, Derived >::setDiagonal(), and THROW_EXCEPTION_FMT.

Referenced by mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setPosePDF() [2/2]

void CPoseRandomSampler::setPosePDF ( const CPose3DPDF & pdf )

This method must be called to select the PDF from which to draw samples.

See also: drawSample

Definition at line 143 of file CPoseRandomSampler.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), clear(), mrpt::rtti::CObject::clone(), mrpt::math::cov(), mrpt::math::MatrixBase< Scalar, Derived >::eig_symmetric(), IS_CLASS, m_fastdraw_gauss_M_3D, m_fastdraw_gauss_Z6, m_pdf3D, mrpt::math::MatrixBase< Scalar, Derived >::matProductOf_AB(), MRPT_END, MRPT_START, mrpt::math::MatrixBase< Scalar, Derived >::setDiagonal(), and THROW_EXCEPTION_FMT.

Here is the call graph for this function:

Member Data Documentation

◆ m_fastdraw_gauss_M_2D

CPose2D mrpt::poses::CPoseRandomSampler::m_fastdraw_gauss_M_2D

protected

Definition at line 53 of file CPoseRandomSampler.h.

Referenced by CPoseRandomSampler(), do_sample_2D(), getSamplingMean2D(), getSamplingMean3D(), operator=(), and setPosePDF().

◆ m_fastdraw_gauss_M_3D

CPose3D mrpt::poses::CPoseRandomSampler::m_fastdraw_gauss_M_3D

protected

Definition at line 54 of file CPoseRandomSampler.h.

Referenced by CPoseRandomSampler(), do_sample_3D(), getSamplingMean2D(), getSamplingMean3D(), operator=(), and setPosePDF().

◆ m_fastdraw_gauss_Z3

mrpt::math::CMatrixDouble33 mrpt::poses::CPoseRandomSampler::m_fastdraw_gauss_Z3

protected

Definition at line 51 of file CPoseRandomSampler.h.

Referenced by CPoseRandomSampler(), do_sample_2D(), operator=(), and setPosePDF().

◆ m_fastdraw_gauss_Z6

mrpt::math::CMatrixDouble66 mrpt::poses::CPoseRandomSampler::m_fastdraw_gauss_Z6

protected

Definition at line 52 of file CPoseRandomSampler.h.

Referenced by CPoseRandomSampler(), do_sample_3D(), operator=(), and setPosePDF().

◆ m_pdf2D

CPosePDF::Ptr mrpt::poses::CPoseRandomSampler::m_pdf2D

protected

A local copy of the PDF.

Definition at line 47 of file CPoseRandomSampler.h.

Referenced by clear(), CPoseRandomSampler(), do_sample_2D(), drawSample(), getOriginalPDFCov2D(), getOriginalPDFCov3D(), getSamplingMean2D(), isPrepared(), operator=(), and setPosePDF().

◆ m_pdf3D

CPose3DPDF::Ptr mrpt::poses::CPoseRandomSampler::m_pdf3D

protected

A local copy of the PDF.

Definition at line 49 of file CPoseRandomSampler.h.

Referenced by clear(), CPoseRandomSampler(), do_sample_3D(), drawSample(), getOriginalPDFCov2D(), getOriginalPDFCov3D(), getSamplingMean3D(), isPrepared(), operator=(), and setPosePDF().

Detailed Description

Public Member Functions

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ CPoseRandomSampler() [1/3]

◆ CPoseRandomSampler() [2/3]

◆ CPoseRandomSampler() [3/3]

Member Function Documentation

◆ clear()

◆ do_sample_2D()

◆ do_sample_3D()

◆ drawSample() [1/2]

◆ drawSample() [2/2]

◆ getOriginalPDFCov2D() [1/2]

◆ getOriginalPDFCov2D() [2/2]

◆ getOriginalPDFCov3D() [1/2]

◆ getOriginalPDFCov3D() [2/2]

◆ getSamplingMean2D()

◆ getSamplingMean3D()

◆ isPrepared()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ setPosePDF() [1/2]

◆ setPosePDF() [2/2]

Member Data Documentation

◆ m_fastdraw_gauss_M_2D

◆ m_fastdraw_gauss_M_3D

◆ m_fastdraw_gauss_Z3

◆ m_fastdraw_gauss_Z6

◆ m_pdf2D

◆ m_pdf3D