Main MRPT website > C++ reference for MRPT 1.9.9
CPointPDFGaussian.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>
16 #include <mrpt/system/os.h>
18 
19 using namespace mrpt::poses;
20 
21 using namespace mrpt::math;
22 using namespace mrpt::random;
23 using namespace mrpt::system;
24 
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
31 /*---------------------------------------------------------------
32  Constructor
33  ---------------------------------------------------------------*/
35  const CPoint3D& init_Mean, const CMatrixDouble33& init_Cov)
36  : mean(init_Mean), cov(init_Cov)
37 {
38 }
39 
40 /*---------------------------------------------------------------
41  Constructor
42  ---------------------------------------------------------------*/
44  : mean(init_Mean), cov()
45 {
46  cov.zeros();
47 }
48 
49 /*---------------------------------------------------------------
50  getMean
51  Returns an estimate of the pose, (the mean, or mathematical expectation of the
52  PDF)
53  ---------------------------------------------------------------*/
55 /*---------------------------------------------------------------
56  getCovarianceAndMean
57  ---------------------------------------------------------------*/
59  CMatrixDouble33& C, CPoint3D& p) const
60 {
61  p = mean;
62  C = cov;
63 }
64 
67 {
68  out << CPoint3D(mean) << cov;
69 }
72 {
73  switch (version)
74  {
75  case 0:
76  {
77  in >> mean;
78 
79  CMatrix c;
80  in >> c;
81  cov = c.cast<double>();
82  }
83  break;
84  case 1:
85  {
86  in >> mean >> cov;
87  }
88  break;
89  default:
91  };
92 }
93 
95 {
96  if (this == &o) return; // It may be used sometimes
97 
98  // Convert to gaussian pdf:
100 }
101 
102 /*---------------------------------------------------------------
103 
104  ---------------------------------------------------------------*/
106 {
107  MRPT_START
108  FILE* f = os::fopen(file.c_str(), "wt");
109  if (!f) return false;
110  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.z());
111  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
112  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
113  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
114  os::fclose(f);
115  return true;
116  MRPT_END
117 }
118 
119 /*---------------------------------------------------------------
120  changeCoordinatesReference
121  ---------------------------------------------------------------*/
123  const CPose3D& newReferenceBase)
124 {
125  const CMatrixDouble33& M = newReferenceBase.getRotationMatrix();
126 
127  // The mean:
128  mean = newReferenceBase + mean;
129 
130  // The covariance:
131  M.multiply_HCHt(CMatrixDouble33(cov), cov); // save in cov
132 }
133 
134 /*---------------------------------------------------------------
135  bayesianFusion
136  ---------------------------------------------------------------*/
138  const CPointPDFGaussian& p1, const CPointPDFGaussian& p2)
139 {
140  MRPT_START
141 
142  CMatrixDouble x1(3, 1), x2(3, 1), x(3, 1);
143  CMatrixDouble C1(p1.cov);
144  CMatrixDouble C2(p2.cov);
145  CMatrixDouble C1_inv = C1.inv();
146  CMatrixDouble C2_inv = C2.inv();
147  CMatrixDouble C;
148 
149  x1(0, 0) = p1.mean.x();
150  x1(1, 0) = p1.mean.y();
151  x1(2, 0) = p1.mean.z();
152  x2(0, 0) = p2.mean.x();
153  x2(1, 0) = p2.mean.y();
154  x2(2, 0) = p2.mean.z();
155 
156  C = (C1_inv + C2_inv).inv();
157  cov = C;
158 
159  x = C * (C1_inv * x1 + C2_inv * x2);
160 
161  mean.x(x(0, 0));
162  mean.y(x(1, 0));
163  mean.z(x(2, 0));
164 
165  // std::cout << "IN1: " << x1 << "\n" << C1 << "\n";
166  // std::cout << "IN2: " << x2 << "\n" << C2 << "\n";
167  // std::cout << "OUT: " << x << "\n" << C << "\n";
168 
169  MRPT_END
170 }
171 
172 /*---------------------------------------------------------------
173  productIntegralWith
174  ---------------------------------------------------------------*/
176 {
177  MRPT_START
178 
179  // --------------------------------------------------------------
180  // 12/APR/2009 - Jose Luis Blanco:
181  // The integral over all the variable space of the product of two
182  // Gaussians variables amounts to simply the evaluation of
183  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
184  // ---------------------------------------------------------------
185  CMatrixDouble33 C = cov;
186  C += p.cov; // Sum of covs:
187  CMatrixDouble33 C_inv;
188  C.inv(C_inv);
189 
190  CMatrixDouble31 MU(UNINITIALIZED_MATRIX); // Diff. of means
191  MU.get_unsafe(0, 0) = mean.x() - p.mean.x();
192  MU.get_unsafe(1, 0) = mean.y() - p.mean.y();
193  MU.get_unsafe(2, 0) = mean.z() - p.mean.z();
194 
195  return std::pow(M_2PI, -0.5 * state_length) * (1.0 / std::sqrt(C.det())) *
196  exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));
197 
198  MRPT_END
199 }
200 
201 /*---------------------------------------------------------------
202  productIntegralWith2D
203  ---------------------------------------------------------------*/
205  const CPointPDFGaussian& p) const
206 {
207  MRPT_START
208 
209  // --------------------------------------------------------------
210  // 12/APR/2009 - Jose Luis Blanco:
211  // The integral over all the variable space of the product of two
212  // Gaussians variables amounts to simply the evaluation of
213  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
214  // ---------------------------------------------------------------
215  CMatrixDouble22 C = cov.block(0, 0, 2, 2);
216  C += p.cov.block(0, 0, 2, 2); // Sum of covs:
217 
218  CMatrixDouble22 C_inv;
219  C.inv(C_inv);
220 
221  CMatrixDouble21 MU(UNINITIALIZED_MATRIX); // Diff. of means
222  MU.get_unsafe(0, 0) = mean.x() - p.mean.x();
223  MU.get_unsafe(1, 0) = mean.y() - p.mean.y();
224 
225  return std::pow(M_2PI, -0.5 * (state_length - 1)) *
226  (1.0 / std::sqrt(C.det())) *
227  exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));
228 
229  MRPT_END
230 }
231 
232 /*---------------------------------------------------------------
233  productIntegralNormalizedWith
234  ---------------------------------------------------------------*/
236  const CPointPDFGaussian& p) const
237 {
238  return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
239 }
240 
241 /*---------------------------------------------------------------
242  productIntegralNormalizedWith
243  ---------------------------------------------------------------*/
245  const CPointPDFGaussian& p) const
246 {
247  return std::exp(-0.5 * square(mahalanobisDistanceTo(p, true)));
248 }
249 
250 /*---------------------------------------------------------------
251  drawSingleSample
252  ---------------------------------------------------------------*/
254 {
255  MRPT_START
256 
257  CVectorDouble vec;
259 
260  ASSERT_(vec.size() == 3);
261  outSample.x(mean.x() + vec[0]);
262  outSample.y(mean.y() + vec[1]);
263  outSample.z(mean.z() + vec[2]);
264 
265  MRPT_END
266 }
267 
268 /*---------------------------------------------------------------
269  bayesianFusion
270  ---------------------------------------------------------------*/
272  const CPointPDF& p1_, const CPointPDF& p2_,
273  const double minMahalanobisDistToDrop)
274 {
275  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
276  MRPT_START
277 
278  // p1: CPointPDFGaussian, p2: CPosePDFGaussian:
281 
282  THROW_EXCEPTION("TODO!!!");
283 
284  MRPT_END
285 }
286 
287 /*---------------------------------------------------------------
288  mahalanobisDistanceTo
289  ---------------------------------------------------------------*/
291  const CPointPDFGaussian& other, bool only_2D) const
292 {
293  // The difference in means:
294  CMatrixDouble13 deltaX;
295  deltaX.get_unsafe(0, 0) = other.mean.x() - mean.x();
296  deltaX.get_unsafe(0, 1) = other.mean.y() - mean.y();
297  deltaX.get_unsafe(0, 2) = other.mean.z() - mean.z();
298 
299  // The inverse of the combined covs:
300  CMatrixDouble33 COV = other.cov;
301  COV += this->cov;
302 
303  if (!only_2D)
304  {
305  CMatrixDouble33 COV_inv;
306  COV.inv(COV_inv);
307  return sqrt(deltaX.multiply_HCHt_scalar(COV_inv));
308  }
309  else
310  {
311  CMatrixDouble22 C = COV.block(0, 0, 2, 2);
312  CMatrixDouble22 COV_inv;
313  C.inv(COV_inv);
314  CMatrixDouble12 deltaX2 = deltaX.block(0, 0, 1, 2);
315  return std::sqrt(deltaX2.multiply_HCHt_scalar(COV_inv));
316  }
317 }
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void getMean(CPoint3D &p) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
#define MRPT_START
Definition: exceptions.h:262
#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
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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.
CPoint3D mean
The mean value.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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 bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
This base provides a set of functions for maths stuff.
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...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
const GLubyte * c
Definition: glext.h:6313
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.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:33
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
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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...
This file implements matrix/vector text and binary serialization.
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
#define MRPT_END
Definition: exceptions.h:266
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:57
GLuint in
Definition: glext.h:7274
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
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
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:39
GLfloat GLfloat p
Definition: glext.h:6305
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
CPointPDFGaussian()
Default constructor.
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
A gaussian distribution for 3D points.
EIGEN_STRONG_INLINE PlainObject inv() const
#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