Class CPose3DPDFGaussianInf

Defined in File CPose3DPDFGaussianInf.h

Inheritance Relationships

Base Type

public mrpt::poses::CPose3DPDF (Class CPose3DPDF)

Class Documentation

class CPose3DPDFGaussianInf : public mrpt::poses::CPose3DPDF 

Declares a class that represents a Probability Density function (PDF) of a 3D pose \( p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t \) as a Gaussian described by its mean and its inverse covariance matrix.

This class implements that PDF using a mono-modal Gaussian distribution in “information” form (inverse covariance matrix).

Uncertainty of pose composition operations ( \( y = x \oplus u \)) is implemented in the method “CPose3DPDFGaussianInf::operator+=”.

See also

getMean, getCovarianceAndMean

virtual void copyFrom(const CPose3DPDF &o) override: Copy operator, translating if necessary (for example, between particles and gaussian representations)

void copyFrom(const CPosePDF &o): Copy operator, translating if necessary (for example, between particles and gaussian representations)

void copyFrom(const CPose3DQuatPDFGaussian &o): Copy from a 6D pose PDF described as a Quaternion

bool saveToTextFile(const std::string &file) const override: Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines.

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(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,phi) datum.

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

Bayesian fusion of two points gauss. distributions, then save the result in this object. The process is as follows:

(x1,S1): Mean and variance of the p1 distribution.
(x2,S2): Mean and variance of the p2 distribution.
(x,S): Mean and variance of the resulting distribution.

\( S = (S_1^{-1} + S_2^{-1})^{-1} \) \( x = S ( S_1^{-1} x_1 + S_2^{-1} x_2 ) \)

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

inline CPose3DPDFGaussianInf operator-() const: Unary - operator, returns the PDF of the inverse pose.

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

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

void operator-=(const CPose3DPDFGaussianInf &Ap): Makes: thisPDF = thisPDF - Ap, where “-” is pose inverse composition (both the mean, and the covariance matrix are updated)

double evaluatePDF(const CPose3D &x) const: Evaluates the PDF at a given point

double evaluateNormalizedPDF(const CPose3D &x) const: Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]

void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const: Returns a 3x3 matrix with submatrix of the inverse covariance for the variables (x,y,yaw) only

double mahalanobisDistanceTo(const CPose3DPDFGaussianInf &theOther): Computes the Mahalanobis distance between the centers of two Gaussians. The variables with a variance exactly equal to 0 are not taken into account in the process, but “infinity” is returned if the corresponding elements are not exactly equal.

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