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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019