Main MRPT website > C++ reference for MRPT 1.9.9
CPoint2DPDFGaussian.cpp
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 
10 #include "poses-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/math/CMatrixD.h>
19 #include <mrpt/system/os.h>
20 
21 using namespace mrpt::poses;
22 
23 using namespace mrpt::math;
24 using namespace mrpt::random;
25 using namespace mrpt::system;
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
37  const CPoint2D& init_Mean, const CMatrixDouble22& init_Cov)
38  : mean(init_Mean), cov(init_Cov)
39 {
40 }
41 
42 /*---------------------------------------------------------------
43  Constructor
44  ---------------------------------------------------------------*/
46  : mean(init_Mean), cov()
47 {
48 }
49 
52 {
53  out << CPoint2D(mean) << cov;
54 }
57 {
58  switch (version)
59  {
60  case 0:
61  {
62  in >> mean >> cov;
63  }
64  break;
65  default:
67  };
68 }
69 
71 {
72  if (this == &o) return; // It may be used sometimes
73 
74  // Convert to gaussian pdf:
76 }
77 
78 /*---------------------------------------------------------------
79 
80  ---------------------------------------------------------------*/
82 {
84 
85  FILE* f = os::fopen(file.c_str(), "wt");
86  if (!f) return false;
87 
88  os::fprintf(f, "%f %f\n", mean.x(), mean.y());
89 
90  os::fprintf(f, "%f %f\n", cov(0, 0), cov(0, 1));
91  os::fprintf(f, "%f %f\n", cov(1, 0), cov(1, 1));
92 
93  os::fclose(f);
94  return true;
95  MRPT_END
96 }
97 
98 /*---------------------------------------------------------------
99  changeCoordinatesReference
100  ---------------------------------------------------------------*/
102  const CPose3D& newReferenceBase)
103 {
104  // Clip the 3x3 rotation matrix
105  const CMatrixDouble22 M =
106  newReferenceBase.getRotationMatrix().block(0, 0, 2, 2);
107 
108  // The mean:
109  mean = CPoint2D(newReferenceBase + mean);
110 
111  // The covariance:
112  cov = M * cov * M.transpose();
113 }
114 
115 /*---------------------------------------------------------------
116  bayesianFusion
117  ---------------------------------------------------------------*/
119  const CPoint2DPDFGaussian& p1, const CPoint2DPDFGaussian& p2)
120 {
121  MRPT_START
122 
123  CMatrixDouble22 C1_inv;
124  p1.cov.inv(C1_inv);
125 
126  CMatrixDouble22 C2_inv;
127  p2.cov.inv(C2_inv);
128 
129  CMatrixDouble22 L = C1_inv;
130  L += C2_inv;
131 
132  L.inv(cov); // The new cov.
133 
134  const Eigen::Vector2d x1{p1.mean.x(), p1.mean.y()};
135  const Eigen::Vector2d x2{p2.mean.x(), p2.mean.y()};
136  CMatrixDouble21 x = cov * (C1_inv * x1 + C2_inv * x2);
137 
138  mean.x(x.get_unsafe(0, 0));
139  mean.y(x.get_unsafe(1, 0));
140 
141  MRPT_END
142 }
143 
144 /*---------------------------------------------------------------
145  productIntegralWith
146  ---------------------------------------------------------------*/
148  const CPoint2DPDFGaussian& p) const
149 {
150  MRPT_START
151  // --------------------------------------------------------------
152  // 12/APR/2009 - Jose Luis Blanco:
153  // The integral over all the variable space of the product of two
154  // Gaussians variables amounts to simply the evaluation of
155  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
156  // ---------------------------------------------------------------
157  CMatrixDouble22 C = cov + p.cov; // Sum of covs:
158 
159  CMatrixDouble22 C_inv;
160  C.inv(C_inv);
161 
162  CMatrixDouble21 MU(UNINITIALIZED_MATRIX); // Diff. of means
163  MU.get_unsafe(0, 0) = mean.x() - p.mean.x();
164  MU.get_unsafe(1, 0) = mean.y() - p.mean.y();
165 
166  return std::pow(M_2PI, -0.5 * state_length) * (1.0 / std::sqrt(C.det())) *
167  exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));
168 
169  MRPT_END
170 }
171 
172 /*---------------------------------------------------------------
173  productIntegralNormalizedWith
174  ---------------------------------------------------------------*/
176  const CPoint2DPDFGaussian& p) const
177 {
178  return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
179 }
180 
181 /*---------------------------------------------------------------
182  drawSingleSample
183  ---------------------------------------------------------------*/
185 {
186  MRPT_START
187 
188  // Eigen3 emits an out-of-array warning here, but it seems to be a false
189  // warning? (WTF)
190  CVectorDouble vec;
192 
193  ASSERT_(vec.size() == 2);
194  outSample.x(mean.x() + vec[0]);
195  outSample.y(mean.y() + vec[1]);
196 
197  MRPT_END
198 }
199 
200 /*---------------------------------------------------------------
201  bayesianFusion
202  ---------------------------------------------------------------*/
204  const CPoint2DPDF& p1_, const CPoint2DPDF& p2_,
205  const double minMahalanobisDistToDrop)
206 {
207  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
208  MRPT_START
209 
210  // p1: CPoint2DPDFGaussian, p2: CPosePDFGaussian:
213 
214  THROW_EXCEPTION("TODO!!!");
215 
216  MRPT_END
217 }
218 
219 /*---------------------------------------------------------------
220  mahalanobisDistanceTo
221  ---------------------------------------------------------------*/
223  const CPoint2DPDFGaussian& other) const
224 {
225  // The difference in means:
226  Eigen::Matrix<double, 2, 1> deltaX;
227  deltaX[0] = other.mean.x() - mean.x();
228  deltaX[1] = other.mean.y() - mean.y();
229 
230  // The inverse of the combined covs:
231  return std::sqrt(
232  deltaX.multiply_HtCH_scalar((other.cov + this->cov).inverse()));
233 }
234 
235 /** Returns the Mahalanobis distance from this PDF to some point */
237  const double x, const double y) const
238 {
239  // The difference in means:
240  Eigen::Matrix<double, 2, 1> deltaX;
241  deltaX[0] = x - mean.x();
242  deltaX[1] = y - mean.y();
243 
244  // The inverse of the combined covs:
245  return std::sqrt(deltaX.multiply_HtCH_scalar(this->cov.inverse()));
246 }
A namespace of pseudo-random numbers generators of diferent distributions.
double productIntegralNormalizedWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
#define MRPT_START
Definition: exceptions.h:262
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
#define M_2PI
Definition: common.h:58
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
A gaussian distribution for 2D points.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void copyFrom(const CPoint2DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void drawSingleSample(CPoint2D &outSample) const override
Draw a sample from the pdf.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
CPoint2DPDFGaussian()
Default constructor.
void bayesianFusion(const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2)
Bayesian fusion of two points gauss.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Declares a class that represents a Probability Distribution function (PDF) of a 2D point (x...
Definition: CPoint2DPDF.h:35
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 2D point.
Definition: CPoint2D.h:35
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static const size_t state_length
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll)...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
This file implements matrix/vector text and binary serialization.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
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
GLenum GLint x
Definition: glext.h:3538
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:232
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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