Main MRPT website > C++ reference for MRPT 1.5.7
CPosePDFSOG.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 #include <mrpt/poses/CPosePDFSOG.h>
15 #include <mrpt/system/os.h>
17 #include <mrpt/math/CMatrixD.h>
18 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/utils/CStream.h>
21 
22 
23 using namespace mrpt;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::utils;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
32 
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
36 CPosePDFSOG::CPosePDFSOG( size_t nModes ) : m_modes(nModes)
37 {
38 }
39 
40 /*---------------------------------------------------------------
41  clear
42  ---------------------------------------------------------------*/
44 {
45  m_modes.clear();
46 }
47 
48 /*---------------------------------------------------------------
49  Resize
50  ---------------------------------------------------------------*/
51 void CPosePDFSOG::resize(const size_t N)
52 {
53  m_modes.resize(N);
54 }
55 
56 
57 /*---------------------------------------------------------------
58  getMean
59  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
60  ---------------------------------------------------------------*/
62 {
63  if (!m_modes.empty())
64  {
65  mrpt::poses::SE_average<2> se_averager;
66  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
67  {
68  const double w = exp((it)->log_w);
69  se_averager.append( (it)->mean, w);
70  }
71  se_averager.get_average(p);
72  }
73  else {
74  p = CPose2D();
75  }
76 }
77 
78 /*---------------------------------------------------------------
79  getEstimatedCovariance
80  ---------------------------------------------------------------*/
82 {
83  size_t N = m_modes.size();
84 
85  this->getMean(estMean2D);
86  estCov.zeros();
87 
88  if (N)
89  {
90  // 1) Get the mean:
91  double sumW = 0;
92  CMatrixDouble31 estMeanMat = CMatrixDouble31(estMean2D);
93  CMatrixDouble33 temp;
94  CMatrixDouble31 estMean_i;
95 
96  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
97  {
98  double w;
99  sumW += w = exp((it)->log_w);
100 
101  estMean_i = CMatrixDouble31( (it)->mean );
102  estMean_i -= estMeanMat;
103 
104  temp.multiply_AAt(estMean_i);
105  temp+= (it)->cov;
106  temp*=w;
107 
108  estCov += temp;
109  }
110 
111  if (sumW!=0)
112  estCov *= (1.0/sumW);
113  }
114 }
115 
116 /*---------------------------------------------------------------
117  writeToStream
118  ---------------------------------------------------------------*/
120 {
121  if (version)
122  *version = 2;
123  else
124  {
125  uint32_t N = m_modes.size();
126  out << N;
127 
128  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
129  {
130  out << (it)->log_w;
131  out << (it)->mean;
132  out << (it)->cov(0,0) << (it)->cov(1,1) << (it)->cov(2,2);
133  out << (it)->cov(0,1) << (it)->cov(0,2) << (it)->cov(1,2);
134  }
135  }
136 }
137 
138 /*---------------------------------------------------------------
139  readFromStream
140  ---------------------------------------------------------------*/
142 {
143  switch(version)
144  {
145  case 0:
146  case 1:
147  case 2:
148  {
149  uint32_t N;
150  float x;
151  double x0;
152 
153  in >> N;
154 
155  resize(N);
156 
157  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
158  {
159  in >> (it)->log_w;
160 
161  // In version 0, weights were linear!!
162  if (version==0) (it)->log_w = log(max(1e-300,(it)->log_w));
163 
164  in >> (it)->mean;
165 
166  if (version==1) // float's
167  {
168  in >> x; (it)->cov(0,0) = x;
169  in >> x; (it)->cov(1,1) = x;
170  in >> x; (it)->cov(2,2) = x;
171 
172  in >> x; (it)->cov(1,0) = x; (it)->cov(0,1) = x;
173  in >> x; (it)->cov(2,0) = x; (it)->cov(0,2) = x;
174  in >> x; (it)->cov(1,2) = x; (it)->cov(2,1) = x;
175  }
176  else
177  {
178  in >> x0; (it)->cov(0,0) = x0;
179  in >> x0; (it)->cov(1,1) = x0;
180  in >> x0; (it)->cov(2,2) = x0;
181 
182  in >> x0; (it)->cov(1,0) = x0; (it)->cov(0,1) = x0;
183  in >> x0; (it)->cov(2,0) = x0; (it)->cov(0,2) = x0;
184  in >> x0; (it)->cov(1,2) = x0; (it)->cov(2,1) = x0;
185  }
186 
187  }
188 
189  } break;
190  default:
192 
193  };
194 }
195 
197 {
198  MRPT_START
199 
200  if (this == &o) return; // It may be used sometimes
201 
203  {
204  m_modes = static_cast<const CPosePDFSOG*>(&o)->m_modes;
205  }
206  else
207  {
208  // Approximate as a mono-modal gaussian pdf:
209  m_modes.resize(1);
210  m_modes[0].log_w = 0;
211  o.getMean( m_modes[0].mean );
212  o.getCovariance( m_modes[0].cov );
213  }
214 
215  MRPT_END
216 }
217 
218 /*---------------------------------------------------------------
219  saveToTextFile
220  ---------------------------------------------------------------*/
222 {
223  FILE *f=os::fopen(file.c_str(),"wt");
224  if (!f) return;
225 
226 
227  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
228  os::fprintf(f,"%e %e %e %e %e %e %e %e %e %e\n",
229  exp((it)->log_w),
230  (it)->mean.x(), (it)->mean.y(), (it)->mean.phi(),
231  (it)->cov(0,0),(it)->cov(1,1),(it)->cov(2,2),
232  (it)->cov(0,1),(it)->cov(0,2),(it)->cov(1,2) );
233  os::fclose(f);
234 }
235 
236 /*---------------------------------------------------------------
237  changeCoordinatesReference
238  ---------------------------------------------------------------*/
239 void CPosePDFSOG::changeCoordinatesReference(const CPose3D &newReferenceBase_ )
240 {
241  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
242 
243  CMatrixDouble44 HM;
244  newReferenceBase.getHomogeneousMatrix(HM);
245 
246  // Clip the 4x4 matrix
247  CMatrixDouble33 M = HM.block(0,0, 3,3).eval();
248 
249  // The variance in phi is unmodified:
250  M(0,2) = 0; M(1,2) = 0;
251  M(2,0) = 0; M(2,1) = 0;
252  M(2,2) = 1;
253 
254  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
255  {
256  // The mean:
257  (it)->mean.composeFrom(newReferenceBase, (it)->mean);
258 
259  // The covariance:
260  // NOTE: The CMatrixDouble33() is NEEDED to create a temporary copy of (it)->cov
261  M.multiply_HCHt( CMatrixDouble33((it)->cov), (it)->cov ); // * (it)->cov * (~M);
262  }
263 
264  assureSymmetry();
265 }
266 
267 /*---------------------------------------------------------------
268  rotateAllCovariances
269  ---------------------------------------------------------------*/
270 void CPosePDFSOG::rotateAllCovariances(const double & ang)
271 {
272  CMatrixDouble33 rot;
273  rot(0,0)=rot(1,1)=cos(ang);
274  rot(0,1)=-sin(ang);
275  rot(1,0)=sin(ang);
276  rot(2,2)=1;
277 
278  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
279  rot.multiply_HCHt( CMatrixDouble33((it)->cov), (it)->cov );
280 }
281 
282 /*---------------------------------------------------------------
283  drawSingleSample
284  ---------------------------------------------------------------*/
286 {
287  MRPT_START
288  MRPT_UNUSED_PARAM(outPart);
289 
290  THROW_EXCEPTION("Not implemented yet!!");
291 
292  MRPT_END
293 }
294 
295 /*---------------------------------------------------------------
296  drawManySamples
297  ---------------------------------------------------------------*/
299  size_t N,
300  std::vector<CVectorDouble> &outSamples ) const
301 {
302  MRPT_START
304  MRPT_UNUSED_PARAM(outSamples);
305 
306  THROW_EXCEPTION("Not implemented yet!!");
307 
308  MRPT_END
309 }
310 
311 
312 /*---------------------------------------------------------------
313  bayesianFusion
314  ---------------------------------------------------------------*/
315 void CPosePDFSOG::bayesianFusion(const CPosePDF &p1_,const CPosePDF &p2_, const double &minMahalanobisDistToDrop )
316 {
317  MRPT_START
318 
319  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
320 
321  // p1: CPosePDFSOG, p2: CPosePDFGaussian:
322 
325 
326  const CPosePDFSOG *p1 = static_cast<const CPosePDFSOG*>(&p1_);
327  const CPosePDFGaussian *p2 = static_cast<const CPosePDFGaussian*>(&p2_);
328 
329  // Compute the new kernel means, covariances, and weights after multiplying to the Gaussian "p2":
330  CPosePDFGaussian auxGaussianProduct,auxSOG_Kernel_i;
331 
332  CMatrixDouble33 covInv;
333  p2->cov.inv(covInv);
334 
336  eta = covInv * eta;
337 
338  // Normal distribution canonical form constant:
339  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
340  double a = -0.5*( 3*log(M_2PI) - log( covInv.det() ) + (eta.adjoint() * p2->cov * eta)(0,0) );
341 
342  this->m_modes.clear();
343  for (const_iterator it =p1->m_modes.begin();it!=p1->m_modes.end();++it)
344  {
345  auxSOG_Kernel_i.mean = (it)->mean;
346  auxSOG_Kernel_i.cov = CMatrixDouble( (it)->cov );
347  auxGaussianProduct.bayesianFusion( auxSOG_Kernel_i, *p2 );
348 
349  // ----------------------------------------------------------------------
350  // The new weight is given by:
351  //
352  // w'_i = w_i * exp( a + a_i - a' )
353  //
354  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1)) + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
355  //
356  // ----------------------------------------------------------------------
357  TGaussianMode newKernel;
358  newKernel.mean = auxGaussianProduct.mean;
359  newKernel.cov = auxGaussianProduct.cov;
360 
361  CMatrixDouble33 covInv_i;
362  auxSOG_Kernel_i.cov.inv(covInv_i);
363 
364  CMatrixDouble31 eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
365  eta_i = covInv_i * eta_i;
366 
367  CMatrixDouble33 new_covInv_i;
368  newKernel.cov.inv(new_covInv_i);
369 
370  CMatrixDouble31 new_eta_i = CMatrixDouble31(newKernel.mean);
371  new_eta_i = new_covInv_i * new_eta_i;
372 
373  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) );
374  double new_a_i = -0.5*( 3*log(M_2PI) - log( new_covInv_i.det() ) + (new_eta_i.adjoint() * newKernel.cov * new_eta_i)(0,0) );
375 
376  //newKernel.w = (it)->w * exp( a + a_i - new_a_i );
377  newKernel.log_w = (it)->log_w + a + a_i - new_a_i;
378 
379  // Add to the results (in "this") the new kernel:
380  this->m_modes.push_back(newKernel );
381  }
382 
383  normalizeWeights();
384 
385  MRPT_END
386 }
387 
388 /*---------------------------------------------------------------
389  inverse
390  ---------------------------------------------------------------*/
392 {
394  CPosePDFSOG *out = static_cast<CPosePDFSOG*>( &o );
395 
396  const_iterator itSrc;
397  iterator itDest;
398 
399  out->m_modes.resize(m_modes.size());
400 
401  for (itSrc=m_modes.begin(),itDest=out->m_modes.begin();itSrc!=m_modes.end();++itSrc,++itDest)
402  {
403  // The mean:
404  (itDest)->mean = -(itSrc)->mean;
405 
406  // The covariance: Is the same:
407  (itDest)->cov = (itSrc)->cov;
408  }
409 }
410 
411 
412 /*---------------------------------------------------------------
413  +=
414  ---------------------------------------------------------------*/
416 {
417  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
418  (it)->mean = (it)->mean + Ap;
419 
420  this->rotateAllCovariances( Ap.phi() );
421 }
422 
423 /*---------------------------------------------------------------
424  evaluatePDF
425  ---------------------------------------------------------------*/
427  const CPose2D &x,
428  bool sumOverAllPhis ) const
429 {
430  if (!sumOverAllPhis)
431  {
432  // Normal evaluation:
434  CMatrixDouble31 MU;
435  double ret = 0;
436 
437  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
438  {
439  MU = CMatrixDouble31((it)->mean);
440  ret+= exp((it)->log_w) * math::normalPDF( X, MU, (it)->cov );
441  }
442 
443  return ret;
444  }
445  else
446  {
447  // Only X,Y:
448  CMatrixDouble X(2,1), MU(2,1),COV(2,2);
449  double ret = 0;
450 
451  X(0,0) = x.x();
452  X(1,0) = x.y();
453 
454  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
455  {
456  MU(0,0) = (it)->mean.x();
457  MU(1,0) = (it)->mean.y();
458 
459  COV(0,0) = (it)->cov(0,0);
460  COV(1,1) = (it)->cov(1,1);
461  COV(0,1) = COV(1,0) = (it)->cov(0,1);
462 
463  ret+= exp((it)->log_w) * math::normalPDF( X, MU, COV );
464  }
465 
466  return ret;
467  }
468 }
469 
470 /*---------------------------------------------------------------
471  evaluateNormalizedPDF
472  ---------------------------------------------------------------*/
474 {
476  CMatrixDouble31 MU;
477  double ret = 0;
478 
479  for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
480  {
481  MU = CMatrixDouble31((it)->mean);
482  ret+= exp((it)->log_w) * math::normalPDF( X, MU, (it)->cov ) / math::normalPDF( MU, MU, (it)->cov );
483  }
484 
485  return ret;
486 }
487 
488 /*---------------------------------------------------------------
489  assureSymmetry
490  ---------------------------------------------------------------*/
492 {
493  // Differences, when they exist, appear in the ~15'th significant
494  // digit, so... just take one of them arbitrarily!
495  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
496  {
497  (it)->cov(0,1) = (it)->cov(1,0);
498  (it)->cov(0,2) = (it)->cov(2,0);
499  (it)->cov(1,2) = (it)->cov(2,1);
500  }
501 }
502 
503 /*---------------------------------------------------------------
504  normalizeWeights
505  ---------------------------------------------------------------*/
507 {
508  MRPT_START
509 
510  if (!m_modes.size()) return;
511 
512  double maxW = m_modes[0].log_w;
513  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
514  maxW = max(maxW,(it)->log_w);
515 
516  for (iterator it=m_modes.begin();it!=m_modes.end();++it)
517  (it)->log_w -= maxW;
518 
519  MRPT_END
520 }
521 
522 /*---------------------------------------------------------------
523  normalizeWeights
524  ---------------------------------------------------------------*/
526  const double & x_min,
527  const double & x_max,
528  const double & y_min,
529  const double & y_max,
530  const double & resolutionXY,
531  const double & phi,
532  CMatrixD &outMatrix,
533  bool sumOverAllPhis )
534 {
535  MRPT_START
536 
537  ASSERT_(x_max>x_min);
538  ASSERT_(y_max>y_min);
539  ASSERT_(resolutionXY>0);
540 
541  const size_t Nx = (size_t)ceil((x_max-x_min)/resolutionXY);
542  const size_t Ny = (size_t)ceil((y_max-y_min)/resolutionXY);
543 
544  outMatrix.setSize(Ny,Nx);
545 
546  for (size_t i=0;i<Ny;i++)
547  {
548  double y = y_min + i*resolutionXY;
549  for (size_t j=0;j<Nx;j++)
550  {
551  double x = x_min + j*resolutionXY;
552  outMatrix(i,j) = evaluatePDF(CPose2D(x,y,phi),sumOverAllPhis);
553  }
554  }
555 
556 
557  MRPT_END
558 }
559 
560 /*---------------------------------------------------------------
561  mergeModes
562  ---------------------------------------------------------------*/
563 void CPosePDFSOG::mergeModes( double max_KLd, bool verbose )
564 {
565  MRPT_START
566 
567  normalizeWeights();
568 
569  size_t N = m_modes.size();
570  if (N<2) return; // Nothing to do
571 
572  // Method described in:
573  // "Kullback-Leibler Approach to Gaussian Mixture Reduction", A.R. Runnalls.
574  // IEEE Transactions on Aerospace and Electronic Systems, 2007.
575  // See Eq.(21) for Bij !!
576 
577  for (size_t i=0;i<(N-1); )
578  {
579  N = m_modes.size(); // It might have changed.
580  double sumW=0;
581 
582  // For getting linear weights:
583  sumW = 0;
584  for (size_t j=0;j<N;j++)
585  sumW += exp(m_modes[j].log_w);
586  ASSERT_(sumW);
587 
588  const double Wi = exp(m_modes[i].log_w) / sumW;
589 
590  double min_Bij = std::numeric_limits<double>::max();
591 
592  CMatrixDouble33 min_Bij_COV;
593  size_t best_j = 0;
594 
595  CMatrixDouble31 MUi = CMatrixDouble31(m_modes[i].mean);
596 
597  // Compute B(i,j), j=[i+1,N-1] (the discriminant)
598  for (size_t j=0;j<N;j++)
599  if (i!=j)
600  {
601  const double Wj = exp(m_modes[j].log_w) / sumW;
602  const double Wij_ = 1.0/(Wi+Wj);
603 
604  CMatrixDouble33 Pij = m_modes[i].cov * (Wi*Wij_);
605  Pij.add_Ac(m_modes[j].cov, Wj*Wij_ );
606 
607  CMatrixDouble31 MUij = CMatrixDouble31(m_modes[j].mean);
608  MUij-=MUi;
609  // Account for circular dimensions:
610  mrpt::math::wrapToPiInPlace( MUij(2,0) );
611 
612  CMatrixDouble33 AUX;
613  AUX.multiply_AAt( MUij ); // AUX = MUij * MUij^T
614 
615  AUX *= Wi*Wj*Wij_*Wij_;
616  Pij += AUX;
617 
618  double Bij = (Wi+Wj)*log( Pij.det() ) - Wi*log(m_modes[i].cov.det()) - Wj*log(m_modes[j].cov.det());
619  if (verbose)
620  {
621  cout << "try merge[" << i << ", " << j << "] -> Bij: " << Bij << endl;
622  //cout << "AUX: " << endl << AUX;
623  //cout << "Wi: " << Wi << " Wj:" << Wj << " Wij_: " << Wij_ << endl;
624  cout << "Pij: " << Pij << endl << " Pi: " << m_modes[i].cov << endl << " Pj: " << m_modes[j].cov << endl;
625  }
626 
627  if (Bij<min_Bij)
628  {
629  min_Bij = Bij;
630  best_j = j;
631  min_Bij_COV = Pij;
632  }
633  }
634 
635  // Is a good move to merge (i,j)??
636  if (verbose)
637  cout << "merge[" << i << ", " << best_j << "] Tempting merge: KLd = " << min_Bij;
638 
639  if (min_Bij<max_KLd)
640  {
641  if (verbose)
642  cout << " Accepted." << endl;
643 
644  // Do the merge (i,j):
645  TGaussianMode Mij;
646  TGaussianMode &Mi = m_modes[i];
647  TGaussianMode &Mj = m_modes[best_j];
648 
649  // Weight:
650  Mij.log_w = log( exp(Mi.log_w) + exp(Mj.log_w) );
651 
652  // Mean:
653  const double Wj = exp(Mj.log_w) / sumW;
654  const double Wij_ = 1.0/(Wi+Wj);
655  const double Wi_ = Wi*Wij_;
656  const double Wj_ = Wj*Wij_;
657 
658  Mij.mean = CPose2D(
659  Wi_ * Mi.mean.x() + Wj_ * Mj.mean.x(),
660  Wi_ * Mi.mean.y() + Wj_ * Mj.mean.y(),
661  Wi_ * Mi.mean.phi() + Wj_ * Mj.mean.phi() );
662 
663  // Cov:
664  Mij.cov = min_Bij_COV;
665 
666  // Replace Mi <- Mij:
667  m_modes[i] = Mij;
668  m_modes.erase( m_modes.begin() + best_j ); // erase Mj
669  } // end merge
670  else
671  {
672  if (verbose)
673  cout << " Nope." << endl;
674 
675  i++;
676  }
677  } // for i
678 
679  normalizeWeights();
680 
681  MRPT_END
682 }
683 
684 /*---------------------------------------------------------------
685  getMostLikelyCovarianceAndMean
686  ---------------------------------------------------------------*/
688 {
689  const_iterator it_best = end();
690  double best_log_w = -std::numeric_limits<double>::max();
691 
692  for (const_iterator i=begin();i!=end();++i)
693  {
694  if (i->log_w>best_log_w)
695  {
696  best_log_w = i->log_w;
697  it_best = i;
698  }
699  }
700 
701  if (it_best!=end())
702  {
703  mean_point = it_best->mean;
704  cov = it_best->cov;
705  }
706  else
707  {
708  cov.unit(3,1.0);
709  cov*=1e20;
710  mean_point = CPose2D(0,0,0);
711  }
712 }
713 
714 
715 /*---------------------------------------------------------------
716  getAs3DObject
717  ---------------------------------------------------------------*/
718 //void CPosePDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
719 //{
720 // outObj->clear();
721 //
722 // for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
723 // {
724 // opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
725 //
726 // ellip->setPose( CPose3D((it)->mean.x(), (it)->mean.y(), (it)->mean.phi()) );
727 // ellip->setCovMatrix((it)->cov);
728 // ellip->setColor(0,0,1,0.6);
729 //
730 // outObj->insert(ellip);
731 // }
732 //
733 //}
Computes weighted and un-weighted averages of SE(2) poses.
Definition: SO_SE_average.h:88
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
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
void clear()
Clear the list of modes.
Definition: CPosePDFSOG.cpp:43
void mergeModes(double max_KLd=0.5, bool verbose=false)
Merge very close modes so the overall number of modes is reduced while preserving the total distribut...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:53
The struct for each mode:
Definition: CPosePDFSOG.h:45
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
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.
double evaluatePDF(const mrpt::poses::CPose2D &x, bool sumOverAllPhis=false) const
Evaluates the PDF at a given point.
#define THROW_EXCEPTION(msg)
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
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 getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
STL namespace.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_OVERRIDE
Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF) ...
Definition: CPosePDFSOG.cpp:61
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void inverse(CPosePDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void rotateAllCovariances(const double &ang)
Rotate all the covariance matrixes by replacing them by , where .
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
#define M_2PI
Definition: mrpt_macros.h:380
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
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.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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:135
void evaluatePDFInArea(const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &resolutionXY, const double &phi, mrpt::math::CMatrixD &outMatrix, bool sumOverAllPhis=false)
Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matri...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
GLuint GLuint end
Definition: glext.h:3512
mrpt::math::CMatrixDouble33 cov
Definition: CPosePDFSOG.h:54
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...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
int version
Definition: mrpt_jpeglib.h:898
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLsizei const GLchar ** string
Definition: glext.h:3919
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
double evaluateNormalizedPDF(const mrpt::poses::CPose2D &x) const
Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0...
void assureSymmetry()
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getMostLikelyCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const
For the most likely Gaussian mode in the SOG, returns the pose covariance matrix (3x3 cov matrix) and...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose2D.cpp:272
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
Definition: CPosePDFSOG.cpp:81
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
CListGaussianModes::iterator iterator
Definition: CPosePDFSOG.h:73
GLenum GLint x
Definition: glext.h:3516
void operator+=(const mrpt::poses::CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
unsigned __int32 uint32_t
Definition: rptypes.h:49
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 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"...
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:72
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CPosePDFSOG.cpp:51
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
CListGaussianModes m_modes
The list of SOG modes.
Definition: CPosePDFSOG.h:80
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.



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