Class CPose3DPDFSOG

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class CPose3DPDFSOG : public mrpt::poses::CPose3DPDF

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

Public Types

using TModesList = std::vector<TGaussianMode, mrpt::aligned_allocator_cpp11<TGaussianMode>>
using const_iterator = TModesList::const_iterator
using iterator = TModesList::iterator

Public Functions

CPose3DPDFSOG(size_t nModes = 1)

Default constructor

Parameters:

nModes – The initial size of CPose3DPDFSOG::m_modes

void clear()

Clear all the gaussian modes

void resize(size_t N)

Set 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.

inline iterator begin()
inline iterator end()
inline const_iterator begin() const
inline const_iterator end() const
void getMean(CPose3D &mean_pose) const override
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(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) override

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

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

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

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 override

Draws a single sample from the distribution

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

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 override

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

virtual void printTo(std::ostream &out) const override

Write a human-readable description of this PDF to the given stream. Derived classes must override this method.

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

TModesList m_modes

Access directly to this array for modify the modes as desired. Note that no weight can be zero!! We must use pointers to satisfy the mem-alignment of the matrixes

struct TGaussianMode

The struct for each mode:

Public Functions

inline TGaussianMode()

Public Members

CPose3DPDFGaussian val
double log_w = {0}

The log-weight