MRPT  2.0.4
CPosePDFGaussianInf.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/TPose2D.h>
14 #include <mrpt/math/ops_matrices.h>
15 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPose3DPDF.h>
21 #include <mrpt/random.h>
24 #include <mrpt/system/os.h>
25 #include <Eigen/Dense>
26 
27 using namespace mrpt;
28 using namespace mrpt::poses;
29 using namespace mrpt::math;
30 using namespace mrpt::random;
31 using namespace mrpt::system;
32 using namespace std;
33 
35 
36 /*---------------------------------------------------------------
37  Constructor
38  ---------------------------------------------------------------*/
39 CPosePDFGaussianInf::CPosePDFGaussianInf() : mean(0, 0, 0), cov_inv() {}
40 /*---------------------------------------------------------------
41  Constructor
42  ---------------------------------------------------------------*/
44  const CPose2D& init_Mean, const CMatrixDouble33& init_CovInv)
45  : mean(init_Mean), cov_inv(init_CovInv)
46 {
47 }
48 
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  : mean(init_Mean), cov_inv()
54 {
55 }
56 
57 uint8_t CPosePDFGaussianInf::serializeGetVersion() const { return 0; }
59 {
60  out << mean.x() << mean.y() << mean.phi();
61  out << cov_inv(0, 0) << cov_inv(1, 1) << cov_inv(2, 2);
62  out << cov_inv(0, 1) << cov_inv(0, 2) << cov_inv(1, 2);
63 }
65  mrpt::serialization::CArchive& in, uint8_t version)
66 {
67  switch (version)
68  {
69  case 0:
70  {
71  TPose2D p;
72  in >> p.x >> p.y >> p.phi;
73  mean = CPose2D(p);
74 
75  in >> cov_inv(0, 0) >> cov_inv(1, 1) >> cov_inv(2, 2);
76  in >> cov_inv(0, 1) >> cov_inv(0, 2) >> cov_inv(1, 2);
77  }
78  break;
79  default:
81  };
82 }
83 
86 {
88  out["mean"] = mean;
89  out["cov_inv"] = CMatrixD(cov_inv);
90 }
93 {
94  uint8_t version;
96  switch (version)
97  {
98  case 1:
99  {
100  in["mean"].readTo(mean);
101  CMatrixD m;
102  in["cov_inv"].readTo(m);
103  cov_inv = m;
104  }
105  break;
106  default:
108  }
109 }
110 
111 /*---------------------------------------------------------------
112  copyFrom
113  ---------------------------------------------------------------*/
115 {
116  if (this == &o) return; // It may be used sometimes
117 
119  { // It's my same class:
120  const auto* ptr = dynamic_cast<const CPosePDFGaussianInf*>(&o);
121  mean = ptr->mean;
122  cov_inv = ptr->cov_inv;
123  }
124  else
125  { // Convert to gaussian pdf:
126  o.getMean(mean);
127 
129  o.getCovariance(o_cov);
130  this->cov_inv = o_cov.inverse_LLt();
131  }
132 }
133 
134 /*---------------------------------------------------------------
135  copyFrom 3D
136  ---------------------------------------------------------------*/
138 {
139  // Convert to gaussian pdf:
140  mean = CPose2D(o.getMeanVal());
141 
143  { // Cov is already in information form:
144  const auto* ptr = dynamic_cast<const CPose3DPDFGaussianInf*>(&o);
145  cov_inv(0, 0) = ptr->cov_inv(0, 0);
146  cov_inv(1, 1) = ptr->cov_inv(1, 1);
147  cov_inv(2, 2) = ptr->cov_inv(3, 3);
148  cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
149  cov_inv(0, 2) = cov_inv(2, 0) = ptr->cov_inv(0, 3);
150  cov_inv(1, 2) = cov_inv(2, 1) = ptr->cov_inv(1, 3);
151  }
152  else
153  {
155  o.getCovariance(C);
156 
157  // Clip to 3x3:
159  o_cov(0, 0) = C(0, 0);
160  o_cov(1, 1) = C(1, 1);
161  o_cov(2, 2) = C(3, 3);
162  o_cov(0, 1) = o_cov(1, 0) = C(0, 1);
163  o_cov(0, 2) = o_cov(2, 0) = C(0, 3);
164  o_cov(1, 2) = o_cov(2, 1) = C(1, 3);
165 
166  this->cov_inv = o_cov.inverse_LLt();
167  }
168 }
169 
170 /*---------------------------------------------------------------
171 
172  ---------------------------------------------------------------*/
173 bool CPosePDFGaussianInf::saveToTextFile(const std::string& file) const
174 {
175  FILE* f = os::fopen(file.c_str(), "wt");
176  if (!f) return false;
177 
178  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
179 
180  for (unsigned int i = 0; i < 3; i++)
181  os::fprintf(
182  f, "%f %f %f\n", cov_inv(i, 0), cov_inv(i, 1), cov_inv(i, 2));
183 
184  os::fclose(f);
185  return true;
186 }
187 
188 /*---------------------------------------------------------------
189  changeCoordinatesReference
190  ---------------------------------------------------------------*/
192  const CPose3D& newReferenceBase_)
193 {
194  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
195 
196  // The mean:
197  mean.composeFrom(newReferenceBase, mean);
198 
199  // The covariance:
200  rotateCov(newReferenceBase.phi());
201 }
202 
203 /*---------------------------------------------------------------
204  changeCoordinatesReference
205  ---------------------------------------------------------------*/
207  const CPose2D& newReferenceBase)
208 {
209  // The mean:
210  mean.composeFrom(newReferenceBase, mean);
211  // The covariance:
212  rotateCov(newReferenceBase.phi());
213 }
214 
215 /*---------------------------------------------------------------
216  changeCoordinatesReference
217  ---------------------------------------------------------------*/
218 void CPosePDFGaussianInf::rotateCov(const double ang)
219 {
220  const double ccos = cos(ang);
221  const double ssin = sin(ang);
222 
223  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
224  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
225 
226  const CMatrixFixed<double, 3, 3> rot(rot_vals);
227 
228  // NEW_COV = H C H^T
229  // NEW_COV^(-1) = (H C H^T)^(-1) = (H^T)^(-1) C^(-1) H^(-1)
230  // rot: Inverse of a rotation matrix is its trasposed.
231  // But we need H^t^-1 -> H !! so rot stays unchanged:
232  cov_inv = rot.asEigen() * cov_inv.asEigen() * rot.asEigen().transpose();
233 }
234 
236 {
237  MRPT_START
238 
239  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
240 
241  CVectorDouble v;
243 
244  outPart.x(mean.x() + v[0]);
245  outPart.y(mean.y() + v[1]);
246  outPart.phi(mean.phi() + v[2]);
247 
248  // Range -pi,pi
249  outPart.normalizePhi();
250 
252  "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
253 }
254 
256  size_t N, std::vector<CVectorDouble>& outSamples) const
257 {
258  MRPT_START
259 
260  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
261 
262  std::vector<CVectorDouble> rndSamples;
263 
265  outSamples.resize(N);
266  for (size_t i = 0; i < N; i++)
267  {
268  outSamples[i].resize(3);
269  outSamples[i][0] = mean.x() + rndSamples[i][0];
270  outSamples[i][1] = mean.y() + rndSamples[i][1];
271  outSamples[i][2] = mean.phi() + rndSamples[i][2];
272 
273  wrapToPiInPlace(outSamples[i][2]);
274  }
275 
276  MRPT_END
277 }
278 
279 /*---------------------------------------------------------------
280  bayesianFusion
281  ---------------------------------------------------------------*/
283  const CPosePDF& p1_, const CPosePDF& p2_,
284  [[maybe_unused]] const double minMahalanobisDistToDrop)
285 {
286  MRPT_START
287 
290 
291  const auto* p1 = dynamic_cast<const CPosePDFGaussianInf*>(&p1_);
292  const auto* p2 = dynamic_cast<const CPosePDFGaussianInf*>(&p2_);
293 
294  const CMatrixDouble33& C1_inv = p1->cov_inv;
295  const CMatrixDouble33& C2_inv = p2->cov_inv;
296 
297  auto x1 = CMatrixDouble31(p1->mean);
298  auto x2 = CMatrixDouble31(p2->mean);
299 
300  this->cov_inv = C1_inv + C2_inv;
301 
302  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
303 
304  auto x = CMatrixDouble31(cov.asEigen() * (C1_inv * x1 + C2_inv * x2));
305 
306  this->mean.x(x(0, 0));
307  this->mean.y(x(1, 0));
308  this->mean.phi(x(2, 0));
309  this->mean.normalizePhi();
310 
311  MRPT_END
312 }
313 
314 /*---------------------------------------------------------------
315  inverse
316  ---------------------------------------------------------------*/
318 {
320  auto* out = dynamic_cast<CPosePDFGaussianInf*>(&o);
321 
322  // The mean:
323  out->mean = CPose2D(0, 0, 0) - mean;
324 
325  // The covariance:
326  const double ccos = ::cos(mean.phi());
327  const double ssin = ::sin(mean.phi());
328 
329  // jacobian:
330  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double H_values[] = {
331  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
332  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
333  0, 0, -1};
334  const CMatrixFixed<double, 3, 3> H(H_values);
335 
336  // o.cov = H * cov * Ht. It's the same with inverse covariances.
337  out->cov_inv.asEigen().noalias() =
338  (H.asEigen() * cov_inv.asEigen() * H.transpose()).eval();
339 }
340 
341 /*---------------------------------------------------------------
342  +=
343  ---------------------------------------------------------------*/
345 {
346  mean = mean + Ap;
347  rotateCov(Ap.phi());
348 }
349 
350 /*---------------------------------------------------------------
351  evaluatePDF
352  ---------------------------------------------------------------*/
354 {
355  auto X = CMatrixDouble31(x);
356  auto MU = CMatrixDouble31(mean);
357 
358  return math::normalPDF(X, MU, cov_inv.inverse());
359 }
360 
361 /*---------------------------------------------------------------
362  evaluateNormalizedPDF
363  ---------------------------------------------------------------*/
365 {
366  auto X = CMatrixDouble31(x);
367  auto MU = CMatrixDouble31(mean);
368 
369  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
370 
371  return math::normalPDF(X, MU, cov) / math::normalPDF(MU, MU, cov);
372 }
373 
374 /*---------------------------------------------------------------
375  enforceCovSymmetry
376  ---------------------------------------------------------------*/
378 {
379  // Differences, when they exist, appear in the ~15'th significant
380  // digit, so... just take one of them arbitrarily!
381  cov_inv(0, 1) = cov_inv(1, 0);
382  cov_inv(0, 2) = cov_inv(2, 0);
383  cov_inv(1, 2) = cov_inv(2, 1);
384 }
385 
386 /*---------------------------------------------------------------
387  mahalanobisDistanceTo
388  ---------------------------------------------------------------*/
390  const CPosePDFGaussianInf& theOther)
391 {
392  MRPT_START
393 
394  auto MU = CVectorFixedDouble<3>(mean);
395  MU -= CVectorFixedDouble<3>(theOther.mean);
396 
397  wrapToPiInPlace(MU[2]);
398 
399  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
400  return 0; // This is the ONLY case where we know the result, whatever
401  // COVs are.
402 
403  CMatrixDouble33 COV_ = this->cov_inv.inverse_LLt();
404  const CMatrixDouble33 cov2 = theOther.cov_inv.inverse_LLt();
405  COV_ += cov2; // COV_ = cov1+cov2
406 
407  const CMatrixDouble33 COV_inv = COV_.inverse_LLt();
408 
409  // (~MU) * (!COV_) * MU
410  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU.asEigen(), COV_inv));
411 
412  MRPT_END
413 }
414 
415 /*---------------------------------------------------------------
416  operator <<
417  ---------------------------------------------------------------*/
419  std::ostream& out, const CPosePDFGaussianInf& obj)
420 {
421  out << "Mean: " << obj.mean << "\n";
422  out << "Inverse cov:\n" << obj.cov_inv << "\n";
423 
424  return out;
425 }
426 
427 /*---------------------------------------------------------------
428  operator +
429  ---------------------------------------------------------------*/
432 {
435  return ret;
436 }
437 
438 /*---------------------------------------------------------------
439  inverseComposition
440  Set 'this' = 'x' - 'ref', computing the mean using the "-"
441  operator and the covariances through the corresponding Jacobians.
442  ---------------------------------------------------------------*/
444  const CPosePDFGaussianInf& xv, const CPosePDFGaussianInf& xi)
445 {
446  // Use implementation in CPosePDFGaussian:
447  const CMatrixDouble33 xv_cov = xv.cov_inv.inverse_LLt();
448  const CMatrixDouble33 xi_cov = xi.cov_inv.inverse_LLt();
449 
450  const CPosePDFGaussian xv_(xv.mean, xv_cov);
451  const CPosePDFGaussian xi_(xi.mean, xi_cov);
452 
453  CPosePDFGaussian RET;
454  RET.inverseComposition(xv_, xi_);
455 
456  // Copy result to "this":
457  this->mean = RET.mean;
458  this->cov_inv = RET.cov.inverse_LLt();
459 }
460 
461 /*---------------------------------------------------------------
462  inverseComposition
463  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
464  the "-" operator and the covariances through the corresponding
465  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
466  ---------------------------------------------------------------*/
468  const CPosePDFGaussianInf& x1, const CPosePDFGaussianInf& x0,
469  const CMatrixDouble33& COV_01)
470 {
471  // Use implementation in CPosePDFGaussian:
472  const CMatrixDouble33 x1_cov = x1.cov_inv.inverse_LLt();
473  const CMatrixDouble33 x0_cov = x0.cov_inv.inverse_LLt();
474 
475  const CPosePDFGaussian x1_(x1.mean, x1_cov);
476  const CPosePDFGaussian x0_(x0.mean, x0_cov);
477 
478  CPosePDFGaussian RET;
479  RET.inverseComposition(x1_, x0_, COV_01);
480 
481  // Copy result to "this":
482  this->mean = RET.mean;
483  this->cov_inv = RET.cov.inverse_LLt();
484 }
485 
486 /*---------------------------------------------------------------
487  +=
488  ---------------------------------------------------------------*/
490 {
491  // COV:
492  const CMatrixDouble33 OLD_COV = this->cov_inv.inverse_LLt();
493 
494  CMatrixDouble33 df_dx, df_du;
495 
497  this->mean, // x
498  Ap.mean, // u
499  df_dx, df_du);
500 
501  const CMatrixDouble33 Ap_cov = Ap.cov_inv.inverse_LLt();
502 
503  this->cov_inv =
504  (multiply_HCHt(df_dx, OLD_COV) + multiply_HCHt(df_du, Ap_cov))
505  .inverse_LLt();
506 
507  // MEAN:
508  this->mean = this->mean + Ap.mean;
509 }
510 
512  const CPosePDFGaussianInf& p1, const CPosePDFGaussianInf& p2)
513 {
514  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
515 }
516 
517 /** Pose compose operator: RES = A (+) B , computing both the mean and the
518  * covariance */
520  const CPosePDFGaussianInf& a, const CPosePDFGaussianInf& b)
521 {
522  CPosePDFGaussianInf res(a);
523  res += b;
524  return res;
525 }
526 
527 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and
528  * the covariance */
530  const CPosePDFGaussianInf& a, const CPosePDFGaussianInf& b)
531 {
533  res.inverseComposition(a, b);
534  return res;
535 }
mrpt::math::cov
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
os.h
mrpt::poses::CPosePDFGaussianInf::evaluateNormalizedPDF
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
Definition: CPosePDFGaussianInf.cpp:364
mrpt::poses::CPosePDFGaussianInf::enforceCovSymmetry
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
Definition: CPosePDFGaussianInf.cpp:377
mrpt::poses::CPose2D::normalizePhi
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
CPose3DPDFGaussianInf.h
poses-precomp.h
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
mrpt::poses::CPosePDFGaussianInf::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPosePDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
Definition: CPosePDFGaussianInf.cpp:389
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::serialization::CSchemeArchiveBase
Virtual base class for "schematic archives" (JSON, XML,...)
Definition: CSchemeArchiveBase.h:75
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::poses::CPosePDFGaussianInf::drawManySamples
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,...
Definition: CPosePDFGaussianInf.cpp:255
MRPT_END_WITH_CLEAN_UP
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
mrpt::poses::CPose2D::composeFrom
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
IS_CLASS
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
Definition: CObject.h:146
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
mrpt::poses::CPosePDFGaussianInf::copyFrom
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPosePDFGaussianInf.cpp:114
mrpt::poses::CPosePDFGaussianInf::operator+=
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
Definition: CPosePDFGaussianInf.cpp:344
mrpt::poses::CPose3DPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition: CPose3DPDF.h:39
mrpt::poses::operator+
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:394
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:46
TPose2D.h
mrpt::poses::CPosePDFGaussianInf::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussianInf.h:51
SCHEMA_DESERIALIZE_DATATYPE_VERSION
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
Definition: CSerializable.h:139
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
wrap2pi.h
mrpt::random::CRandomGenerator::drawGaussianMultivariateMany
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix.
Definition: RandomGenerators.h:342
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses::CPosePDF::jacobiansPoseComposition
static void jacobiansPoseComposition(const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true)
This static method computes the pose composition Jacobians, with these formulas:
Definition: CPosePDF.cpp:32
mrpt::poses::CPosePDFGaussianInf::rotateCov
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
Definition: CPosePDFGaussianInf.cpp:218
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses::operator<<
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
mrpt::poses::CPosePDFGaussianInf::bayesianFusion
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
Definition: CPosePDFGaussianInf.cpp:282
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses::operator-
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:356
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPosePDFGaussianInf::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPosePDFGaussianInf.cpp:191
mrpt::math::wrapToPiInPlace
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
mrpt::serialization::CSchemeArchiveBase::readTo
void readTo(CSerializable &obj)
Definition: CSchemeArchiveBase.h:140
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::math::normalPDF
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
random.h
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::math::MatrixBase::inverse_LLt
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
Definition: MatrixBase_impl.h:195
mrpt::poses::CPosePDFGaussianInf::inverse
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPosePDFGaussianInf.cpp:317
mrpt::math::MatrixBase::inverse
Derived inverse() const
Returns the inverse of a general matrix using LU.
Definition: MatrixBase_impl.h:183
mrpt::poses::CPosePDFGaussianInf::saveToTextFile
bool saveToTextFile(const std::string &file) const override
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
Definition: CPosePDFGaussianInf.cpp:173
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::poses::CPosePDFGaussianInf::CPosePDFGaussianInf
CPosePDFGaussianInf()
Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!...
Definition: CPosePDFGaussianInf.cpp:39
mrpt::poses::CPosePDFGaussian::inverseComposition
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
Definition: CPosePDFGaussian.cpp:429
mrpt::poses::CPosePDFGaussianInf::cov_inv
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
Definition: CPosePDFGaussianInf.h:53
mrpt::math::multiply_HtCH_scalar
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
Definition: ops_matrices.h:54
mrpt::poses::CPosePDFGaussianInf::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPosePDFGaussianInf.cpp:64
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::math::multiply_HCHt
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
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
ops_matrices.h
mrpt::poses::CPosePDFGaussianInf::evaluatePDF
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
Definition: CPosePDFGaussianInf.cpp:353
mrpt::poses::CPose3DPDFGaussianInf
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Definition: CPose3DPDFGaussianInf.h:39
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
distributions.h
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixDynamic.h:540
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
CSchemeArchiveBase.h
mrpt::poses::CPosePDFGaussianInf
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
Definition: CPosePDFGaussianInf.h:33
A
Definition: is_defined_unittest.cpp:13
mrpt::math::CProbabilityDensityFunction::getMeanVal
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition: CProbabilityDensityFunction.h:77
mrpt::math::MatrixVectorBase::saveToTextFile
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
Definition: MatrixVectorBase_impl.h:159
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::random::CRandomGenerator::drawGaussianMultivariate
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
Definition: RandomGenerators.h:255
mrpt::math::MatrixVectorBase::transpose
auto transpose()
Definition: MatrixVectorBase.h:159
mrpt::math::CVectorDynamic< double >
CPosePDFGaussianInf.h
CPose3D.h
mrpt::poses::CPosePDF
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition: CPosePDF.h:38
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:57
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
mrpt::poses::CPosePDFGaussianInf::inverseComposition
void inverseComposition(const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
Definition: CPosePDFGaussianInf.cpp:443
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::poses::CPosePDFGaussianInf::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPosePDFGaussianInf.cpp:57
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CPosePDFGaussian.h
mrpt::poses::CPosePDFGaussianInf::drawSingleSample
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
Definition: CPosePDFGaussianInf.cpp:235
mrpt::math::CProbabilityDensityFunction::getMean
virtual void getMean(type_value &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
mrpt::poses::operator==
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
CPose3DPDF.h
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::math::CProbabilityDensityFunction::getCovariance
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition: CProbabilityDensityFunction.h:88
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::math::CMatrixDouble31
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:372
mrpt::poses::CPosePDFGaussianInf::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPosePDFGaussianInf.cpp:58
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::system
Definition: backtrace.h:14
SCHEMA_SERIALIZE_DATATYPE_VERSION
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
Definition: CSerializable.h:131
mrpt::poses::CPosePDF::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020