Main MRPT website > C++ reference for MRPT 1.9.9
CPointPDFSOG.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/CPosePDF.h>
14 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/random.h>
20 #include <mrpt/system/os.h>
21 
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 
25 using namespace mrpt::bayes;
26 using namespace mrpt::random;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
32 /*---------------------------------------------------------------
33  Constructor
34  ---------------------------------------------------------------*/
35 CPointPDFSOG::CPointPDFSOG(size_t nModes) : m_modes(nModes) {}
36 /*---------------------------------------------------------------
37  clear
38  ---------------------------------------------------------------*/
39 void CPointPDFSOG::clear() { m_modes.clear(); }
40 /*---------------------------------------------------------------
41  Resize
42  ---------------------------------------------------------------*/
43 void CPointPDFSOG::resize(const size_t N) { m_modes.resize(N); }
44 /*---------------------------------------------------------------
45  getMean
46  Returns an estimate of the pose, (the mean, or mathematical expectation of the
47  PDF)
48  ---------------------------------------------------------------*/
50 {
51  size_t N = m_modes.size();
52  double X = 0, Y = 0, Z = 0;
53 
54  if (N)
55  {
57  double sumW = 0;
58 
59  for (it = m_modes.begin(); it != m_modes.end(); ++it)
60  {
61  double w;
62  sumW += w = exp(it->log_w);
63  X += it->val.mean.x() * w;
64  Y += it->val.mean.y() * w;
65  Z += it->val.mean.z() * w;
66  }
67  if (sumW > 0)
68  {
69  X /= sumW;
70  Y /= sumW;
71  Z /= sumW;
72  }
73  }
74 
75  p.x(X);
76  p.y(Y);
77  p.z(Z);
78 }
79 
80 /*---------------------------------------------------------------
81  getCovarianceAndMean
82  ---------------------------------------------------------------*/
84  CMatrixDouble33& estCov, CPoint3D& p) const
85 {
86  size_t N = m_modes.size();
87 
88  getMean(p);
89  estCov.zeros();
90 
91  if (N)
92  {
93  // 1) Get the mean:
94  double sumW = 0;
96 
98 
99  CMatrixDouble33 partCov;
100 
101  for (it = m_modes.begin(); it != m_modes.end(); ++it)
102  {
103  double w;
104  sumW += w = exp(it->log_w);
105 
106  // estCov += w * ( it->val.cov +
107  // ((estMean_i-estMean)*(~(estMean_i-estMean))) );
108  CMatrixDouble31 estMean_i = CMatrixDouble31(it->val.mean);
109  estMean_i -= estMean;
110  partCov.multiply_AAt(estMean_i);
111  partCov += it->val.cov;
112  partCov *= w;
113  estCov += partCov;
114  }
115 
116  if (sumW != 0) estCov *= (1.0 / sumW);
117  }
118 }
119 
122 {
123  uint32_t N = m_modes.size();
124  out << N;
125  for (const auto& m : m_modes)
126  {
127  out << m.log_w;
128  out << m.val.mean;
130  }
131 }
134 {
135  switch (version)
136  {
137  case 0:
138  case 1:
139  {
140  uint32_t N;
141  in >> N;
142  this->resize(N);
143  for (auto& m : m_modes)
144  {
145  in >> m.log_w;
146 
147  // In version 0, weights were linear!!
148  if (version == 0) m.log_w = log(max(1e-300, m.log_w));
149 
150  in >> m.val.mean;
152  }
153  }
154  break;
155  default:
157  };
158 }
159 
161 {
162  MRPT_START
163 
164  if (this == &o) return; // It may be used sometimes
165 
167  {
168  m_modes = static_cast<const CPointPDFSOG*>(&o)->m_modes;
169  }
170  else
171  {
172  // Approximate as a mono-modal gaussian pdf:
173  this->resize(1);
174  m_modes[0].log_w = 0;
175  o.getCovarianceAndMean(m_modes[0].val.cov, m_modes[0].val.mean);
176  }
177 
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  saveToTextFile
183  ---------------------------------------------------------------*/
185 {
186  FILE* f = os::fopen(file.c_str(), "wt");
187  if (!f) return false;
188 
189  for (CListGaussianModes::const_iterator it = m_modes.begin();
190  it != m_modes.end(); ++it)
191  os::fprintf(
192  f, "%e %e %e %e %e %e %e %e %e %e\n", exp(it->log_w),
193  it->val.mean.x(), it->val.mean.y(), it->val.mean.z(),
194  it->val.cov(0, 0), it->val.cov(1, 1), it->val.cov(2, 2),
195  it->val.cov(0, 1), it->val.cov(0, 2), it->val.cov(1, 2));
196  os::fclose(f);
197  return true;
198 }
199 
200 /*---------------------------------------------------------------
201  changeCoordinatesReference
202  ---------------------------------------------------------------*/
204 {
205  for (auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
206 }
207 
208 /*---------------------------------------------------------------
209  drawSingleSample
210  ---------------------------------------------------------------*/
212 {
213  MRPT_START
214 
215  ASSERT_(m_modes.size() > 0);
216 
217  // 1st: Select a mode with a probability proportional to its weight:
218  vector<double> logWeights(m_modes.size());
219  vector<size_t> outIdxs;
222  for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
223  ++it, ++itW)
224  *itW = it->log_w;
225 
226  CParticleFilterCapable::computeResampling(
227  CParticleFilter::prMultinomial, // Resampling algorithm
228  logWeights, // input: log weights
229  outIdxs // output: indexes
230  );
231 
232  // we need just one: take the first (arbitrary)
233  size_t selectedIdx = outIdxs[0];
234  ASSERT_(selectedIdx < m_modes.size());
235  const CPointPDFGaussian* selMode = &m_modes[selectedIdx].val;
236 
237  // 2nd: Draw a position from the selected Gaussian:
238  CVectorDouble vec;
239  getRandomGenerator().drawGaussianMultivariate(vec, selMode->cov);
240 
241  ASSERT_(vec.size() == 3);
242  outSample.x(selMode->mean.x() + vec[0]);
243  outSample.y(selMode->mean.y() + vec[1]);
244  outSample.z(selMode->mean.z() + vec[2]);
245 
246  MRPT_END
247 }
248 
249 /*---------------------------------------------------------------
250  bayesianFusion
251  ---------------------------------------------------------------*/
253  const CPointPDF& p1_, const CPointPDF& p2_,
254  const double minMahalanobisDistToDrop)
255 {
256  MRPT_START
257 
258  // p1: CPointPDFSOG, p2: CPosePDFGaussian:
259 
262 
263  const CPointPDFSOG* p1 = static_cast<const CPointPDFSOG*>(&p1_);
264  const CPointPDFSOG* p2 = static_cast<const CPointPDFSOG*>(&p2_);
265 
266  // Compute the new kernel means, covariances, and weights after multiplying
267  // to the Gaussian "p2":
268  CPointPDFGaussian auxGaussianProduct, auxSOG_Kernel_i;
269 
270  float minMahalanobisDistToDrop2 = square(minMahalanobisDistToDrop);
271 
272  this->m_modes.clear();
273  bool is2D =
274  false; // to detect & avoid errors in 3x3 matrix inversions of range=2.
275 
276  for (CListGaussianModes::const_iterator it1 = p1->m_modes.begin();
277  it1 != p1->m_modes.end(); ++it1)
278  {
279  CMatrixDouble33 c = it1->val.cov;
280 
281  // Is a 2D covariance??
282  if (c.get_unsafe(2, 2) == 0)
283  {
284  is2D = true;
285  c.set_unsafe(2, 2, 1);
286  }
287 
288  ASSERT_(c(0, 0) != 0 && c(0, 0) != 0);
289 
291  c.inv(covInv);
292 
293  CMatrixDouble31 eta = covInv * CMatrixDouble31(it1->val.mean);
294 
295  // Normal distribution canonical form constant:
296  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
297  double a = -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
298  eta.multiply_HtCH_scalar(
299  c)); // (~eta * (*it1).val.cov * eta)(0,0) );
300 
301  for (CListGaussianModes::const_iterator it2 = p2->m_modes.begin();
302  it2 != p2->m_modes.end(); ++it2)
303  {
304  auxSOG_Kernel_i = (*it2).val;
305  if (auxSOG_Kernel_i.cov.get_unsafe(2, 2) == 0)
306  {
307  auxSOG_Kernel_i.cov.set_unsafe(2, 2, 1);
308  is2D = true;
309  }
310  ASSERT_(
311  auxSOG_Kernel_i.cov(0, 0) > 0 && auxSOG_Kernel_i.cov(1, 1) > 0);
312 
313  // Should we drop this product term??
314  bool reallyComputeThisOne = true;
315  if (minMahalanobisDistToDrop > 0)
316  {
317  // Approximate (fast) mahalanobis distance (square):
318  float mahaDist2;
319 
320  float stdX2 =
321  max(auxSOG_Kernel_i.cov.get_unsafe(0, 0),
322  (*it1).val.cov.get_unsafe(0, 0));
323  mahaDist2 =
324  square(auxSOG_Kernel_i.mean.x() - (*it1).val.mean.x()) /
325  stdX2;
326 
327  float stdY2 =
328  max(auxSOG_Kernel_i.cov.get_unsafe(1, 1),
329  (*it1).val.cov.get_unsafe(1, 1));
330  mahaDist2 +=
331  square(auxSOG_Kernel_i.mean.y() - (*it1).val.mean.y()) /
332  stdY2;
333 
334  if (!is2D)
335  {
336  float stdZ2 =
337  max(auxSOG_Kernel_i.cov.get_unsafe(2, 2),
338  (*it1).val.cov.get_unsafe(2, 2));
339  mahaDist2 +=
340  square(auxSOG_Kernel_i.mean.z() - (*it1).val.mean.z()) /
341  stdZ2;
342  }
343 
344  reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
345  }
346 
347  if (reallyComputeThisOne)
348  {
349  auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, (*it1).val);
350 
351  // ----------------------------------------------------------------------
352  // The new weight is given by:
353  //
354  // w'_i = w_i * exp( a + a_i - a' )
355  //
356  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1))
357  // + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
358  //
359  // ----------------------------------------------------------------------
360  TGaussianMode newKernel;
361 
362  newKernel.val = auxGaussianProduct; // Copy mean & cov
363 
364  CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inv();
365  CMatrixDouble31 eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
366  eta_i = covInv_i * eta_i;
367 
368  CMatrixDouble33 new_covInv_i = newKernel.val.cov.inv();
369  CMatrixDouble31 new_eta_i = CMatrixDouble31(newKernel.val.mean);
370  new_eta_i = new_covInv_i * new_eta_i;
371 
372  double a_i =
373  -0.5 *
374  (3 * log(M_2PI) - log(new_covInv_i.det()) +
375  (eta_i.adjoint() * auxSOG_Kernel_i.cov * eta_i)(0, 0));
376  double new_a_i =
377  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
378  (new_eta_i.adjoint() * newKernel.val.cov *
379  new_eta_i)(0, 0));
380 
381  newKernel.log_w =
382  (it1)->log_w + (it2)->log_w + a + a_i - new_a_i;
383 
384  // Fix 2D case:
385  if (is2D) newKernel.val.cov(2, 2) = 0;
386 
387  // Add to the results (in "this") the new kernel:
388  this->m_modes.push_back(newKernel);
389  } // end if reallyComputeThisOne
390  } // end for it2
391 
392  } // end for it1
393 
394  normalizeWeights();
395 
396  MRPT_END
397 }
398 
399 /*---------------------------------------------------------------
400  assureSymmetry
401  ---------------------------------------------------------------*/
403 {
404  MRPT_START
405  // Differences, when they exist, appear in the ~15'th significant
406  // digit, so... just take one of them arbitrarily!
407  for (CListGaussianModes::iterator it = m_modes.begin(); it != m_modes.end();
408  ++it)
409  {
410  it->val.cov(0, 1) = it->val.cov(1, 0);
411  it->val.cov(0, 2) = it->val.cov(2, 0);
412  it->val.cov(1, 2) = it->val.cov(2, 1);
413  }
414 
415  MRPT_END
416 }
417 
418 /*---------------------------------------------------------------
419  normalizeWeights
420  ---------------------------------------------------------------*/
422 {
423  MRPT_START
424 
425  if (!m_modes.size()) return;
426 
428  double maxW = m_modes[0].log_w;
429  for (it = m_modes.begin(); it != m_modes.end(); ++it)
430  maxW = max(maxW, it->log_w);
431 
432  for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;
433 
434  MRPT_END
435 }
436 
437 /*---------------------------------------------------------------
438  ESS
439  ---------------------------------------------------------------*/
440 double CPointPDFSOG::ESS() const
441 {
442  MRPT_START
444  double cum = 0;
445 
446  /* Sum of weights: */
447  double sumLinearWeights = 0;
448  for (it = m_modes.begin(); it != m_modes.end(); ++it)
449  sumLinearWeights += exp(it->log_w);
450 
451  /* Compute ESS: */
452  for (it = m_modes.begin(); it != m_modes.end(); ++it)
453  cum += square(exp(it->log_w) / sumLinearWeights);
454 
455  if (cum == 0)
456  return 0;
457  else
458  return 1.0 / (m_modes.size() * cum);
459  MRPT_END
460 }
461 
462 /*---------------------------------------------------------------
463  evaluatePDFInArea
464  ---------------------------------------------------------------*/
466  float x_min, float x_max, float y_min, float y_max, float resolutionXY,
467  float z, CMatrixD& outMatrix, bool sumOverAllZs)
468 {
469  MRPT_START
470 
471  ASSERT_(x_max > x_min);
472  ASSERT_(y_max > y_min);
473  ASSERT_(resolutionXY > 0);
474 
475  const size_t Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
476  const size_t Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
477  outMatrix.setSize(Ny, Nx);
478 
479  for (size_t i = 0; i < Ny; i++)
480  {
481  const float y = y_min + i * resolutionXY;
482  for (size_t j = 0; j < Nx; j++)
483  {
484  float x = x_min + j * resolutionXY;
485  outMatrix(i, j) = evaluatePDF(CPoint3D(x, y, z), sumOverAllZs);
486  }
487  }
488 
489  MRPT_END
490 }
491 
492 /*---------------------------------------------------------------
493  evaluatePDF
494  ---------------------------------------------------------------*/
495 double CPointPDFSOG::evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const
496 {
497  if (!sumOverAllZs)
498  {
499  // Normal evaluation:
501  double ret = 0;
502 
503  CMatrixDouble31 MU;
504 
505  for (CListGaussianModes::const_iterator it = m_modes.begin();
506  it != m_modes.end(); ++it)
507  {
508  MU = CMatrixDouble31(it->val.mean);
509  ret += exp(it->log_w) * math::normalPDF(X, MU, it->val.cov);
510  }
511 
512  return ret;
513  }
514  else
515  {
516  // Only X,Y:
517  CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
518  double ret = 0;
519 
520  X(0, 0) = x.x();
521  X(1, 0) = x.y();
522 
523  for (CListGaussianModes::const_iterator it = m_modes.begin();
524  it != m_modes.end(); ++it)
525  {
526  MU(0, 0) = it->val.mean.x();
527  MU(1, 0) = it->val.mean.y();
528 
529  COV(0, 0) = it->val.cov(0, 0);
530  COV(1, 1) = it->val.cov(1, 1);
531  COV(0, 1) = COV(1, 0) = it->val.cov(0, 1);
532 
533  ret += exp(it->log_w) * math::normalPDF(X, MU, COV);
534  }
535 
536  return ret;
537  }
538 }
539 
540 /*---------------------------------------------------------------
541  getMostLikelyMode
542  ---------------------------------------------------------------*/
544 {
545  if (this->empty())
546  {
547  outVal = CPointPDFGaussian();
548  }
549  else
550  {
551  const_iterator it_best = m_modes.end();
552  for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
553  if (it_best == m_modes.end() || it->log_w > it_best->log_w)
554  it_best = it;
555 
556  outVal = it_best->val;
557  }
558 }
559 
560 /*---------------------------------------------------------------
561  getAs3DObject
562  ---------------------------------------------------------------*/
563 // void CPointPDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjects::Ptr &outObj
564 // )
565 // const
566 //{
567 // // For each gaussian node
568 // for (CListGaussianModes::const_iterator it = m_modes.begin(); it!=
569 // m_modes.end();++it)
570 // {
571 // opengl::CEllipsoid::Ptr obj =
572 // mrpt::make_aligned_shared<opengl::CEllipsoid>();
573 //
574 // obj->setPose( it->val.mean);
575 // obj->setCovMatrix(it->val.cov, it->val.cov(2,2)==0 ? 2:3);
576 //
577 // obj->setQuantiles(3);
578 // obj->enableDrawSolid3D(false);
579 // obj->setColor(1,0,0, 0.5);
580 //
581 // outObj->insert( obj );
582 // } // end for each gaussian node
583 //}
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
Scalar * iterator
Definition: eigen_plugins.h:26
EIGEN_STRONG_INLINE bool empty() const
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:24
#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
void getMean(CPoint3D &mean_point) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
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.
CPoint3D mean
The mean value.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void resize(const size_t N)
Resize the number of SOG modes.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:36
The struct for each mode:
Definition: CPointPDFSOG.h:43
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
void clear()
Clear all the gaussian modes.
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
STL namespace.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
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.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:62
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
const GLubyte * c
Definition: glext.h:6313
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
int val
Definition: mrpt_jpeglib.h:955
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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...
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:33
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:54
This file implements matrix/vector text and binary serialization.
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
void evaluatePDFInArea(float x_min, float x_max, float y_min, float y_max, float resolutionXY, float z, mrpt::math::CMatrixD &outMatrix, bool sumOverAllZs=false)
Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains value...
GLenum GLint GLint y
Definition: glext.h:3538
void getMostLikelyMode(CPointPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
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
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const
Evaluates the PDF at a given point.
CListGaussianModes m_modes
The list of SOG modes.
Definition: CPointPDFSOG.h:64
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
A gaussian distribution for 3D points.
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...



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