MRPT  2.0.1
CPose3DQuatPDFGaussianInf.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/CPose3DPDF.h>
14 #include <mrpt/poses/CPosePDF.h>
15 
16 namespace mrpt::poses
17 {
18 class CPosePDFGaussian;
19 class CPose3DPDFGaussian;
20 
21 /** Declares a class that represents a Probability Density function (PDF) of a
22  * 3D pose using a quaternion \f$ p(\mathbf{x}) = [x ~ y ~ z ~ qr ~ qx ~ qy ~
23  * qz]^\top \f$.
24  *
25  * This class implements that PDF using a mono-modal Gaussian distribution
26  * storing the information matrix instead of its inverse, the covariance matrix.
27  * See mrpt::poses::CPose3DQuatPDF for more details, or
28  * mrpt::poses::CPose3DPDF for classes based on Euler angles instead.
29  *
30  * Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is
31  * implemented in the methods "CPose3DQuatPDFGaussianInf::operator+=" and
32  * "CPose3DQuatPDF::jacobiansPoseComposition".
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 CPose3DQuat, CPose3DQuatPDF, CPose3DPDF, CPose3DQuatPDFGaussian
40  * \ingroup poses_pdf_grp
41  */
43 {
44  // This must be added to any CSerializable derived class:
47 
48  public:
49  /** Default constructor - set all values to zero. */
51 
52  /** Constructor which left all the member uninitialized, for using when
53  * speed is critical - as argument, use UNINITIALIZED_QUATERNION. */
55  mrpt::math::TConstructorFlags_Quaternions constructor_dummy_param);
56 
57  /** Constructor from a default mean value, information matrix equals to
58  * zero. */
59  explicit CPose3DQuatPDFGaussianInf(const CPose3DQuat& init_Mean);
60 
61  /** Constructor with mean and inverse covariance (information matrix). */
63  const CPose3DQuat& init_Mean,
64  const mrpt::math::CMatrixDouble77& init_CovInv);
65 
66  /** The mean value */
68  /** The 7x7 information matrix (the inverse of the covariance) */
70 
71  inline const CPose3DQuat& getPoseMean() const { return mean; }
72  inline CPose3DQuat& getPoseMean() { return mean; }
73 
74  void getMean(CPose3DQuat& mean_pose) const override { mean_pose = mean; }
75  bool isInfType() const override { return true; }
76 
77  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override
78  {
79  return {cov_inv.inverse_LLt(), mean};
80  }
81 
82  /** Returns the information (inverse covariance) matrix (a STATE_LEN x
83  * STATE_LEN matrix) \sa getMean, getCovarianceAndMean */
85  {
86  inf = cov_inv;
87  }
88 
89  /** Copy operator, translating if necesary (for example, between particles
90  * and gaussian representations) */
91  void copyFrom(const CPose3DQuatPDF& o) override;
92 
93  /** Save the PDF to a text file, containing the 3D pose in the first line (x
94  * y z qr qx qy qz), then the information matrix in the next 7 lines. */
95  bool saveToTextFile(const std::string& file) const override;
96 
97  /** this = p (+) this. This can be used to convert a PDF from local
98  * coordinates to global, providing the point (newReferenceBase) from which
99  * "to project" the current pdf. Result PDF substituted the currently
100  * stored one in the object. */
101  void changeCoordinatesReference(const CPose3DQuat& newReferenceBase);
102 
103  /** this = p (+) this. This can be used to convert a PDF from local
104  * coordinates to global, providing the point (newReferenceBase) from which
105  * "to project" the current pdf. Result PDF substituted the currently
106  * stored one in the object. */
107  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
108 
109  /** Draws a single sample from the distribution */
110  void drawSingleSample(CPose3DQuat& outPart) const override;
111  /** Draws a number of samples from the distribution, and saves as a list of
112  * 1x7 vectors, where each row contains a (x,y,z,qr,qx,qy,qz) datum */
113  void drawManySamples(
114  size_t N,
115  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
116  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
117  void inverse(CPose3DQuatPDF& o) const override;
118 
119  /** Unary - operator, returns the PDF of the inverse pose. */
121  {
123  this->inverse(p);
124  return p;
125  }
126 
127  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the
128  * mean, and the covariance matrix are updated). */
129  void operator+=(const CPose3DQuat& Ap);
130  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the
131  * mean, and the covariance matrix are updated) (see formulas in
132  * jacobiansPoseComposition ). */
133  void operator+=(const CPose3DQuatPDFGaussianInf& Ap);
134  /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition
135  * (both the mean, and the covariance matrix are updated). */
136  void operator-=(const CPose3DQuatPDFGaussianInf& Ap);
137 
138  /** Evaluates the PDF at a given point */
139  double evaluatePDF(const CPose3DQuat& x) const;
140  /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in
141  * the range [0,1] */
142  double evaluateNormalizedPDF(const CPose3DQuat& x) const;
143 
144 }; // End of class def.
145 
146 bool operator==(
147  const CPose3DQuatPDFGaussianInf& p1, const CPose3DQuatPDFGaussianInf& p2);
148 /** Pose composition for two 3D pose Gaussians \sa
149  * CPose3DQuatPDFGaussianInf::operator += */
150 CPose3DQuatPDFGaussianInf operator+(
151  const CPose3DQuatPDFGaussianInf& x, const CPose3DQuatPDFGaussianInf& u);
152 /** Inverse pose composition for two 3D pose Gaussians \sa
153  * CPose3DQuatPDFGaussianInf::operator -= */
154 CPose3DQuatPDFGaussianInf operator-(
155  const CPose3DQuatPDFGaussianInf& x, const CPose3DQuatPDFGaussianInf& u);
156 
157 /** Dumps the mean and covariance matrix to a text stream. */
158 std::ostream& operator<<(
159  std::ostream& out, const CPose3DQuatPDFGaussianInf& obj);
160 
161 } // namespace mrpt::poses
void inverse(CPose3DQuatPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
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
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
void copyFrom(const CPose3DQuatPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
CPose3DQuatPDFGaussianInf operator-() const
Unary - operator, returns the PDF of the inverse pose.
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:370
double evaluateNormalizedPDF(const CPose3DQuat &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
void operator+=(const CPose3DQuat &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
mrpt::math::CMatrixDouble77 cov_inv
The 7x7 information matrix (the inverse of the covariance)
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
void getInformationMatrix(mrpt::math::CMatrixDouble77 &inf) const override
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) ...
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
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line (x y z qr qx qy qz)...
double evaluatePDF(const CPose3DQuat &x) const
Evaluates the PDF at a given point.
Classes for 2D/3D geometry representation, both of single values and probability density 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 1x7 vectors, where each row contains a (x,y,z,qr,qx,qy,qz) datum.
bool isInfType() const override
Returns whether the class instance holds the uncertainty in covariance or information form...
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
void operator-=(const CPose3DQuatPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void drawSingleSample(CPose3DQuat &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...
void getMean(CPose3DQuat &mean_pose) const override
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...



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