MRPT  2.0.4
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 #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 
106 uint8_t CPosePDFSOG::serializeGetVersion() const { return 2; }
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 }
119  mrpt::serialization::CArchive& in, uint8_t version)
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  ---------------------------------------------------------------*/
182 bool CPosePDFSOG::saveToTextFile(const std::string& file) const
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  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
248 void CPosePDFSOG::drawSingleSample([[maybe_unused]] CPose2D& outPart) const
249 {
250  MRPT_START
251  THROW_EXCEPTION("Not implemented yet!!");
252  MRPT_END
253 }
254 
255 /*---------------------------------------------------------------
256  drawManySamples
257  ---------------------------------------------------------------*/
259  [[maybe_unused]] size_t N,
260  [[maybe_unused]] std::vector<CVectorDouble>& outSamples) const
261 {
262  MRPT_START
263 
264  THROW_EXCEPTION("Not implemented yet!!");
265 
266  MRPT_END
267 }
268 
269 /*---------------------------------------------------------------
270  bayesianFusion
271  ---------------------------------------------------------------*/
273  const CPosePDF& p1_, const CPosePDF& p2_,
274  [[maybe_unused]] const double minMahalanobisDistToDrop)
275 {
276  MRPT_START
277 
278  // p1: CPosePDFSOG, p2: CPosePDFGaussian:
279 
282 
283  const auto* p1 = dynamic_cast<const CPosePDFSOG*>(&p1_);
284  const auto* p2 = dynamic_cast<const CPosePDFGaussian*>(&p2_);
285 
286  // Compute the new kernel means, covariances, and weights after multiplying
287  // to the Gaussian "p2":
288  CPosePDFGaussian auxGaussianProduct, auxSOG_Kernel_i;
289 
290  const CMatrixDouble33 covInv = p2->cov.inverse_LLt();
291 
292  auto eta = CMatrixDouble31(p2->mean);
293  eta = covInv * eta;
294 
295  // Normal distribution canonical form constant:
296  // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
297  double a =
298  -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
299  (eta.transpose() * p2->cov.asEigen() * eta.asEigen())(0, 0));
300 
301  this->m_modes.clear();
302  for (const auto& m : p1->m_modes)
303  {
304  auxSOG_Kernel_i.mean = m.mean;
305  auxSOG_Kernel_i.cov = CMatrixDouble(m.cov);
306  auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, *p2);
307 
308  // ----------------------------------------------------------------------
309  // The new weight is given by:
310  //
311  // w'_i = w_i * exp( a + a_i - a' )
312  //
313  // a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1)) +
314  // (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
315  //
316  // ----------------------------------------------------------------------
317  TGaussianMode newKernel;
318  newKernel.mean = auxGaussianProduct.mean;
319  newKernel.cov = auxGaussianProduct.cov;
320 
321  const CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inverse_LLt();
322 
323  auto eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
324  eta_i = covInv_i * eta_i;
325 
326  const CMatrixDouble33 new_covInv_i = newKernel.cov.inverse_LLt();
327 
328  auto new_eta_i = CMatrixDouble31(newKernel.mean);
329  new_eta_i = new_covInv_i * new_eta_i;
330 
331  double a_i =
332  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
333  (eta_i.transpose() * auxSOG_Kernel_i.cov.asEigen() *
334  eta_i.asEigen())(0, 0));
335  double new_a_i =
336  -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
337  (new_eta_i.transpose() * newKernel.cov.asEigen() *
338  new_eta_i.asEigen())(0, 0));
339 
340  // newKernel.w = (it)->w * exp( a + a_i - new_a_i );
341  newKernel.log_w = m.log_w + a + a_i - new_a_i;
342 
343  // Add to the results (in "this") the new kernel:
344  this->m_modes.push_back(newKernel);
345  }
346 
347  normalizeWeights();
348 
349  MRPT_END
350 }
351 
352 /*---------------------------------------------------------------
353  inverse
354  ---------------------------------------------------------------*/
356 {
358  auto* out = dynamic_cast<CPosePDFSOG*>(&o);
359 
360  const_iterator itSrc;
361  iterator itDest;
362 
363  out->m_modes.resize(m_modes.size());
364 
365  for (itSrc = m_modes.begin(), itDest = out->m_modes.begin();
366  itSrc != m_modes.end(); ++itSrc, ++itDest)
367  {
368  // The mean:
369  (itDest)->mean = -(itSrc)->mean;
370 
371  // The covariance: Is the same:
372  (itDest)->cov = (itSrc)->cov;
373  }
374 }
375 
376 /*---------------------------------------------------------------
377  +=
378  ---------------------------------------------------------------*/
380 {
381  for (auto& m : m_modes) m.mean = m.mean + Ap;
382 
383  this->rotateAllCovariances(Ap.phi());
384 }
385 
386 /*---------------------------------------------------------------
387  evaluatePDF
388  ---------------------------------------------------------------*/
389 double CPosePDFSOG::evaluatePDF(const CPose2D& x, bool sumOverAllPhis) const
390 {
391  if (!sumOverAllPhis)
392  {
393  // Normal evaluation:
394  auto X = CMatrixDouble31(x);
395  CMatrixDouble31 MU;
396  double ret = 0;
397 
398  for (const auto& m : m_modes)
399  {
400  MU = CMatrixDouble31(m.mean);
401  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov);
402  }
403 
404  return ret;
405  }
406  else
407  {
408  // Only X,Y:
409  CMatrixDouble X(2, 1), MU(2, 1), COV(2, 2);
410  double ret = 0;
411 
412  X(0, 0) = x.x();
413  X(1, 0) = x.y();
414 
415  for (const auto& m : m_modes)
416  {
417  MU(0, 0) = m.mean.x();
418  MU(1, 0) = m.mean.y();
419 
420  COV(0, 0) = m.cov(0, 0);
421  COV(1, 1) = m.cov(1, 1);
422  COV(0, 1) = COV(1, 0) = m.cov(0, 1);
423 
424  ret += exp(m.log_w) * math::normalPDF(X, MU, COV);
425  }
426 
427  return ret;
428  }
429 }
430 
431 /*---------------------------------------------------------------
432  evaluateNormalizedPDF
433  ---------------------------------------------------------------*/
435 {
436  auto X = CMatrixDouble31(x);
437  CMatrixDouble31 MU;
438  double ret = 0;
439 
440  for (const auto& m : m_modes)
441  {
442  MU = CMatrixDouble31(m.mean);
443  ret += exp(m.log_w) * math::normalPDF(X, MU, m.cov) /
444  math::normalPDF(MU, MU, m.cov);
445  }
446 
447  return ret;
448 }
449 
450 /*---------------------------------------------------------------
451  enforceCovSymmetry
452  ---------------------------------------------------------------*/
454 {
455  // Differences, when they exist, appear in the ~15'th significant
456  // digit, so... just take one of them arbitrarily!
457  for (auto& m : m_modes)
458  {
459  m.cov(0, 1) = m.cov(1, 0);
460  m.cov(0, 2) = m.cov(2, 0);
461  m.cov(1, 2) = m.cov(2, 1);
462  }
463 }
464 
465 /*---------------------------------------------------------------
466  normalizeWeights
467  ---------------------------------------------------------------*/
469 {
470  MRPT_START
471 
472  if (!m_modes.size()) return;
473 
474  double maxW = m_modes[0].log_w;
475  for (auto& m : m_modes) maxW = max(maxW, m.log_w);
476 
477  for (auto& m : m_modes) m.log_w -= maxW;
478 
479  MRPT_END
480 }
481 
482 /*---------------------------------------------------------------
483  normalizeWeights
484  ---------------------------------------------------------------*/
486  double x_min, double x_max, double y_min, double y_max, double resolutionXY,
487  double phi, CMatrixDouble& outMatrix, bool sumOverAllPhis)
488 {
489  MRPT_START
490 
491  ASSERT_(x_max > x_min);
492  ASSERT_(y_max > y_min);
493  ASSERT_(resolutionXY > 0);
494 
495  const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
496  const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
497 
498  outMatrix.setSize(Ny, Nx);
499 
500  for (size_t i = 0; i < Ny; i++)
501  {
502  double y = y_min + i * resolutionXY;
503  for (size_t j = 0; j < Nx; j++)
504  {
505  double x = x_min + j * resolutionXY;
506  outMatrix(i, j) = evaluatePDF(CPose2D(x, y, phi), sumOverAllPhis);
507  }
508  }
509 
510  MRPT_END
511 }
512 
513 /*---------------------------------------------------------------
514  mergeModes
515  ---------------------------------------------------------------*/
516 void CPosePDFSOG::mergeModes(double max_KLd, bool verbose)
517 {
518  MRPT_START
519 
520  normalizeWeights();
521 
522  size_t N = m_modes.size();
523  if (N < 2) return; // Nothing to do
524 
525  // Method described in:
526  // "Kullback-Leibler Approach to Gaussian Mixture Reduction", A.R. Runnalls.
527  // IEEE Transactions on Aerospace and Electronic Systems, 2007.
528  // See Eq.(21) for Bij !!
529 
530  for (size_t i = 0; i < (N - 1);)
531  {
532  N = m_modes.size(); // It might have changed.
533  double sumW = 0;
534 
535  // For getting linear weights:
536  sumW = 0;
537  for (size_t j = 0; j < N; j++) sumW += exp(m_modes[j].log_w);
538  ASSERT_(sumW);
539 
540  const double Wi = exp(m_modes[i].log_w) / sumW;
541 
542  double min_Bij = std::numeric_limits<double>::max();
543 
544  CMatrixDouble33 min_Bij_COV;
545  size_t best_j = 0;
546 
547  auto MUi = CMatrixDouble31(m_modes[i].mean);
548 
549  // Compute B(i,j), j=[i+1,N-1] (the discriminant)
550  for (size_t j = 0; j < N; j++)
551  if (i != j)
552  {
553  const double Wj = exp(m_modes[j].log_w) / sumW;
554  const double Wij_ = 1.0 / (Wi + Wj);
555 
556  auto Pij =
557  CMatrixDouble33(m_modes[i].cov.asEigen() * Wi * Wij_);
558  Pij.asEigen() += m_modes[j].cov.asEigen() * Wj * Wij_;
559 
560  auto MUij = CMatrixDouble31(m_modes[j].mean);
561  MUij -= MUi;
562  // Account for circular dimensions:
563  mrpt::math::wrapToPiInPlace(MUij(2, 0));
564 
565  CMatrixDouble33 AUX;
566  AUX.matProductOf_AAt(MUij); // AUX = MUij * MUij^T
567 
568  AUX *= Wi * Wj * Wij_ * Wij_;
569  Pij += AUX;
570 
571  double Bij = (Wi + Wj) * log(Pij.det()) -
572  Wi * log(m_modes[i].cov.det()) -
573  Wj * log(m_modes[j].cov.det());
574  if (verbose)
575  {
576  cout << "try merge[" << i << ", " << j
577  << "] -> Bij: " << Bij << endl;
578  // cout << "AUX: " << endl << AUX;
579  // cout << "Wi: " << Wi << " Wj:" << Wj << " Wij_: " << Wij_
580  // << endl;
581  cout << "Pij: " << Pij << endl
582  << " Pi: " << m_modes[i].cov << endl
583  << " Pj: " << m_modes[j].cov << endl;
584  }
585 
586  if (Bij < min_Bij)
587  {
588  min_Bij = Bij;
589  best_j = j;
590  min_Bij_COV = Pij;
591  }
592  }
593 
594  // Is a good move to merge (i,j)??
595  if (verbose)
596  cout << "merge[" << i << ", " << best_j
597  << "] Tempting merge: KLd = " << min_Bij;
598 
599  if (min_Bij < max_KLd)
600  {
601  if (verbose) cout << " Accepted." << endl;
602 
603  // Do the merge (i,j):
604  TGaussianMode Mij;
605  TGaussianMode& Mi = m_modes[i];
606  TGaussianMode& Mj = m_modes[best_j];
607 
608  // Weight:
609  Mij.log_w = log(exp(Mi.log_w) + exp(Mj.log_w));
610 
611  // Mean:
612  const double Wj = exp(Mj.log_w) / sumW;
613  const double Wij_ = 1.0 / (Wi + Wj);
614  const double Wi_ = Wi * Wij_;
615  const double Wj_ = Wj * Wij_;
616 
617  Mij.mean = CPose2D(
618  Wi_ * Mi.mean.x() + Wj_ * Mj.mean.x(),
619  Wi_ * Mi.mean.y() + Wj_ * Mj.mean.y(),
620  Wi_ * Mi.mean.phi() + Wj_ * Mj.mean.phi());
621 
622  // Cov:
623  Mij.cov = min_Bij_COV;
624 
625  // Replace Mi <- Mij:
626  m_modes[i] = Mij;
627  m_modes.erase(m_modes.begin() + best_j); // erase Mj
628  } // end merge
629  else
630  {
631  if (verbose) cout << " Nope." << endl;
632 
633  i++;
634  }
635  } // for i
636 
637  normalizeWeights();
638 
639  MRPT_END
640 }
641 
642 /*---------------------------------------------------------------
643  getMostLikelyCovarianceAndMean
644  ---------------------------------------------------------------*/
646  CMatrixDouble33& cov, CPose2D& mean_point) const
647 {
648  auto it_best = end();
649  double best_log_w = -std::numeric_limits<double>::max();
650 
651  for (auto i = begin(); i != end(); ++i)
652  {
653  if (i->log_w > best_log_w)
654  {
655  best_log_w = i->log_w;
656  it_best = i;
657  }
658  }
659 
660  if (it_best != end())
661  {
662  mean_point = it_best->mean;
663  cov = it_best->cov;
664  }
665  else
666  {
667  cov.setIdentity();
668  cov.asEigen() *= 1e20;
669  mean_point = CPose2D(0, 0, 0);
670  }
671 }
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:282
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: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 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
#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:415
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:264
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:44
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.4 Git: 1e710274b Thu May 28 15:30:14 2020 +0200 at jue may 28 15:45:10 CEST 2020