MRPT  2.0.1
CPose3DPDFGaussianInf.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CMatrixD.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose3DPDF.h>
14 #include <mrpt/poses/CPosePDF.h>
15 
16 namespace mrpt::poses
17 {
18 class CPosePDFGaussian;
19 class CPose3DQuatPDFGaussian;
20 
21 /** Declares a class that represents a Probability Density function (PDF) of a
22  * 3D pose \f$ p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t \f$ as a
23  * Gaussian described by its mean and its inverse covariance matrix.
24  *
25  * This class implements that PDF using a mono-modal Gaussian distribution in
26  * "information" form (inverse covariance matrix).
27  *
28  * Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is
29  * implemented in the method "CPose3DPDFGaussianInf::operator+=".
30  *
31  * For further details on implemented methods and the theory behind them,
32  * see <a
33  * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
34  * >this report</a>.
35  *
36  * \sa CPose3D, CPose3DPDF, CPose3DPDFParticles, CPose3DPDFGaussian
37  * \ingroup poses_pdf_grp
38  */
40 {
41  // This must be added to any CSerializable derived class:
44 
45  protected:
46  /** Assures the symmetry of the covariance matrix (eventually certain
47  * operations in the math-coprocessor lead to non-symmetric matrixes!)
48  */
49  void enforceCovSymmetry();
50 
51  public:
52  /** @name Data fields
53  @{ */
54 
55  /** The mean value */
57  /** The inverse of the 6x6 covariance matrix */
59 
60  /** @} */
61 
62  inline const CPose3D& getPoseMean() const { return mean; }
63  inline CPose3D& getPoseMean() { return mean; }
64  /** Default constructor - mean: all zeros, inverse covariance=all zeros ->
65  * so be careful! */
67 
68  /** Constructor with a mean value, inverse covariance=all zeros -> so be
69  * careful! */
70  explicit CPose3DPDFGaussianInf(const CPose3D& init_Mean);
71 
72  /** Uninitialized constructor: leave all fields uninitialized - Call with
73  * UNINITIALIZED_POSE as argument */
74  CPose3DPDFGaussianInf(TConstructorFlags_Poses constructor_dummy_param);
75 
76  /** Constructor with mean and inv cov. */
78  const CPose3D& init_Mean,
79  const mrpt::math::CMatrixDouble66& init_CovInv);
80 
81  /** Constructor from a 6D pose PDF described as a Quaternion */
83 
84  void getMean(CPose3D& mean_pose) const override { mean_pose = mean; }
85 
86  bool isInfType() const override { return true; }
87 
88  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override
89  {
90  return {cov_inv.inverse_LLt(), this->mean};
91  }
92 
93  /** Returns the information (inverse covariance) matrix (a STATE_LEN x
94  * STATE_LEN matrix) \sa getMean, getCovarianceAndMean */
96  {
97  inf = cov_inv;
98  }
99 
100  /** Copy operator, translating if necesary (for example, between particles
101  * and gaussian representations) */
102  void copyFrom(const CPose3DPDF& o) override;
103 
104  /** Copy operator, translating if necesary (for example, between particles
105  * and gaussian representations) */
106  void copyFrom(const CPosePDF& o);
107 
108  /** Copy from a 6D pose PDF described as a Quaternion
109  */
110  void copyFrom(const CPose3DQuatPDFGaussian& o);
111 
112  /** Save the PDF to a text file, containing the 3D pose in the first line,
113  * then the covariance matrix in next 3 lines. */
114  bool saveToTextFile(const std::string& file) const override;
115 
116  /** this = p (+) this. This can be used to convert a PDF from local
117  * coordinates to global, providing the point (newReferenceBase) from which
118  * "to project" the current pdf. Result PDF substituted the currently
119  * stored one in the object. */
120  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
121 
122  /** Draws a single sample from the distribution */
123  void drawSingleSample(CPose3D& outPart) const override;
124 
125  /** Draws a number of samples from the distribution, and saves as a list of
126  * 1x6 vectors, where each row contains a (x,y,phi) datum. */
127  void drawManySamples(
128  size_t N,
129  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
130 
131  /** Bayesian fusion of two points gauss. distributions, then save the result
132  *in this object.
133  * The process is as follows:<br>
134  * - (x1,S1): Mean and variance of the p1 distribution.
135  * - (x2,S2): Mean and variance of the p2 distribution.
136  * - (x,S): Mean and variance of the resulting distribution.
137  *
138  * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
139  * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
140  */
141  void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) override;
142 
143  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
144  void inverse(CPose3DPDF& o) const override;
145 
146  /** Unary - operator, returns the PDF of the inverse pose. */
148  {
150  this->inverse(p);
151  return p;
152  }
153 
154  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the
155  * mean, and the covariance matrix are updated) */
156  void operator+=(const CPose3D& Ap);
157  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the
158  * mean, and the covariance matrix are updated) */
159  void operator+=(const CPose3DPDFGaussianInf& Ap);
160  /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition
161  * (both the mean, and the covariance matrix are updated) */
162  void operator-=(const CPose3DPDFGaussianInf& Ap);
163  /** Evaluates the PDF at a given point */
164  double evaluatePDF(const CPose3D& x) const;
165  /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in
166  * the range [0,1] */
167  double evaluateNormalizedPDF(const CPose3D& x) const;
168  /** Returns a 3x3 matrix with submatrix of the inverse covariance for the
169  * variables (x,y,yaw) only */
170  void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble& out_cov) const;
171 
172  /** Computes the Mahalanobis distance between the centers of two Gaussians.
173  * The variables with a variance exactly equal to 0 are not taken into
174  * account in the process, but
175  * "infinity" is returned if the corresponding elements are not exactly
176  * equal.
177  */
178  double mahalanobisDistanceTo(const CPose3DPDFGaussianInf& theOther);
179 
180 }; // End of class def.
181 bool operator==(
182  const CPose3DPDFGaussianInf& p1, const CPose3DPDFGaussianInf& p2);
183 /** Pose composition for two 3D pose Gaussians \sa CPose3DPDFGaussian::operator
184  * += */
185 CPose3DPDFGaussianInf operator+(
186  const CPose3DPDFGaussianInf& x, const CPose3DPDFGaussianInf& u);
187 /** Pose composition for two 3D pose Gaussians \sa
188  * CPose3DPDFGaussianInf::operator -= */
189 CPose3DPDFGaussianInf operator-(
190  const CPose3DPDFGaussianInf& x, const CPose3DPDFGaussianInf& u);
191 /** Dumps the mean and covariance matrix to a text stream. */
192 std::ostream& operator<<(std::ostream& out, const CPose3DPDFGaussianInf& obj);
193 
194 } // namespace mrpt::poses
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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.
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:394
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
void getInformationMatrix(mrpt::math::CMatrixDouble66 &inf) const override
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) ...
CPose3DPDFGaussianInf()
Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful! ...
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
CPose3DPDFGaussianInf operator-() const
Unary - operator, returns the PDF of the inverse pose.
double mahalanobisDistanceTo(const CPose3DPDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:356
void getMean(CPose3D &mean_pose) const override
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
bool isInfType() const override
Returns whether the class instance holds the uncertainty in covariance or information form...
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void operator-=(const CPose3DPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:369
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.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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 ...
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020