Class CPointPDFSOG

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class CPointPDFSOG : public mrpt::poses::CPointPDF

Declares a class that represents a Probability Density function (PDF) of a 3D point \( p(\mathbf{x}) = [x ~ y ~ z ]^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 CPointPDFSOG::m_modes

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

See also

CPointPDF, CPosePDF,

Public Types

using CListGaussianModes = std::deque<TGaussianMode>
using const_iterator = std::deque<TGaussianMode>::const_iterator
using iterator = std::deque<TGaussianMode>::iterator

Public Functions

CPointPDFSOG(size_t nModes = 1)

Default constructor

Parameters:

nModes – The initial size of CPointPDFSOG::m_modes

void clear()

Clear all the gaussian modes

inline const TGaussianMode &operator[](size_t i) const

Access to individual beacons

inline TGaussianMode &operator[](size_t i)

Access to individual beacons

inline const TGaussianMode &get(size_t i) const

Access to individual beacons

inline TGaussianMode &get(size_t i)

Access to individual beacons

inline void push_back(const TGaussianMode &m)

Inserts a copy of the given mode into the SOG

inline iterator begin()
inline iterator end()
inline const_iterator begin() const
inline const_iterator end() const
inline iterator erase(iterator i)
void resize(size_t N)

Resize the number of SOG modes

inline size_t size() const

Return the number of Gaussian modes.

inline bool empty() const

Return whether there is any Gaussian mode.

void getMean(CPoint3D &mean_point) const override
virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override
void normalizeWeights()

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

void getMostLikelyMode(CPointPDFGaussian &outVal) const

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

double ESS() const

Computes the “Effective sample size” (typical measure for Particle Filters), applied to the weights of the individual Gaussian modes, as a measure of the equality of the modes (in the range [0,total # of modes]).

virtual void copyFrom(const CPointPDF &o) override

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

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

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 weight)

  • x_mean (gaussian mean value)

  • y_mean (gaussian mean value)

  • x_mean (gaussian mean value)

  • C11 (Covariance elements)

  • C22 (Covariance elements)

  • C33 (Covariance elements)

  • C12 (Covariance elements)

  • C13 (Covariance elements)

  • C23 (Covariance elements)

virtual void changeCoordinatesReference(const CPose3D &newReferenceBase) override

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.

void drawSingleSample(CPoint3D &outSample) const override

Draw a sample from the pdf.

virtual void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop = 0) override

Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)

Parameters:

  • p1 – The first distribution to fuse

  • p2 – The second distribution to fuse

  • minMahalanobisDistToDrop – If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.

void evaluatePDFInArea(float x_min, float x_max, float y_min, float y_max, float resolutionXY, float z, mrpt::math::CMatrixD &outMatrix, bool sumOverAllZs = false)

Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).

double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const

Evaluates the PDF at a given point

Protected Functions

void enforceCovSymmetry()

Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)

Protected Attributes

CListGaussianModes m_modes

The list of SOG modes

struct TGaussianMode

The struct for each mode:

Public Functions

inline TGaussianMode()

Public Members

CPointPDFGaussian val
double log_w = {0}

The log-weight