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