MRPT  2.0.4
CPointPDFSOG.cpp
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 
10 #include "poses-precomp.h" // Precompiled headers
11 
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPosePDF.h>
18 #include <mrpt/random.h>
20 #include <mrpt/system/os.h>
21 #include <Eigen/Dense>
22 
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
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  {
56  CListGaussianModes::const_iterator it;
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 std::tuple<CMatrixDouble33, CPoint3D> CPointPDFSOG::getCovarianceAndMean() const
81 {
82  size_t N = m_modes.size();
83 
84  CMatrixDouble33 estCov;
85  CPoint3D p;
86  getMean(p);
87  estCov.setZero();
88 
89  if (N)
90  {
91  // 1) Get the mean:
92  double sumW = 0;
93  auto estMean = CMatrixDouble31(p);
94 
95  CListGaussianModes::const_iterator it;
96 
97  for (it = m_modes.begin(); it != m_modes.end(); ++it)
98  {
99  double w;
100  sumW += w = exp(it->log_w);
101 
102  auto estMean_i = CMatrixDouble31(it->val.mean);
103  estMean_i -= estMean;
104 
105  auto partCov =
106  CMatrixDouble33(estMean_i.asEigen() * estMean_i.transpose());
107  partCov += it->val.cov;
108  partCov *= w;
109  estCov += partCov;
110  }
111 
112  if (sumW != 0) estCov *= (1.0 / sumW);
113  }
114 
115  return {estCov, p};
116 }
117 
118 uint8_t CPointPDFSOG::serializeGetVersion() const { return 1; }
120 {
121  uint32_t N = m_modes.size();
122  out << N;
123  for (const auto& m : m_modes)
124  {
125  out << m.log_w;
126  out << m.val.mean;
128  }
129 }
131  mrpt::serialization::CArchive& in, uint8_t version)
132 {
133  switch (version)
134  {
135  case 0:
136  case 1:
137  {
138  uint32_t N;
139  in >> N;
140  this->resize(N);
141  for (auto& m : m_modes)
142  {
143  in >> m.log_w;
144 
145  // In version 0, weights were linear!!
146  if (version == 0) m.log_w = log(max(1e-300, m.log_w));
147 
148  in >> m.val.mean;
150  }
151  }
152  break;
153  default:
155  };
156 }
157 
159 {
160  MRPT_START
161 
162  if (this == &o) return; // It may be used sometimes
163 
165  {
166  m_modes = dynamic_cast<const CPointPDFSOG*>(&o)->m_modes;
167  }
168  else
169  {
170  // Approximate as a mono-modal gaussian pdf:
171  this->resize(1);
172  m_modes[0].log_w = 0;
173  o.getCovarianceAndMean(m_modes[0].val.cov, m_modes[0].val.mean);
174  }
175 
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  saveToTextFile
181  ---------------------------------------------------------------*/
182 bool CPointPDFSOG::saveToTextFile(const std::string& file) const
183 {
184  FILE* f = os::fopen(file.c_str(), "wt");
185  if (!f) return false;
186 
187  for (const auto& m_mode : m_modes)
188  os::fprintf(
189  f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m_mode.log_w),
190  m_mode.val.mean.x(), m_mode.val.mean.y(), m_mode.val.mean.z(),
191  m_mode.val.cov(0, 0), m_mode.val.cov(1, 1), m_mode.val.cov(2, 2),
192  m_mode.val.cov(0, 1), m_mode.val.cov(0, 2), m_mode.val.cov(1, 2));
193  os::fclose(f);
194  return true;
195 }
196 
197 /*---------------------------------------------------------------
198  changeCoordinatesReference
199  ---------------------------------------------------------------*/
201 {
202  for (auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
203 }
204 
205 /*---------------------------------------------------------------
206  drawSingleSample
207  ---------------------------------------------------------------*/
209 {
210  MRPT_START
211 
212  ASSERT_(m_modes.size() > 0);
213 
214  // 1st: Select a mode with a probability proportional to its weight:
215  vector<double> logWeights(m_modes.size());
216  vector<size_t> outIdxs;
217  vector<double>::iterator itW;
218  CListGaussianModes::const_iterator it;
219  for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
220  ++it, ++itW)
221  *itW = it->log_w;
222 
223  CParticleFilterCapable::computeResampling(
224  CParticleFilter::prMultinomial, // Resampling algorithm
225  logWeights, // input: log weights
226  outIdxs // output: indexes
227  );
228 
229  // we need just one: take the first (arbitrary)
230  size_t selectedIdx = outIdxs[0];
231  ASSERT_(selectedIdx < m_modes.size());
232  const CPointPDFGaussian* selMode = &m_modes[selectedIdx].val;
233 
234  // 2nd: Draw a position from the selected Gaussian:
235  CVectorDouble vec;
236  getRandomGenerator().drawGaussianMultivariate(vec, selMode->cov);
237 
238  ASSERT_(vec.size() == 3);
239  outSample.x(selMode->mean.x() + vec[0]);
240  outSample.y(selMode->mean.y() + vec[1]);
241  outSample.z(selMode->mean.z() + vec[2]);
242 
243  MRPT_END
244 }
245 
246 /*---------------------------------------------------------------
247  bayesianFusion
248  ---------------------------------------------------------------*/
250  const CPointPDF& p1_, const CPointPDF& p2_,
251  const double minMahalanobisDistToDrop)
252 {
253  MRPT_START
254 
255  // p1: CPointPDFSOG, p2: CPosePDFGaussian:
256 
259 
260  const auto* p1 = dynamic_cast<const CPointPDFSOG*>(&p1_);
261  const auto* p2 = dynamic_cast<const CPointPDFSOG*>(&p2_);
262 
263  // Compute the new kernel means, covariances, and weights after multiplying
264  // to the Gaussian "p2":
265  CPointPDFGaussian auxGaussianProduct, auxSOG_Kernel_i;
266 
267  const double minMahalanobisDistToDrop2 = square(minMahalanobisDistToDrop);
268 
269  this->m_modes.clear();
270  bool is2D =
271  false; // to detect & avoid errors in 3x3 matrix inversions of range=2.
272 
273  for (const auto& m : p1->m_modes)
274  {
275  CMatrixDouble33 c = m.val.cov;
276 
277  // Is a 2D covariance??
278  if (c(2, 2) == 0)
279  {
280  is2D = true;
281  c(2, 2) = 1;
282  }
283 
284  ASSERT_(c(0, 0) != 0 && c(0, 0) != 0);
285 
286  const CMatrixDouble33 covInv = c.inverse_LLt();
287 
288  Eigen::Vector3d eta = covInv * CMatrixDouble31(m.val.mean);
289 
290  // Normal distribution canonical form constant:
291  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
292  double a = -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
293  (eta.transpose() * c.asEigen() * eta)(0, 0));
294 
295  for (const auto& m2 : p2->m_modes)
296  {
297  auxSOG_Kernel_i = m2.val;
298  if (auxSOG_Kernel_i.cov(2, 2) == 0)
299  {
300  auxSOG_Kernel_i.cov(2, 2) = 1;
301  is2D = true;
302  }
303  ASSERT_(
304  auxSOG_Kernel_i.cov(0, 0) > 0 && auxSOG_Kernel_i.cov(1, 1) > 0);
305 
306  // Should we drop this product term??
307  bool reallyComputeThisOne = true;
308  if (minMahalanobisDistToDrop > 0)
309  {
310  // Approximate (fast) mahalanobis distance (square):
311  double stdX2 = max(auxSOG_Kernel_i.cov(0, 0), m.val.cov(0, 0));
312  double mahaDist2 =
313  square(auxSOG_Kernel_i.mean.x() - m.val.mean.x()) / stdX2;
314 
315  double stdY2 = max(auxSOG_Kernel_i.cov(1, 1), m.val.cov(1, 1));
316  mahaDist2 +=
317  square(auxSOG_Kernel_i.mean.y() - m.val.mean.y()) / stdY2;
318 
319  if (!is2D)
320  {
321  double stdZ2 =
322  max(auxSOG_Kernel_i.cov(2, 2), m.val.cov(2, 2));
323  mahaDist2 +=
324  square(auxSOG_Kernel_i.mean.z() - m.val.mean.z()) /
325  stdZ2;
326  }
327 
328  reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
329  }
330 
331  if (reallyComputeThisOne)
332  {
333  auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, m.val);
334 
335  // ----------------------------------------------------------------------
336  // The new weight is given by:
337  //
338  // w'_i = w_i * exp( a + a_i - a' )
339  //
340  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1))
341  // + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
342  //
343  // ----------------------------------------------------------------------
344  TGaussianMode newKernel;
345 
346  newKernel.val = auxGaussianProduct; // Copy mean & cov
347 
348  CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
349  Eigen::Vector3d eta_i =
350  CMatrixDouble31(auxSOG_Kernel_i.mean).asEigen();
351  eta_i = covInv_i.asEigen() * eta_i;
352 
353  CMatrixDouble33 new_covInv_i = newKernel.val.cov.inverse_LLt();
354  Eigen::Vector3d new_eta_i =
355  CMatrixDouble31(newKernel.val.mean).asEigen();
356  new_eta_i = new_covInv_i.asEigen() * new_eta_i;
357 
358  double a_i =
359  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
360  (eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
361  eta_i)(0, 0));
362  double new_a_i =
363  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
364  (new_eta_i.transpose() *
365  newKernel.val.cov.asEigen() * new_eta_i)(0, 0));
366 
367  newKernel.log_w = m.log_w + m2.log_w + a + a_i - new_a_i;
368 
369  // Fix 2D case:
370  if (is2D) newKernel.val.cov(2, 2) = 0;
371 
372  // Add to the results (in "this") the new kernel:
373  this->m_modes.push_back(newKernel);
374  } // end if reallyComputeThisOne
375  } // end for it2
376 
377  } // end for it1
378 
379  normalizeWeights();
380 
381  MRPT_END
382 }
383 
384 /*---------------------------------------------------------------
385  enforceCovSymmetry
386  ---------------------------------------------------------------*/
388 {
389  MRPT_START
390  // Differences, when they exist, appear in the ~15'th significant
391  // digit, so... just take one of them arbitrarily!
392  for (auto& m_mode : m_modes)
393  {
394  m_mode.val.cov(0, 1) = m_mode.val.cov(1, 0);
395  m_mode.val.cov(0, 2) = m_mode.val.cov(2, 0);
396  m_mode.val.cov(1, 2) = m_mode.val.cov(2, 1);
397  }
398 
399  MRPT_END
400 }
401 
402 /*---------------------------------------------------------------
403  normalizeWeights
404  ---------------------------------------------------------------*/
406 {
407  MRPT_START
408 
409  if (!m_modes.size()) return;
410 
411  CListGaussianModes::iterator it;
412  double maxW = m_modes[0].log_w;
413  for (it = m_modes.begin(); it != m_modes.end(); ++it)
414  maxW = max(maxW, it->log_w);
415 
416  for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;
417 
418  MRPT_END
419 }
420 
421 /*---------------------------------------------------------------
422  ESS
423  ---------------------------------------------------------------*/
424 double CPointPDFSOG::ESS() const
425 {
426  MRPT_START
427  CListGaussianModes::const_iterator it;
428  double cum = 0;
429 
430  /* Sum of weights: */
431  double sumLinearWeights = 0;
432  for (it = m_modes.begin(); it != m_modes.end(); ++it)
433  sumLinearWeights += exp(it->log_w);
434 
435  /* Compute ESS: */
436  for (it = m_modes.begin(); it != m_modes.end(); ++it)
437  cum += square(exp(it->log_w) / sumLinearWeights);
438 
439  if (cum == 0)
440  return 0;
441  else
442  return 1.0 / (m_modes.size() * cum);
443  MRPT_END
444 }
445 
446 /*---------------------------------------------------------------
447  evaluatePDFInArea
448  ---------------------------------------------------------------*/
450  float x_min, float x_max, float y_min, float y_max, float resolutionXY,
451  float z, CMatrixD& outMatrix, bool sumOverAllZs)
452 {
453  MRPT_START
454 
455  ASSERT_(x_max > x_min);
456  ASSERT_(y_max > y_min);
457  ASSERT_(resolutionXY > 0);
458 
459  const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
460  const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
461  outMatrix.setSize(Ny, Nx);
462 
463  for (size_t i = 0; i < Ny; i++)
464  {
465  const float y = y_min + i * resolutionXY;
466  for (size_t j = 0; j < Nx; j++)
467  {
468  float x = x_min + j * resolutionXY;
469  outMatrix(i, j) = evaluatePDF(CPoint3D(x, y, z), sumOverAllZs);
470  }
471  }
472 
473  MRPT_END
474 }
475 
476 /*---------------------------------------------------------------
477  evaluatePDF
478  ---------------------------------------------------------------*/
479 double CPointPDFSOG::evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const
480 {
481  if (!sumOverAllZs)
482  {
483  // Normal evaluation:
484  auto X = CMatrixDouble31(x);
485  double ret = 0;
486 
487  CMatrixDouble31 MU;
488 
489  for (const auto& m_mode : m_modes)
490  {
491  MU = CMatrixDouble31(m_mode.val.mean);
492  ret += exp(m_mode.log_w) * math::normalPDF(X, MU, m_mode.val.cov);
493  }
494 
495  return ret;
496  }
497  else
498  {
499  // Only X,Y:
500  CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
501  double ret = 0;
502 
503  X(0, 0) = x.x();
504  X(1, 0) = x.y();
505 
506  for (const auto& m_mode : m_modes)
507  {
508  MU(0, 0) = m_mode.val.mean.x();
509  MU(1, 0) = m_mode.val.mean.y();
510 
511  COV(0, 0) = m_mode.val.cov(0, 0);
512  COV(1, 1) = m_mode.val.cov(1, 1);
513  COV(0, 1) = COV(1, 0) = m_mode.val.cov(0, 1);
514 
515  ret += exp(m_mode.log_w) * math::normalPDF(X, MU, COV);
516  }
517 
518  return ret;
519  }
520 }
521 
522 /*---------------------------------------------------------------
523  getMostLikelyMode
524  ---------------------------------------------------------------*/
526 {
527  if (this->empty())
528  {
529  outVal = CPointPDFGaussian();
530  }
531  else
532  {
533  auto it_best = m_modes.end();
534  for (auto it = m_modes.begin(); it != m_modes.end(); ++it)
535  if (it_best == m_modes.end() || it->log_w > it_best->log_w)
536  it_best = it;
537 
538  outVal = it_best->val;
539  }
540 }
541 
542 /*---------------------------------------------------------------
543  getAs3DObject
544  ---------------------------------------------------------------*/
545 // void CPointPDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjects::Ptr &outObj
546 // )
547 // const
548 //{
549 // // For each gaussian node
550 // for (CListGaussianModes::const_iterator it = m_modes.begin(); it!=
551 // m_modes.end();++it)
552 // {
553 // opengl::CEllipsoid3D::Ptr obj =
554 // std::make_shared<opengl::CEllipsoid3D>();
555 //
556 // obj->setPose( it->val.mean);
557 // obj->setCovMatrix(it->val.cov, it->val.cov(2,2)==0 ? 2:3);
558 //
559 // obj->setQuantiles(3);
560 // obj->enableDrawSolid3D(false);
561 // obj->setColor(1,0,0, 0.5);
562 //
563 // outObj->insert( obj );
564 // } // end for each gaussian node
565 //}
A namespace of pseudo-random numbers generators of diferent distributions.
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:241
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
#define M_2PI
Definition: common.h:58
void getMean(CPoint3D &mean_point) const override
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:282
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to 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:33
The struct for each mode:
Definition: CPointPDFSOG.h:40
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
void clear()
Clear all the gaussian modes.
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
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:102
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:372
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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:957
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Scalar det() const
Determinant of matrix.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
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...
A class used to store a 3D point.
Definition: CPoint3D.h:31
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:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool empty() const
Definition: ts_hash_map.h:191
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:415
return_t square(const num_t x)
Inline function for the square of a number.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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.
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
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...
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
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...
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:264
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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:36
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.
images resize(NUM_IMGS)
double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const
Evaluates the PDF at a given point.
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!)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 02a75dd25 Thu Jun 4 01:49:55 2020 +0200 at jue jun 4 02:00:12 CEST 2020