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

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020 