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



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