MRPT  1.9.9
CPointPDFGaussian.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/CMatrixF.h>
12 #include <mrpt/poses/CPointPDF.h>
13 
14 namespace mrpt::poses
15 {
16 /** A gaussian distribution for 3D points. Also a method for bayesian fusion is
17  * provided.
18  *
19  * \sa CPointPDF
20  * \ingroup poses_pdf_grp
21  */
23 {
26 
27  public:
28  /** Default constructor
29  */
31 
32  /** Constructor
33  */
34  CPointPDFGaussian(const CPoint3D& init_Mean);
35 
36  /** Constructor
37  */
39  const CPoint3D& init_Mean, const mrpt::math::CMatrixDouble33& init_Cov);
40 
41  /** The mean value */
43  /** The 3x3 covariance matrix */
45 
46  void getMean(CPoint3D& p) const override;
47  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override
48  {
49  return {cov, mean};
50  }
51 
52  /** Copy operator, translating if necesary (for example, between particles
53  * and gaussian representations) */
54  void copyFrom(const CPointPDF& o) override;
55 
56  /** Save PDF's particles to a text file, containing the 2D pose in the first
57  * line, then the covariance matrix in next 3 lines. */
58  bool saveToTextFile(const std::string& file) const override;
59 
60  /** this = p (+) this. This can be used to convert a PDF from local
61  * coordinates to global, providing the point (newReferenceBase) from which
62  * "to project" the current pdf. Result PDF substituted the currently
63  * stored one in the object. Both the mean value and the covariance matrix
64  * are updated correctly. */
65  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
66 
67  /** Bayesian fusion of two points gauss. distributions, then save the result
68  *in this object.
69  * The process is as follows:<br>
70  * - (x1,S1): Mean and variance of the p1 distribution.
71  * - (x2,S2): Mean and variance of the p2 distribution.
72  * - (x,S): Mean and variance of the resulting distribution.
73  *
74  * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
75  * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
76  */
77  void bayesianFusion(
78  const CPointPDFGaussian& p1, const CPointPDFGaussian& p2);
79 
80  /** Computes the "correspondence likelihood" of this PDF with another one:
81  * This is implemented as the integral from -inf to +inf of the product of
82  * both PDF.
83  * The resulting number is >=0.
84  * \sa productIntegralNormalizedWith
85  * \exception std::exception On errors like covariance matrix with null
86  * determinant, etc...
87  */
88  double productIntegralWith(const CPointPDFGaussian& p) const;
89 
90  /** Computes the "correspondence likelihood" of this PDF with another one:
91  * This is implemented as the integral from -inf to +inf of the product of
92  * both PDF.
93  * The resulting number is >=0.
94  * NOTE: This version ignores the "z" coordinates!!
95  * \sa productIntegralNormalizedWith
96  * \exception std::exception On errors like covariance matrix with null
97  * determinant, etc...
98  */
99  double productIntegralWith2D(const CPointPDFGaussian& p) const;
100 
101  /** Computes the "correspondence likelihood" of this PDF with another one:
102  * This is implemented as the integral from -inf to +inf of the product of
103  * both PDF.
104  * The resulting number is in the range [0,1]
105  * Note that the resulting value is in fact
106  * \f[ exp( -\frac{1}{2} D^2 ) \f]
107  * , with \f$ D^2 \f$ being the square Mahalanobis distance between the
108  * two pdfs.
109  * \sa productIntegralWith
110  * \exception std::exception On errors like covariance matrix with null
111  * determinant, etc...
112  */
113  double productIntegralNormalizedWith(const CPointPDFGaussian& p) const;
114 
115  /** Computes the "correspondence likelihood" of this PDF with another one:
116  * This is implemented as the integral from -inf to +inf of the product of
117  * both PDF.
118  * The resulting number is in the range [0,1]. This versions ignores the
119  * "z" coordinate.
120  *
121  * Note that the resulting value is in fact
122  * \f[ exp( -\frac{1}{2} D^2 ) \f]
123  * , with \f$ D^2 \f$ being the square Mahalanobis distance between the
124  * two pdfs.
125  * \sa productIntegralWith
126  * \exception std::exception On errors like covariance matrix with null
127  * determinant, etc...
128  */
129  double productIntegralNormalizedWith2D(const CPointPDFGaussian& p) const;
130 
131  /** Draw a sample from the pdf */
132  void drawSingleSample(CPoint3D& outSample) const override;
133 
134  /** Bayesian fusion of two point distributions (product of two
135  * distributions->new distribution), then save the result in this object
136  * (WARNING: See implementing classes to see classes that can and cannot be
137  * mixtured!)
138  * \param p1 The first distribution to fuse
139  * \param p2 The second distribution to fuse
140  * \param minMahalanobisDistToDrop If set to different of 0, the result of
141  * very separate Gaussian modes (that will result in negligible components)
142  * in SOGs will be dropped to reduce the number of modes in the output.
143  */
144  void bayesianFusion(
145  const CPointPDF& p1, const CPointPDF& p2,
146  const double minMahalanobisDistToDrop = 0) override;
147 
148  /** Returns the Mahalanobis distance from this PDF to another PDF, that is,
149  * it's evaluation at (0,0,0) */
150  double mahalanobisDistanceTo(
151  const CPointPDFGaussian& other, bool only_2D = false) const;
152 
153 }; // End of class def.
154 } // namespace mrpt::poses
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getMean(CPoint3D &p) const override
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CPoint3D mean
The mean value.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
STL namespace.
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
double productIntegralNormalizedWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
double mahalanobisDistanceTo(const CPointPDFGaussian &other, bool only_2D=false) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
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...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A gaussian distribution for 3D points.
CPoint3D type_value
The type of the state the PDF represents.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020