Class CPose3DPDFGaussian

Inheritance Relationships

Base Type

Class Documentation

class CPose3DPDFGaussian : 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 \).

This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details.

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

See also

CPose3D, CPose3DPDF, CPose3DPDFParticles

Note

Read also: “A tutorial on SE(3) transformation parameterizations and

on-manifold optimization”, in blanco_se3_tutorial

Public Functions

CPose3DPDFGaussian()

Default constructor

explicit CPose3DPDFGaussian(const CPose3D &init_Mean)

Constructor

CPose3DPDFGaussian(TConstructorFlags_Poses constructor_dummy_param)

Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument

CPose3DPDFGaussian(const CPose3D &init_Mean, const mrpt::math::CMatrixDouble66 &init_Cov)

Constructor

explicit CPose3DPDFGaussian(const CPosePDFGaussian &o)

Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll).

explicit CPose3DPDFGaussian(const CPose3DQuatPDFGaussian &o)

Constructor from a 6D pose PDF described as a Quaternion

inline const CPose3D &getPoseMean() const
inline CPose3D &getPoseMean()
inline void getMean(CPose3D &mean_pose) const override

Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).

See also

getCovariance

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

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

See also

getMean

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.

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 CPose3DPDFGaussian 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 CPose3DPDFGaussian &Ap)

Makes: thisPDF = thisPDF + Ap, where “+” is pose composition (both the mean, and the covariance matrix are updated).

void operator-=(const CPose3DPDFGaussian &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].

double mahalanobisDistanceTo(const CPose3DPDFGaussian &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.

void getCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const

Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only.

Public Members

CPose3D mean

The mean value

mrpt::math::CMatrixDouble66 cov

The 6x6 covariance matrix

Protected Functions

void enforceCovSymmetry()

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