MRPT  1.9.9
CPosePDFSOG.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-2019, 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 
12 #include <mrpt/math/CMatrixD.h>
15 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/poses/CPosePDFSOG.h>
21 #include <mrpt/system/os.h>
22 #include <Eigen/Dense>
23 #include <iostream>
24 
25 using namespace mrpt;
26 using namespace mrpt::poses;
27 using namespace mrpt::math;
28 using namespace mrpt::system;
29 using namespace std;
30 
32 
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
36 CPosePDFSOG::CPosePDFSOG(size_t nModes) : m_modes(nModes) {}
37 /*---------------------------------------------------------------
38  clear
39  ---------------------------------------------------------------*/
40 void CPosePDFSOG::clear() { m_modes.clear(); }
41 /*---------------------------------------------------------------
42  Resize
43  ---------------------------------------------------------------*/
44 void CPosePDFSOG::resize(const size_t N) { m_modes.resize(N); }
45 /*---------------------------------------------------------------
46  getMean
47  Returns an estimate of the pose, (the mean, or mathematical expectation of the
48  PDF)
49  ---------------------------------------------------------------*/
51 {
52  if (!m_modes.empty())
53  {
54  mrpt::poses::SE_average<2> se_averager;
55  for (const auto& m : m_modes)
56  {
57  const double w = exp(m.log_w);
58  se_averager.append(m.mean, w);
59  }
60  se_averager.get_average(p);
61  }
62  else
63  {
64  p = CPose2D();
65  }
66 }
67 
68 std::tuple<CMatrixDouble33, CPose2D> CPosePDFSOG::getCovarianceAndMean() const
69 {
70  const size_t N = m_modes.size();
71 
73  CPose2D estMean2D;
74 
75  this->getMean(estMean2D);
76  cov.setZero();
77 
78  if (N)
79  {
80  // 1) Get the mean:
81  double sumW = 0;
82  auto estMeanMat = CMatrixDouble31(estMean2D);
83  CMatrixDouble33 temp;
84  CMatrixDouble31 estMean_i;
85 
86  for (const auto& m : m_modes)
87  {
88  double w;
89  sumW += w = exp(m.log_w);
90 
91  estMean_i = CMatrixDouble31(m.mean);
92  estMean_i -= estMeanMat;
93 
94  temp.matProductOf_AAt(estMean_i);
95  temp += m.cov;
96  temp *= w;
97 
98  cov += temp;
99  }
100 
101  if (sumW != 0) cov *= (1.0 / sumW);
102  }
103  return {cov, estMean2D};
104 }
105 
108 {
109  uint32_t N = m_modes.size();
110  out << N;
111 
112  for (const auto m : m_modes)
113  {
114  out << m.log_w << m.mean;
116  }
117 }
120 {
121  switch (version)
122  {
123  case 0:
124  case 1:
125  case 2:
126  {
127  uint32_t N;
128  in >> N;
129  resize(N);
130  for (auto& m : m_modes)
131  {
132  in >> m.log_w;
133 
134  // In version 0, weights were linear!!
135  if (version == 0) m.log_w = log(max(1e-300, m.log_w));
136 
137  in >> m.mean;
138 
139  if (version == 1) // float's
140  {
141  CMatrixFloat33 mf;
143  m.cov = mf.cast_double();
144  }
145  else
146  {
148  }
149  }
150  }
151  break;
152  default:
154  };
155 }
156 
158 {
159  MRPT_START
160 
161  if (this == &o) return; // It may be used sometimes
162 
164  {
165  m_modes = dynamic_cast<const CPosePDFSOG*>(&o)->m_modes;
166  }
167  else
168  {
169  // Approximate as a mono-modal gaussian pdf:
170  m_modes.resize(1);
171  m_modes[0].log_w = 0;
172  o.getMean(m_modes[0].mean);
173  o.getCovariance(m_modes[0].cov);
174  }
175 
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  saveToTextFile
181  ---------------------------------------------------------------*/
183 {
184  FILE* f = os::fopen(file.c_str(), "wt");
185  if (!f) return false;
186 
187  for (const auto& m : m_modes)
188  os::fprintf(
189  f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m.log_w), m.mean.x(),
190  m.mean.y(), m.mean.phi(), m.cov(0, 0), m.cov(1, 1), m.cov(2, 2),
191  m.cov(0, 1), m.cov(0, 2), m.cov(1, 2));
192  os::fclose(f);
193  return true;
194 }
195 
196 /*---------------------------------------------------------------
197  changeCoordinatesReference
198  ---------------------------------------------------------------*/
199 void CPosePDFSOG::changeCoordinatesReference(const CPose3D& newReferenceBase_)
200 {
201  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
202 
203  CMatrixDouble44 HM;
204  newReferenceBase.getHomogeneousMatrix(HM);
205 
206  // Clip the 4x4 matrix
207  auto M = CMatrixDouble33(HM.block<3, 3>(0, 0));
208 
209  // The variance in phi is unmodified:
210  M(0, 2) = 0;
211  M(1, 2) = 0;
212  M(2, 0) = 0;
213  M(2, 1) = 0;
214  M(2, 2) = 1;
215 
216  for (auto& m : m_modes)
217  {
218  // The mean:
219  m.mean.composeFrom(newReferenceBase, m.mean);
220 
221  // The covariance:
222  // NOTE: the CMatrixDouble33 is NEEDED to create a temporary copy to
223  // allow aliasing
224  m.cov = mrpt::math::multiply_HCHt(M, CMatrixDouble33(m.cov));
225  }
226 
227  enforceCovSymmetry();
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 (auto& m : m_modes)
242  m.cov = mrpt::math::multiply_HCHt(rot, CMatrixDouble33(m.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 auto* p1 = dynamic_cast<const CPosePDFSOG*>(&p1_);
290  const auto* p2 = dynamic_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  const CMatrixDouble33 covInv = p2->cov.inverse_LLt();
297 
298  auto eta = CMatrixDouble31(p2->mean);
299  eta = covInv * eta;
300 
301  // Normal distribution canonical form constant:
302  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
303  double a =
304  -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
305  (eta.transpose() * p2->cov.asEigen() * eta.asEigen())(0, 0));
306 
307  this->m_modes.clear();
308  for (const auto& m : p1->m_modes)
309  {
310  auxSOG_Kernel_i.mean = m.mean;
311  auxSOG_Kernel_i.cov = CMatrixDouble(m.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  const CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
328 
329  auto eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
330  eta_i = covInv_i * eta_i;
331 
332  const CMatrixDouble33 new_covInv_i = newKernel.cov.inverse_LLt();
333 
334  auto new_eta_i = CMatrixDouble31(newKernel.mean);
335  new_eta_i = new_covInv_i * new_eta_i;
336 
337  double a_i =
338  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
339  (eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
340  eta_i.asEigen())(0, 0));
341  double new_a_i =
342  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
343  (new_eta_i.transpose() * newKernel.cov.asEigen() *
344  new_eta_i.asEigen())(0, 0));
345 
346  // newKernel.w = (it)->w * exp( a + a_i - new_a_i );
347  newKernel.log_w = m.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  auto* out = dynamic_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 (auto& m : m_modes) m.mean = m.mean + Ap;
388 
389  this->rotateAllCovariances(Ap.phi());
390 }
391 
392 /*---------------------------------------------------------------
393  evaluatePDF
394  ---------------------------------------------------------------*/
395 double CPosePDFSOG::evaluatePDF(const CPose2D& x, bool sumOverAllPhis) const
396 {
397  if (!sumOverAllPhis)
398  {
399  // Normal evaluation:
400  auto X = CMatrixDouble31(x);
401  CMatrixDouble31 MU;
402  double ret = 0;
403 
404  for (const auto& m : m_modes)
405  {
406  MU = CMatrixDouble31(m.mean);
407  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov);
408  }
409 
410  return ret;
411  }
412  else
413  {
414  // Only X,Y:
415  CMatrixDouble X(2, 1), MU(2, 1), COV(2, 2);
416  double ret = 0;
417 
418  X(0, 0) = x.x();
419  X(1, 0) = x.y();
420 
421  for (const auto& m : m_modes)
422  {
423  MU(0, 0) = m.mean.x();
424  MU(1, 0) = m.mean.y();
425 
426  COV(0, 0) = m.cov(0, 0);
427  COV(1, 1) = m.cov(1, 1);
428  COV(0, 1) = COV(1, 0) = m.cov(0, 1);
429 
430  ret += exp(m.log_w) * math::normalPDF(X, MU, COV);
431  }
432 
433  return ret;
434  }
435 }
436 
437 /*---------------------------------------------------------------
438  evaluateNormalizedPDF
439  ---------------------------------------------------------------*/
441 {
442  auto X = CMatrixDouble31(x);
443  CMatrixDouble31 MU;
444  double ret = 0;
445 
446  for (const auto& m : m_modes)
447  {
448  MU = CMatrixDouble31(m.mean);
449  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov) /
450  math::normalPDF(MU, MU, m.cov);
451  }
452 
453  return ret;
454 }
455 
456 /*---------------------------------------------------------------
457  enforceCovSymmetry
458  ---------------------------------------------------------------*/
460 {
461  // Differences, when they exist, appear in the ~15'th significant
462  // digit, so... just take one of them arbitrarily!
463  for (auto& m : m_modes)
464  {
465  m.cov(0, 1) = m.cov(1, 0);
466  m.cov(0, 2) = m.cov(2, 0);
467  m.cov(1, 2) = m.cov(2, 1);
468  }
469 }
470 
471 /*---------------------------------------------------------------
472  normalizeWeights
473  ---------------------------------------------------------------*/
475 {
476  MRPT_START
477 
478  if (!m_modes.size()) return;
479 
480  double maxW = m_modes[0].log_w;
481  for (auto& m : m_modes) maxW = max(maxW, m.log_w);
482 
483  for (auto& m : m_modes) m.log_w -= maxW;
484 
485  MRPT_END
486 }
487 
488 /*---------------------------------------------------------------
489  normalizeWeights
490  ---------------------------------------------------------------*/
492  const double& x_min, const double& x_max, const double& y_min,
493  const double& y_max, const double& resolutionXY, const double& phi,
494  CMatrixDouble& outMatrix, bool sumOverAllPhis)
495 {
496  MRPT_START
497 
498  ASSERT_(x_max > x_min);
499  ASSERT_(y_max > y_min);
500  ASSERT_(resolutionXY > 0);
501 
502  const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
503  const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
504 
505  outMatrix.setSize(Ny, Nx);
506 
507  for (size_t i = 0; i < Ny; i++)
508  {
509  double y = y_min + i * resolutionXY;
510  for (size_t j = 0; j < Nx; j++)
511  {
512  double x = x_min + j * resolutionXY;
513  outMatrix(i, j) = evaluatePDF(CPose2D(x, y, phi), sumOverAllPhis);
514  }
515  }
516 
517  MRPT_END
518 }
519 
520 /*---------------------------------------------------------------
521  mergeModes
522  ---------------------------------------------------------------*/
523 void CPosePDFSOG::mergeModes(double max_KLd, bool verbose)
524 {
525  MRPT_START
526 
527  normalizeWeights();
528 
529  size_t N = m_modes.size();
530  if (N < 2) return; // Nothing to do
531 
532  // Method described in:
533  // "Kullback-Leibler Approach to Gaussian Mixture Reduction", A.R. Runnalls.
534  // IEEE Transactions on Aerospace and Electronic Systems, 2007.
535  // See Eq.(21) for Bij !!
536 
537  for (size_t i = 0; i < (N - 1);)
538  {
539  N = m_modes.size(); // It might have changed.
540  double sumW = 0;
541 
542  // For getting linear weights:
543  sumW = 0;
544  for (size_t j = 0; j < N; j++) sumW += exp(m_modes[j].log_w);
545  ASSERT_(sumW);
546 
547  const double Wi = exp(m_modes[i].log_w) / sumW;
548 
549  double min_Bij = std::numeric_limits<double>::max();
550 
551  CMatrixDouble33 min_Bij_COV;
552  size_t best_j = 0;
553 
554  auto MUi = CMatrixDouble31(m_modes[i].mean);
555 
556  // Compute B(i,j), j=[i+1,N-1] (the discriminant)
557  for (size_t j = 0; j < N; j++)
558  if (i != j)
559  {
560  const double Wj = exp(m_modes[j].log_w) / sumW;
561  const double Wij_ = 1.0 / (Wi + Wj);
562 
563  auto Pij =
564  CMatrixDouble33(m_modes[i].cov.asEigen() * Wi * Wij_);
565  Pij.asEigen() += m_modes[j].cov.asEigen() * Wj * Wij_;
566 
567  auto MUij = CMatrixDouble31(m_modes[j].mean);
568  MUij -= MUi;
569  // Account for circular dimensions:
570  mrpt::math::wrapToPiInPlace(MUij(2, 0));
571 
572  CMatrixDouble33 AUX;
573  AUX.matProductOf_AAt(MUij); // AUX = MUij * MUij^T
574 
575  AUX *= Wi * Wj * Wij_ * Wij_;
576  Pij += AUX;
577 
578  double Bij = (Wi + Wj) * log(Pij.det()) -
579  Wi * log(m_modes[i].cov.det()) -
580  Wj * log(m_modes[j].cov.det());
581  if (verbose)
582  {
583  cout << "try merge[" << i << ", " << j
584  << "] -> Bij: " << Bij << endl;
585  // cout << "AUX: " << endl << AUX;
586  // cout << "Wi: " << Wi << " Wj:" << Wj << " Wij_: " << Wij_
587  // << endl;
588  cout << "Pij: " << Pij << endl
589  << " Pi: " << m_modes[i].cov << endl
590  << " Pj: " << m_modes[j].cov << endl;
591  }
592 
593  if (Bij < min_Bij)
594  {
595  min_Bij = Bij;
596  best_j = j;
597  min_Bij_COV = Pij;
598  }
599  }
600 
601  // Is a good move to merge (i,j)??
602  if (verbose)
603  cout << "merge[" << i << ", " << best_j
604  << "] Tempting merge: KLd = " << min_Bij;
605 
606  if (min_Bij < max_KLd)
607  {
608  if (verbose) cout << " Accepted." << endl;
609 
610  // Do the merge (i,j):
611  TGaussianMode Mij;
612  TGaussianMode& Mi = m_modes[i];
613  TGaussianMode& Mj = m_modes[best_j];
614 
615  // Weight:
616  Mij.log_w = log(exp(Mi.log_w) + exp(Mj.log_w));
617 
618  // Mean:
619  const double Wj = exp(Mj.log_w) / sumW;
620  const double Wij_ = 1.0 / (Wi + Wj);
621  const double Wi_ = Wi * Wij_;
622  const double Wj_ = Wj * Wij_;
623 
624  Mij.mean = CPose2D(
625  Wi_ * Mi.mean.x() + Wj_ * Mj.mean.x(),
626  Wi_ * Mi.mean.y() + Wj_ * Mj.mean.y(),
627  Wi_ * Mi.mean.phi() + Wj_ * Mj.mean.phi());
628 
629  // Cov:
630  Mij.cov = min_Bij_COV;
631 
632  // Replace Mi <- Mij:
633  m_modes[i] = Mij;
634  m_modes.erase(m_modes.begin() + best_j); // erase Mj
635  } // end merge
636  else
637  {
638  if (verbose) cout << " Nope." << endl;
639 
640  i++;
641  }
642  } // for i
643 
644  normalizeWeights();
645 
646  MRPT_END
647 }
648 
649 /*---------------------------------------------------------------
650  getMostLikelyCovarianceAndMean
651  ---------------------------------------------------------------*/
653  CMatrixDouble33& cov, CPose2D& mean_point) const
654 {
655  auto it_best = end();
656  double best_log_w = -std::numeric_limits<double>::max();
657 
658  for (auto i = begin(); i != end(); ++i)
659  {
660  if (i->log_w > best_log_w)
661  {
662  best_log_w = i->log_w;
663  it_best = i;
664  }
665  }
666 
667  if (it_best != end())
668  {
669  mean_point = it_best->mean;
670  cov = it_best->cov;
671  }
672  else
673  {
674  cov.setIdentity();
675  cov.asEigen() *= 1e20;
676  mean_point = CPose2D(0, 0, 0);
677  }
678 }
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.
void clear()
Clear the list of modes.
Definition: CPosePDFSOG.cpp:40
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:241
CPose2D mean
The mean value.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
The struct for each mode:
Definition: CPosePDFSOG.h:41
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:66
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to 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.
void getMean(CPose2D &mean_pose) const override
Definition: CPosePDFSOG.cpp:50
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...
void enforceCovSymmetry()
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
STL namespace.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
virtual void getMean(type_value &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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:4199
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
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...
Definition: CPosePDFSOG.cpp:68
unsigned char uint8_t
Definition: rptypes.h:44
#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
CListGaussianModes::iterator iterator
Definition: CPosePDFSOG.h:67
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
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.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:89
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:357
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
GLuint GLuint end
Definition: glext.h:3532
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:45
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
Scalar det() const
Determinant of matrix.
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.
CMatrixDouble 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
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
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...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:229
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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:86
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:317
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
GLuint in
Definition: glext.h:7391
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
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:3542
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
GLenum GLint x
Definition: glext.h:3542
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:50
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:34
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
CMatrixFixed< double, ROWS, COLS > cast_double() const
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CPosePDFSOG.cpp:44
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CListGaussianModes m_modes
The list of SOG modes.
Definition: CPosePDFSOG.h:77
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average 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::CMatrixDouble &outMatrix, bool sumOverAllPhis=false)
Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matri...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019