CPoseRandomSampler.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CPoseRandomSampler_H
#define CPoseRandomSampler_H

#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/math_frwds.h>
#include <memory>  // unique_ptr

namespace mrpt::poses
{
/** 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 enought to draw only
 *  the 3 required dimensions, avoiding a waste of time with the other 3 missing
 * components.
 *
 * \ingroup poses_pdf_grp
 * \sa CPosePDF, CPose3DPDF
 */
class CPoseRandomSampler
{
   protected:
    // Only ONE of these can be not-NULL at a time.
    /** A local copy of the PDF */
    std::unique_ptr<const CPosePDF> m_pdf2D;
    /** A local copy of the PDF */
    std::unique_ptr<const CPose3DPDF> m_pdf3D;

    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;

    /** Clear internal pdf */
    void clear();

    /** Used internally: sample from m_pdf2D */
    void do_sample_2D(CPose2D& p) const;
    /** Used internally: sample from m_pdf3D */
    void do_sample_3D(CPose3D& p) const;

   public:
    /** Default constructor */
    CPoseRandomSampler();

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

    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPosePDF* pdf);

    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPosePDF::Ptr& pdf);

    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPosePDF& pdf) { setPosePDF(&pdf); }
    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPose3DPDF* pdf);

    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPose3DPDF::Ptr& pdf);

    /** This method must be called to select the PDF from which to draw samples.
      * \sa drawSample
      */
    void setPosePDF(const CPose3DPDF& pdf) { setPosePDF(&pdf); }
    /** Generate a new sample from the selected PDF.
      * \return A reference to the same object passed as argument.
      * \sa setPosePDF
      */
    CPose2D& drawSample(CPose2D& p) const;

    /** Generate a new sample from the selected PDF.
      * \return A reference to the same object passed as argument.
      * \sa setPosePDF
      */
    CPose3D& drawSample(CPose3D& p) const;

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

    /** If the object has been loaded with setPosePDF this method returns the 2D
     * pose mean samples will be drawn around. \return A reference to the
     * argument */
    CPose2D& getSamplingMean2D(CPose2D& out_mean) const;

    /** If the object has been loaded with setPosePDF this method returns the 3D
     * pose mean samples will be drawn around. \return A reference to the
     * argument */
    CPose3D& getSamplingMean3D(CPose3D& out_mean) const;

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

    /** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ]
     * \f$. */
    inline void getOriginalPDFCov2D(mrpt::math::CMatrixDouble& cov3x3) const
    {
        mrpt::math::CMatrixDouble33 M;
        this->getOriginalPDFCov2D(M);
        cov3x3 = mrpt::math::CMatrixDouble(M);
    }

    /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~
     * yaw ~ pitch ~ roll ] \f$. */
    void getOriginalPDFCov3D(mrpt::math::CMatrixDouble66& cov6x6) const;

    /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~
     * yaw ~ pitch ~ roll ] \f$. */
    inline void getOriginalPDFCov3D(mrpt::math::CMatrixDouble& cov6x6) const
    {
        mrpt::math::CMatrixDouble66 M;
        this->getOriginalPDFCov3D(M);
        cov6x6 = mrpt::math::CMatrixDouble(M);
    }

};  // End of class def.
}
#endif