class mrpt::poses::CPose3DPDFSOG

Overview

Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose \(p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t\).

This class implements that PDF as the following multi-modal Gaussian distribution:

\(p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i )\)

Where the number of modes N is the size of CPose3DPDFSOG::m_modes. Angles are always in radians.

See mrpt::poses::CPose3DPDF for more details.

See also:

CPose3DPDF

#include <mrpt/poses/CPose3DPDFSOG.h>

class CPose3DPDFSOG: public mrpt::poses::CPose3DPDF
{
public:
    // typedefs

    typedef std::vector<TGaussianMode, mrpt::aligned_allocator_cpp11<TGaussianMode>> TModesList;
    typedef TModesList::const_iterator const_iterator;
    typedef TModesList::iterator iterator;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // structs

    struct TGaussianMode;

    // construction

    CPose3DPDFSOG(size_t nModes = 1);

    // methods

    void clear();
    void resize(size_t N);
    size_t size() const;
    bool empty() const;
    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;
    void getMean(CPose3D& mean_pose) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void normalizeWeights();
    void getMostLikelyMode(CPose3DPDFGaussian& outVal) const;
    virtual void copyFrom(const CPose3DPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2);
    void drawSingleSample(CPose3D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    virtual void inverse(CPose3DPDF& o) const;
    void appendFrom(const CPose3DPDFSOG& o);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

    virtual void copyFrom(const CPose3DPDF& o) = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) = 0;
    virtual void inverse(CPose3DPDF& o) const = 0;

Construction

CPose3DPDFSOG(size_t nModes = 1)

Default constructor.

Parameters:

nModes

The initial size of CPose3DPDFSOG::m_modes

Methods

void clear()

Clear all the gaussian modes.

void resize(size_t N)

Set the number of SOG modes.

size_t size() const

Return the number of Gaussian modes.

bool empty() const

Return whether there is any Gaussian mode.

virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const

Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.

See also:

getMean, getInformationMatrix

void normalizeWeights()

Normalize the weights in m_modes such as the maximum log-weight is 0.

void getMostLikelyMode(CPose3DPDFGaussian& outVal) const

Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG)

virtual void copyFrom(const CPose3DPDF& o)

Copy operator, translating if necessary (for example, between particles and gaussian representations)

virtual bool saveToTextFile(const std::string& file) const

Save the density to a text file, with the following format: There is one row per Gaussian “mode”, and each row contains 10 elements:

  • w (The linear weight)

  • x_mean (gaussian mean value)

  • y_mean (gaussian mean value)

  • x_mean (gaussian mean value)

  • yaw_mean (gaussian mean value, in radians)

  • pitch_mean (gaussian mean value, in radians)

  • roll_mean (gaussian mean value, in radians)

  • C11,C22,C33,C44,C55,C66 (Covariance elements)

  • C12,C13,C14,C15,C16 (Covariance elements)

  • C23,C24,C25,C25 (Covariance elements)

  • C34,C35,C36 (Covariance elements)

  • C45,C46 (Covariance elements)

  • C56 (Covariance elements)

virtual void changeCoordinatesReference(const CPose3D& newReferenceBase)

this = p (+) this.

This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which “to project” the current pdf. Result PDF substituted the currently stored one in the object.

virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2)

Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object and p2 a mrpt::poses::CPose3DPDFSOG object)

void drawSingleSample(CPose3D& outPart) const

Draws a single sample from the distribution.

virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const

Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.

virtual void inverse(CPose3DPDF& o) const

Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.

void appendFrom(const CPose3DPDFSOG& o)

Append the Gaussian modes from “o” to the current set of modes of “this” density.