MRPT  2.0.5
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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 //
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 
23 #include <Eigen/Dense>
24 #include <iostream>
25 
26 
27 using namespace mrpt;
28 using namespace mrpt::poses;
29 using namespace mrpt::math;
30 using namespace mrpt::system;
31 using namespace std;
32 
34 
35 /*---------------------------------------------------------------
36  Constructor
37  ---------------------------------------------------------------*/
38 CPosePDFSOG::CPosePDFSOG(size_t nModes) : m_modes(nModes) {}
39 /*---------------------------------------------------------------
40  clear
41  ---------------------------------------------------------------*/
42 void CPosePDFSOG::clear() { m_modes.clear(); }
43 /*---------------------------------------------------------------
44  Resize
45  ---------------------------------------------------------------*/
46 void CPosePDFSOG::resize(const size_t N) { m_modes.resize(N); }
47 /*---------------------------------------------------------------
48  getMean
49  Returns an estimate of the pose, (the mean, or mathematical expectation of the
50  PDF)
51  ---------------------------------------------------------------*/
53 {
54  if (!m_modes.empty())
55  {
56  mrpt::poses::SE_average<2> se_averager;
57  for (const auto& m : m_modes)
58  {
59  const double w = exp(m.log_w);
60  se_averager.append(m.mean, w);
61  }
62  se_averager.get_average(p);
63  }
64  else
65  {
66  p = CPose2D();
67  }
68 }
69 
70 std::tuple<CMatrixDouble33, CPose2D> CPosePDFSOG::getCovarianceAndMean() const
71 {
72  const size_t N = m_modes.size();
73 
75  CPose2D estMean2D;
76 
77  this->getMean(estMean2D);
78  cov.setZero();
79 
80  if (N)
81  {
82  // 1) Get the mean:
83  double sumW = 0;
84  auto estMeanMat = CMatrixDouble31(estMean2D);
85  CMatrixDouble33 temp;
86  CMatrixDouble31 estMean_i;
87 
88  for (const auto& m : m_modes)
89  {
90  double w;
91  sumW += w = exp(m.log_w);
92 
93  estMean_i = CMatrixDouble31(m.mean);
94  estMean_i -= estMeanMat;
95 
96  temp.matProductOf_AAt(estMean_i);
97  temp += m.cov;
98  temp *= w;
99 
100  cov += temp;
101  }
102 
103  if (sumW != 0) cov *= (1.0 / sumW);
104  }
105  return {cov, estMean2D};
106 }
107 
108 uint8_t CPosePDFSOG::serializeGetVersion() const { return 2; }
110 {
111  uint32_t N = m_modes.size();
112  out << N;
113 
114  for (const auto& m : m_modes)
115  {
116  out << m.log_w << m.mean;
118  }
119 }
121  mrpt::serialization::CArchive& in, uint8_t version)
122 {
123  switch (version)
124  {
125  case 0:
126  case 1:
127  case 2:
128  {
129  uint32_t N;
130  in >> N;
131  resize(N);
132  for (auto& m : m_modes)
133  {
134  in >> m.log_w;
135 
136  // In version 0, weights were linear!!
137  if (version == 0) m.log_w = log(max(1e-300, m.log_w));
138 
139  in >> m.mean;
140 
141  if (version == 1) // float's
142  {
143  CMatrixFloat33 mf;
145  m.cov = mf.cast_double();
146  }
147  else
148  {
150  }
151  }
152  }
153  break;
154  default:
156  };
157 }
158 
160 {
161  MRPT_START
162 
163  if (this == &o) return; // It may be used sometimes
164 
166  {
167  m_modes = dynamic_cast<const CPosePDFSOG*>(&o)->m_modes;
168  }
169  else
170  {
171  // Approximate as a mono-modal gaussian pdf:
172  m_modes.resize(1);
173  m_modes[0].log_w = 0;
174  o.getMean(m_modes[0].mean);
175  o.getCovariance(m_modes[0].cov);
176  }
177 
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  saveToTextFile
183  ---------------------------------------------------------------*/
184 bool CPosePDFSOG::saveToTextFile(const std::string& file) const
185 {
186  FILE* f = os::fopen(file.c_str(), "wt");
187  if (!f) return false;
188 
189  for (const auto& m : m_modes)
190  os::fprintf(
191  f, "%e %e %e %e %e %e %e %e %e %e\n", exp(m.log_w), m.mean.x(),
192  m.mean.y(), m.mean.phi(), m.cov(0, 0), m.cov(1, 1), m.cov(2, 2),
193  m.cov(0, 1), m.cov(0, 2), m.cov(1, 2));
194  os::fclose(f);
195  return true;
196 }
197 
198 /*---------------------------------------------------------------
199  changeCoordinatesReference
200  ---------------------------------------------------------------*/
201 void CPosePDFSOG::changeCoordinatesReference(const CPose3D& newReferenceBase_)
202 {
203  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
204 
205  CMatrixDouble44 HM;
206  newReferenceBase.getHomogeneousMatrix(HM);
207 
208  // Clip the 4x4 matrix
209  auto M = CMatrixDouble33(HM.block<3, 3>(0, 0));
210 
211  // The variance in phi is unmodified:
212  M(0, 2) = 0;
213  M(1, 2) = 0;
214  M(2, 0) = 0;
215  M(2, 1) = 0;
216  M(2, 2) = 1;
217 
218  for (auto& m : m_modes)
219  {
220  // The mean:
221  m.mean.composeFrom(newReferenceBase, m.mean);
222 
223  // The covariance:
224  // NOTE: the CMatrixDouble33 is NEEDED to create a temporary copy to
225  // allow aliasing
226  m.cov = mrpt::math::multiply_HCHt(M, CMatrixDouble33(m.cov));
227  }
228 
229  enforceCovSymmetry();
230 }
231 
232 /*---------------------------------------------------------------
233  rotateAllCovariances
234  ---------------------------------------------------------------*/
236 {
237  CMatrixDouble33 rot;
238  rot(0, 0) = rot(1, 1) = cos(ang);
239  rot(0, 1) = -sin(ang);
240  rot(1, 0) = sin(ang);
241  rot(2, 2) = 1;
242 
243  for (auto& m : m_modes)
244  m.cov = mrpt::math::multiply_HCHt(rot, CMatrixDouble33(m.cov));
245 }
246 
247 /*---------------------------------------------------------------
248  drawSingleSample
249  ---------------------------------------------------------------*/
250 void CPosePDFSOG::drawSingleSample([[maybe_unused]] CPose2D& outPart) const
251 {
252  MRPT_START
253  THROW_EXCEPTION("Not implemented yet!!");
254  MRPT_END
255 }
256 
257 /*---------------------------------------------------------------
258  drawManySamples
259  ---------------------------------------------------------------*/
261  [[maybe_unused]] size_t N,
262  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
263 {
264  MRPT_START
265 
266  THROW_EXCEPTION("Not implemented yet!!");
267 
268  MRPT_END
269 }
270 
271 /*---------------------------------------------------------------
272  bayesianFusion
273  ---------------------------------------------------------------*/
275  const CPosePDF& p1_, const CPosePDF& p2_,
276  [[maybe_unused]] const double minMahalanobisDistToDrop)
277 {
278  MRPT_START
279 
280  // p1: CPosePDFSOG, p2: CPosePDFGaussian:
281 
284 
285  const auto* p1 = dynamic_cast<const CPosePDFSOG*>(&p1_);
286  const auto* p2 = dynamic_cast<const CPosePDFGaussian*>(&p2_);
287 
288  // Compute the new kernel means, covariances, and weights after multiplying
289  // to the Gaussian "p2":
290  CPosePDFGaussian auxGaussianProduct, auxSOG_Kernel_i;
291 
292  const CMatrixDouble33 covInv = p2->cov.inverse_LLt();
293 
294  auto eta = CMatrixDouble31(p2->mean);
295  eta = covInv * eta;
296 
297  // Normal distribution canonical form constant:
298  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
299  double a =
300  -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
301  (eta.transpose() * p2->cov.asEigen() * eta.asEigen())(0, 0));
302 
303  this->m_modes.clear();
304  for (const auto& m : p1->m_modes)
305  {
306  auxSOG_Kernel_i.mean = m.mean;
307  auxSOG_Kernel_i.cov = CMatrixDouble(m.cov);
308  auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, *p2);
309 
310  // ----------------------------------------------------------------------
311  // The new weight is given by:
312  //
313  // w'_i = w_i * exp( a + a_i - a' )
314  //
315  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1)) +
316  // (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
317  //
318  // ----------------------------------------------------------------------
319  TGaussianMode newKernel;
320  newKernel.mean = auxGaussianProduct.mean;
321  newKernel.cov = auxGaussianProduct.cov;
322 
323  const CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
324 
325  auto eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
326  eta_i = covInv_i * eta_i;
327 
328  const CMatrixDouble33 new_covInv_i = newKernel.cov.inverse_LLt();
329 
330  auto new_eta_i = CMatrixDouble31(newKernel.mean);
331  new_eta_i = new_covInv_i * new_eta_i;
332 
333  double a_i =
334  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
335  (eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
336  eta_i.asEigen())(0, 0));
337  double new_a_i =
338  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
339  (new_eta_i.transpose() * newKernel.cov.asEigen() *
340  new_eta_i.asEigen())(0, 0));
341 
342  // newKernel.w = (it)->w * exp( a + a_i - new_a_i );
343  newKernel.log_w = m.log_w + a + a_i - new_a_i;
344 
345  // Add to the results (in "this") the new kernel:
346  this->m_modes.push_back(newKernel);
347  }
348 
349  normalizeWeights();
350 
351  MRPT_END
352 }
353 
354 /*---------------------------------------------------------------
355  inverse
356  ---------------------------------------------------------------*/
358 {
360  auto* out = dynamic_cast<CPosePDFSOG*>(&o);
361 
362  const_iterator itSrc;
363  iterator itDest;
364 
365  out->m_modes.resize(m_modes.size());
366 
367  for (itSrc = m_modes.begin(), itDest = out->m_modes.begin();
368  itSrc != m_modes.end(); ++itSrc, ++itDest)
369  {
370  // The mean:
371  (itDest)->mean = -(itSrc)->mean;
372 
373  // The covariance: Is the same:
374  (itDest)->cov = (itSrc)->cov;
375  }
376 }
377 
378 /*---------------------------------------------------------------
379  +=
380  ---------------------------------------------------------------*/
382 {
383  for (auto& m : m_modes) m.mean = m.mean + Ap;
384 
385  this->rotateAllCovariances(Ap.phi());
386 }
387 
388 /*---------------------------------------------------------------
389  evaluatePDF
390  ---------------------------------------------------------------*/
391 double CPosePDFSOG::evaluatePDF(const CPose2D& x, bool sumOverAllPhis) const
392 {
393  if (!sumOverAllPhis)
394  {
395  // Normal evaluation:
396  auto X = CMatrixDouble31(x);
397  CMatrixDouble31 MU;
398  double ret = 0;
399 
400  for (const auto& m : m_modes)
401  {
402  MU = CMatrixDouble31(m.mean);
403  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov);
404  }
405 
406  return ret;
407  }
408  else
409  {
410  // Only X,Y:
411  CMatrixDouble X(2, 1), MU(2, 1), COV(2, 2);
412  double ret = 0;
413 
414  X(0, 0) = x.x();
415  X(1, 0) = x.y();
416 
417  for (const auto& m : m_modes)
418  {
419  MU(0, 0) = m.mean.x();
420  MU(1, 0) = m.mean.y();
421 
422  COV(0, 0) = m.cov(0, 0);
423  COV(1, 1) = m.cov(1, 1);
424  COV(0, 1) = COV(1, 0) = m.cov(0, 1);
425 
426  ret += exp(m.log_w) * math::normalPDF(X, MU, COV);
427  }
428 
429  return ret;
430  }
431 }
432 
433 /*---------------------------------------------------------------
434  evaluateNormalizedPDF
435  ---------------------------------------------------------------*/
437 {
438  auto X = CMatrixDouble31(x);
439  CMatrixDouble31 MU;
440  double ret = 0;
441 
442  for (const auto& m : m_modes)
443  {
444  MU = CMatrixDouble31(m.mean);
445  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov) /
446  math::normalPDF(MU, MU, m.cov);
447  }
448 
449  return ret;
450 }
451 
452 /*---------------------------------------------------------------
453  enforceCovSymmetry
454  ---------------------------------------------------------------*/
456 {
457  // Differences, when they exist, appear in the ~15'th significant
458  // digit, so... just take one of them arbitrarily!
459  for (auto& m : m_modes)
460  {
461  m.cov(0, 1) = m.cov(1, 0);
462  m.cov(0, 2) = m.cov(2, 0);
463  m.cov(1, 2) = m.cov(2, 1);
464  }
465 }
466 
467 /*---------------------------------------------------------------
468  normalizeWeights
469  ---------------------------------------------------------------*/
471 {
472  MRPT_START
473 
474  if (!m_modes.size()) return;
475 
476  double maxW = m_modes[0].log_w;
477  for (auto& m : m_modes) maxW = max(maxW, m.log_w);
478 
479  for (auto& m : m_modes) m.log_w -= maxW;
480 
481  MRPT_END
482 }
483 
484 /*---------------------------------------------------------------
485  normalizeWeights
486  ---------------------------------------------------------------*/
488  double x_min, double x_max, double y_min, double y_max, double resolutionXY,
489  double phi, CMatrixDouble& outMatrix, bool sumOverAllPhis)
490 {
491  MRPT_START
492 
493  ASSERT_(x_max > x_min);
494  ASSERT_(y_max > y_min);
495  ASSERT_(resolutionXY > 0);
496 
497  const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
498  const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
499 
500  outMatrix.setSize(Ny, Nx);
501 
502  for (size_t i = 0; i < Ny; i++)
503  {
504  double y = y_min + i * resolutionXY;
505  for (size_t j = 0; j < Nx; j++)
506  {
507  double x = x_min + j * resolutionXY;
508  outMatrix(i, j) = evaluatePDF(CPose2D(x, y, phi), sumOverAllPhis);
509  }
510  }
511 
512  MRPT_END
513 }
514 
515 /*---------------------------------------------------------------
516  mergeModes
517  ---------------------------------------------------------------*/
518 void CPosePDFSOG::mergeModes(double max_KLd, bool verbose)
519 {
520  MRPT_START
521 
522  normalizeWeights();
523 
524  size_t N = m_modes.size();
525  if (N < 2) return; // Nothing to do
526 
527  // Method described in:
528  // "Kullback-Leibler Approach to Gaussian Mixture Reduction", A.R. Runnalls.
529  // IEEE Transactions on Aerospace and Electronic Systems, 2007.
530  // See Eq.(21) for Bij !!
531 
532  for (size_t i = 0; i < (N - 1);)
533  {
534  N = m_modes.size(); // It might have changed.
535  double sumW = 0;
536 
537  // For getting linear weights:
538  sumW = 0;
539  for (size_t j = 0; j < N; j++) sumW += exp(m_modes[j].log_w);
540  ASSERT_(sumW);
541 
542  const double Wi = exp(m_modes[i].log_w) / sumW;
543 
544  double min_Bij = std::numeric_limits<double>::max();
545 
546  CMatrixDouble33 min_Bij_COV;
547  size_t best_j = 0;
548 
549  auto MUi = CMatrixDouble31(m_modes[i].mean);
550 
551  // Compute B(i,j), j=[i+1,N-1] (the discriminant)
552  for (size_t j = 0; j < N; j++)
553  if (i != j)
554  {
555  const double Wj = exp(m_modes[j].log_w) / sumW;
556  const double Wij_ = 1.0 / (Wi + Wj);
557 
558  auto Pij =
559  CMatrixDouble33(m_modes[i].cov.asEigen() * Wi * Wij_);
560  Pij.asEigen() += m_modes[j].cov.asEigen() * Wj * Wij_;
561 
562  auto MUij = CMatrixDouble31(m_modes[j].mean);
563  MUij -= MUi;
564  // Account for circular dimensions:
565  mrpt::math::wrapToPiInPlace(MUij(2, 0));
566 
567  CMatrixDouble33 AUX;
568  AUX.matProductOf_AAt(MUij); // AUX = MUij * MUij^T
569 
570  AUX *= Wi * Wj * Wij_ * Wij_;
571  Pij += AUX;
572 
573  double Bij = (Wi + Wj) * log(Pij.det()) -
574  Wi * log(m_modes[i].cov.det()) -
575  Wj * log(m_modes[j].cov.det());
576  if (verbose)
577  {
578  cout << "try merge[" << i << ", " << j
579  << "] -> Bij: " << Bij << endl;
580  // cout << "AUX: " << endl << AUX;
581  // cout << "Wi: " << Wi << " Wj:" << Wj << " Wij_: " << Wij_
582  // << endl;
583  cout << "Pij: " << Pij << endl
584  << " Pi: " << m_modes[i].cov << endl
585  << " Pj: " << m_modes[j].cov << endl;
586  }
587 
588  if (Bij < min_Bij)
589  {
590  min_Bij = Bij;
591  best_j = j;
592  min_Bij_COV = Pij;
593  }
594  }
595 
596  // Is a good move to merge (i,j)??
597  if (verbose)
598  cout << "merge[" << i << ", " << best_j
599  << "] Tempting merge: KLd = " << min_Bij;
600 
601  if (min_Bij < max_KLd)
602  {
603  if (verbose) cout << " Accepted." << endl;
604 
605  // Do the merge (i,j):
606  TGaussianMode Mij;
607  TGaussianMode& Mi = m_modes[i];
608  TGaussianMode& Mj = m_modes[best_j];
609 
610  // Weight:
611  Mij.log_w = log(exp(Mi.log_w) + exp(Mj.log_w));
612 
613  // Mean:
614  const double Wj = exp(Mj.log_w) / sumW;
615  const double Wij_ = 1.0 / (Wi + Wj);
616  const double Wi_ = Wi * Wij_;
617  const double Wj_ = Wj * Wij_;
618 
619  Mij.mean = CPose2D(
620  Wi_ * Mi.mean.x() + Wj_ * Mj.mean.x(),
621  Wi_ * Mi.mean.y() + Wj_ * Mj.mean.y(),
622  Wi_ * Mi.mean.phi() + Wj_ * Mj.mean.phi());
623 
624  // Cov:
625  Mij.cov = min_Bij_COV;
626 
627  // Replace Mi <- Mij:
628  m_modes[i] = Mij;
629  m_modes.erase(m_modes.begin() + best_j); // erase Mj
630  } // end merge
631  else
632  {
633  if (verbose) cout << " Nope." << endl;
634 
635  i++;
636  }
637  } // for i
638 
639  normalizeWeights();
640 
641  MRPT_END
642 }
643 
644 /*---------------------------------------------------------------
645  getMostLikelyCovarianceAndMean
646  ---------------------------------------------------------------*/
648  CMatrixDouble33& cov, CPose2D& mean_point) const
649 {
650  auto it_best = end();
651  double best_log_w = -std::numeric_limits<double>::max();
652 
653  for (auto i = begin(); i != end(); ++i)
654  {
655  if (i->log_w > best_log_w)
656  {
657  best_log_w = i->log_w;
658  it_best = i;
659  }
660  }
661 
662  if (it_best != end())
663  {
664  mean_point = it_best->mean;
665  cov = it_best->cov;
666  }
667  else
668  {
669  cov.setIdentity();
670  cov.asEigen() *= 1e20;
671  mean_point = CPose2D(0, 0, 0);
672  }
673 }
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:42
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:286
void rotateAllCovariances(double ang)
Rotate all the covariance matrixes by replacing them by , where .
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:52
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 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:70
#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:367
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:102
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:372
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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
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:149
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:419
const_iterator end() const
Definition: ts_hash_map.h:246
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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:85
mrpt::vision::TStereoCalibResults out
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.
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
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) ...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
void operator+=(const mrpt::poses::CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void evaluatePDFInArea(double x_min, double x_max, double y_min, double y_max, double resolutionXY, 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...
images resize(NUM_IMGS)
CMatrixFixed< double, ROWS, COLS > cast_double() const
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CPosePDFSOG.cpp:46
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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.



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020