Class CPosePDFSOG

Defined in File CPosePDFSOG.h

Nested Relationships

Nested Types

Struct CPosePDFSOG::TGaussianMode

Inheritance Relationships

Base Type

public mrpt::poses::CPosePDF (Class CPosePDF)

Class Documentation

class CPosePDFSOG : public mrpt::poses::CPosePDF 

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

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

See also

getMean

void normalizeWeights(): Normalize the weights in m_modes such as the maximum log-weight is 0

virtual void copyFrom(const CPosePDF &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)
phi_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 rotateAllCovariances(double ang): Rotate all the covariance matrixes by replacing them by \( \mathbf{R}~\mathbf{COV}~\mathbf{R}^t \), where \( \mathbf{R} = \left[ \begin{array}{ccc} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \)

void drawSingleSample(CPose2D &outPart) const override: Draws a single sample from the distribution

virtual 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 1x3 vectors, where each row contains a (x,y,phi) datum.

virtual void inverse(CPosePDF &o) const override: Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF

void operator+=(const mrpt::poses::CPose2D &Ap): Makes: thisPDF = thisPDF + Ap, where “+” is pose composition (both the mean, and the covariance matrix are updated).

double evaluatePDF(const mrpt::poses::CPose2D &x, bool sumOverAllPhis = false) const: Evaluates the PDF at a given point.

double evaluateNormalizedPDF(const mrpt::poses::CPose2D &x) const: Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0,1].

void evaluatePDFInArea(double x_min, double x_max, double y_min, double y_max, double resolutionXY, double phi, mrpt::math::CMatrixDouble &outMatrix, bool sumOverAllPhis = false): Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matrix (each row contains values for a fixed y-coordinate value).

virtual void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop = 0) override: Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPosePDFSOG object and p2 a mrpt::poses::CPosePDFGaussian object)

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(): Ensures 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

CPose2D mean

mrpt::math::CMatrixDouble33 cov

double log_w = {0}: The log-weight

Friends

inline friend std::ostream &operator<<(std::ostream &o, const TGaussianMode &mode)