MRPT  2.0.4
CPosePDFGaussian.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/CMatrixF.h>
15 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/CPose3DPDF.h>
20 #include <mrpt/random.h>
23 #include <mrpt/system/os.h>
24 #include <Eigen/Dense>
25 
26 using namespace mrpt;
27 using namespace mrpt::poses;
28 using namespace mrpt::math;
29 using namespace mrpt::random;
30 using namespace mrpt::system;
31 using namespace std;
32 
34 
35 /*---------------------------------------------------------------
36  Constructor
37  ---------------------------------------------------------------*/
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  const CPose2D& init_Mean, const CMatrixDouble33& init_Cov)
44  : mean(init_Mean), cov(init_Cov)
45 {
46 }
47 
48 /*---------------------------------------------------------------
49  Constructor
50  ---------------------------------------------------------------*/
52  : mean(init_Mean), cov()
53 {
54  cov.setZero();
55 }
56 
57 uint8_t CPosePDFGaussian::serializeGetVersion() const { return 2; }
59 {
60  out << mean;
62 }
64  mrpt::serialization::CArchive& in, uint8_t version)
65 {
66  switch (version)
67  {
68  case 2:
69  {
70  in >> mean;
72  }
73  break;
74  case 1:
75  {
76  in >> mean;
79  cov = m.cast_double();
80  }
81  break;
82  case 0:
83  {
84  CMatrixF auxCov;
85  in >> mean >> auxCov;
86  cov = auxCov.cast_double();
87  }
88  break;
89  default:
91  };
92 }
93 
96 {
98  out["mean"] = mean;
99  out["cov"] = CMatrixD(cov);
100 }
103 {
104  uint8_t version;
106  switch (version)
107  {
108  case 1:
109  {
110  in["mean"].readTo(mean);
111  CMatrixD m;
112  in["cov"].readTo(m);
113  cov = m;
114  }
115  break;
116  default:
118  }
119 }
120 
121 /*---------------------------------------------------------------
122  copyFrom
123  ---------------------------------------------------------------*/
125 {
126  if (this == &o) return; // It may be used sometimes
127 
128  // Convert to gaussian pdf:
129  o.getMean(mean);
130  o.getCovariance(cov);
131 }
132 
133 /*---------------------------------------------------------------
134  copyFrom 3D
135  ---------------------------------------------------------------*/
137 {
138  // Convert to gaussian pdf:
139  mean = CPose2D(o.getMeanVal());
140  CMatrixDouble66 C;
141  o.getCovariance(C);
142 
143  // Clip to 3x3:
144  C(2, 0) = C(0, 2) = C(0, 3);
145  C(2, 1) = C(1, 2) = C(1, 3);
146  C(2, 2) = C(3, 3);
147  cov = C.block<3, 3>(0, 0);
148 }
149 
150 /*---------------------------------------------------------------
151 
152  ---------------------------------------------------------------*/
153 bool CPosePDFGaussian::saveToTextFile(const std::string& file) const
154 {
155  FILE* f = os::fopen(file.c_str(), "wt");
156  if (!f) return false;
157 
158  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
159 
160  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
161  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
162  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
163 
164  os::fclose(f);
165  return true;
166 }
167 
168 /*---------------------------------------------------------------
169  changeCoordinatesReference
170  ---------------------------------------------------------------*/
172  const CPose3D& newReferenceBase_)
173 {
174  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
175 
176  // The mean:
177  mean.composeFrom(newReferenceBase, mean);
178 
179  // The covariance:
180  rotateCov(newReferenceBase.phi());
181 }
182 
183 /*---------------------------------------------------------------
184  changeCoordinatesReference
185  ---------------------------------------------------------------*/
187  const CPose2D& newReferenceBase)
188 {
189  // The mean:
190  mean.composeFrom(newReferenceBase, mean);
191  // The covariance:
192  rotateCov(newReferenceBase.phi());
193 }
194 
195 /*---------------------------------------------------------------
196  changeCoordinatesReference
197  ---------------------------------------------------------------*/
198 void CPosePDFGaussian::rotateCov(const double ang)
199 {
200  const double ccos = cos(ang);
201  const double ssin = sin(ang);
202 
203  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
204  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
205 
206  const CMatrixFixed<double, 3, 3> rot(rot_vals);
207  cov = (rot.asEigen() * cov.asEigen() * rot.asEigen().transpose()).eval();
208 }
209 
210 /*---------------------------------------------------------------
211  drawSingleSample
212  ---------------------------------------------------------------*/
214 {
215  MRPT_START
216 
217  CVectorDouble v;
219 
220  outPart.x(mean.x() + v[0]);
221  outPart.y(mean.y() + v[1]);
222  outPart.phi(mean.phi() + v[2]);
223 
224  // Range -pi,pi
225  outPart.normalizePhi();
226 
228  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
229 }
230 
231 /*---------------------------------------------------------------
232  drawManySamples
233  ---------------------------------------------------------------*/
235  size_t N, std::vector<CVectorDouble>& outSamples) const
236 {
237  MRPT_START
238 
239  std::vector<CVectorDouble> rndSamples;
240 
242  outSamples.resize(N);
243  for (size_t i = 0; i < N; i++)
244  {
245  outSamples[i].resize(3);
246  outSamples[i][0] = mean.x() + rndSamples[i][0];
247  outSamples[i][1] = mean.y() + rndSamples[i][1];
248  outSamples[i][2] = mean.phi() + rndSamples[i][2];
249 
250  wrapToPiInPlace(outSamples[i][2]);
251  }
252 
253  MRPT_END
254 }
255 
256 /*---------------------------------------------------------------
257  bayesianFusion
258  ---------------------------------------------------------------*/
260  const CPosePDF& p1_, const CPosePDF& p2_,
261  [[maybe_unused]] const double minMahalanobisDistToDrop)
262 {
263  MRPT_START
264 
267 
268  const auto* p1 = dynamic_cast<const CPosePDFGaussian*>(&p1_);
269  const auto* p2 = dynamic_cast<const CPosePDFGaussian*>(&p2_);
270 
271  const CMatrixDouble33 C1 = p1->cov;
272  const CMatrixDouble33 C2 = p2->cov;
273 
274  const CMatrixDouble33 C1_inv = C1.inverse_LLt();
275  const CMatrixDouble33 C2_inv = C2.inverse_LLt();
276 
277  auto x1 = CMatrixDouble31(p1->mean);
278  auto x2 = CMatrixDouble31(p2->mean);
279 
280  CMatrixDouble33 auxC = C1_inv + C2_inv;
281  this->cov = auxC.inverse_LLt();
282  this->enforceCovSymmetry();
283 
284  auto x = CMatrixDouble31(this->cov.asEigen() * (C1_inv * x1 + C2_inv * x2));
285 
286  this->mean.x(x(0, 0));
287  this->mean.y(x(1, 0));
288  this->mean.phi(x(2, 0));
289  this->mean.normalizePhi();
290 
291  MRPT_END
292 }
293 
294 /*---------------------------------------------------------------
295  inverse
296  ---------------------------------------------------------------*/
298 {
300  auto* out = dynamic_cast<CPosePDFGaussian*>(&o);
301 
302  // The mean:
303  out->mean = CPose2D(0, 0, 0) - mean;
304 
305  // The covariance:
306  const double ccos = ::cos(mean.phi());
307  const double ssin = ::sin(mean.phi());
308 
309  // jacobian:
310  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double H_values[] = {
311  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
312  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
313  0, 0, -1};
314  const CMatrixFixed<double, 3, 3> H(H_values);
315 
316  out->cov.asEigen().noalias() =
317  (H.asEigen() * cov.asEigen() * H.transpose()).eval();
318 }
319 
320 /*---------------------------------------------------------------
321  +=
322  ---------------------------------------------------------------*/
324 {
325  mean = mean + Ap;
326  rotateCov(Ap.phi());
327 }
328 
329 /*---------------------------------------------------------------
330  evaluatePDF
331  ---------------------------------------------------------------*/
333 {
334  auto X = CMatrixDouble31(x);
335  auto MU = CMatrixDouble31(mean);
336 
337  return math::normalPDF(X, MU, this->cov);
338 }
339 
340 /*---------------------------------------------------------------
341  evaluateNormalizedPDF
342  ---------------------------------------------------------------*/
344 {
345  auto X = CMatrixDouble31(x);
346  auto MU = CMatrixDouble31(mean);
347 
348  return math::normalPDF(X, MU, this->cov) /
349  math::normalPDF(MU, MU, this->cov);
350 }
351 
352 /*---------------------------------------------------------------
353  enforceCovSymmetry
354  ---------------------------------------------------------------*/
356 {
357  // Differences, when they exist, appear in the ~15'th significant
358  // digit, so... just take one of them arbitrarily!
359  cov(0, 1) = cov(1, 0);
360  cov(0, 2) = cov(2, 0);
361  cov(1, 2) = cov(2, 1);
362 }
363 
364 /*---------------------------------------------------------------
365  mahalanobisDistanceTo
366  ---------------------------------------------------------------*/
368 {
369  MRPT_START
370 
371  auto MU = CVectorFixedDouble<3>(mean);
372  MU -= CVectorFixedDouble<3>(theOther.mean);
373 
374  wrapToPiInPlace(MU[2]);
375 
376  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
377  return 0; // This is the ONLY case where we know the result, whatever
378  // COVs are.
379 
380  CMatrixDouble33 COV_ = cov;
381  COV_ += theOther.cov;
382 
383  const CMatrixDouble33 COV_inv = COV_.inverse_LLt();
384 
385  // (~MU) * (!COV_) * MU
386  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU.asEigen(), COV_inv));
387 
388  MRPT_END
389 }
390 
391 /*---------------------------------------------------------------
392  operator <<
393  ---------------------------------------------------------------*/
395  std::ostream& out, const CPosePDFGaussian& obj)
396 {
397  out << "Mean: " << obj.mean << "\n";
398  out << "Covariance:\n" << obj.cov << "\n";
399 
400  return out;
401 }
402 
403 /*---------------------------------------------------------------
404  operator +
405  ---------------------------------------------------------------*/
408 {
411  return ret;
412 }
413 
414 /*---------------------------------------------------------------
415  assureMinCovariance
416  ---------------------------------------------------------------*/
417 void CPosePDFGaussian::assureMinCovariance(double minStdXY, double minStdPhi)
418 {
419  cov(0, 0) = max(cov(0, 0), square(minStdXY));
420  cov(1, 1) = max(cov(1, 1), square(minStdXY));
421  cov(2, 2) = max(cov(2, 2), square(minStdPhi));
422 }
423 
424 /*---------------------------------------------------------------
425  inverseComposition
426  Set 'this' = 'x' - 'ref', computing the mean using the "-"
427  operator and the covariances through the corresponding Jacobians.
428  ---------------------------------------------------------------*/
430  const CPosePDFGaussian& xv, const CPosePDFGaussian& xi)
431 {
432  // COV:
433  double cpi = cos(xi.mean.phi());
434  double spi = sin(xi.mean.phi());
435 
436  // jacob: dh_xv
437  CMatrixDouble33 dh_xv;
438 
439  dh_xv(0, 0) = cpi;
440  dh_xv(0, 1) = spi;
441  dh_xv(1, 0) = -spi;
442  dh_xv(1, 1) = cpi;
443  dh_xv(2, 2) = 1;
444 
445  // jacob: dh_xi
446  CMatrixDouble33 dh_xi;
447 
448  double xv_xi = xv.mean.x() - xi.mean.x();
449  double yv_yi = xv.mean.y() - xi.mean.y();
450 
451  dh_xi(0, 0) = -cpi;
452  dh_xi(0, 1) = -spi;
453  dh_xi(0, 2) = -xv_xi * spi + yv_yi * cpi;
454  dh_xi(1, 0) = spi;
455  dh_xi(1, 1) = -cpi;
456  dh_xi(1, 2) = -xv_xi * cpi - yv_yi * spi;
457  dh_xi(2, 2) = -1;
458 
459  // Build the cov:
460  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T
461  this->cov = mrpt::math::multiply_HCHt(dh_xv, xv.cov) +
462  mrpt::math::multiply_HCHt(dh_xi, xi.cov);
463 
464  // Mean:
465  mean = xv.mean - xi.mean;
466 }
467 
468 /*---------------------------------------------------------------
469  inverseComposition
470  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
471  the "-" operator and the covariances through the corresponding
472  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
473  ---------------------------------------------------------------*/
475  const CPosePDFGaussian& x1, const CPosePDFGaussian& x0,
476  const CMatrixDouble33& COV_01)
477 {
478  double cp0 = cos(x0.mean.phi());
479  double sp0 = sin(x0.mean.phi());
480 
481  // jacob: dh_x1
482  CMatrixDouble33 dh_x1;
483 
484  dh_x1(0, 0) = cp0;
485  dh_x1(0, 1) = sp0;
486  dh_x1(1, 0) = -sp0;
487  dh_x1(1, 1) = cp0;
488  dh_x1(2, 2) = 1;
489 
490  // jacob: dh_x0
491  CMatrixDouble33 dh_x0;
492 
493  double xv_xi = x1.mean.x() - x0.mean.x();
494  double yv_yi = x1.mean.y() - x0.mean.y();
495 
496  dh_x0(0, 0) = -cp0;
497  dh_x0(0, 1) = -sp0;
498  dh_x0(0, 2) = -xv_xi * sp0 + yv_yi * cp0;
499  dh_x0(1, 0) = sp0;
500  dh_x0(1, 1) = -cp0;
501  dh_x0(1, 2) = -xv_xi * cp0 - yv_yi * sp0;
502  dh_x0(2, 2) = -1;
503 
504  // Build the cov:
505  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T + A + At
506  // being A = dh_dx0 * COV01 * dh_dx1^t
507  this->cov = mrpt::math::multiply_HCHt(dh_x0, x0.cov) +
508  mrpt::math::multiply_HCHt(dh_x1, x1.cov);
509 
510  auto M =
511  CMatrixDouble33(dh_x0.asEigen() * COV_01.asEigen() * dh_x1.asEigen());
512 
513  this->cov.asEigen() += (M.asEigen() * M.transpose()).eval();
514 
515  // mean
516  mean = x1.mean - x0.mean;
517 }
518 
519 /*---------------------------------------------------------------
520  +=
521  ---------------------------------------------------------------*/
523 {
524  // COV:
525  const CMatrixDouble33 OLD_COV = this->cov;
527 
529  this->mean, // x
530  Ap.mean, // u
531  df_dx, df_du);
532 
533  this->cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV) +
534  mrpt::math::multiply_HCHt(df_du, Ap.cov);
535 
536  // MEAN:
537  this->mean = this->mean + Ap.mean;
538 }
539 
540 /** Returns the PDF of the 2D point \f$ g = q \oplus l\f$ with "q"=this pose and
541  * "l" a point without uncertainty */
543  const mrpt::math::TPoint2D& l, CPoint2DPDFGaussian& g) const
544 {
545  // Mean:
546  double gx, gy;
547  mean.composePoint(l.x, l.y, gx, gy);
548  g.mean.x(gx);
549  g.mean.y(gy);
550 
551  // COV:
554  this->mean, // x
555  this->mean, // u
556  df_dx, df_du,
557  true, // Eval df_dx
558  false // Eval df_du (not needed)
559  );
560 
561  const auto dp_dx = CMatrixFixed<double, 2, 3>(df_dx.block<2, 3>(0, 0));
562  g.cov = dp_dx * this->cov * dp_dx.transpose();
563 }
564 
566  const CPosePDFGaussian& p1, const CPosePDFGaussian& p2)
567 {
568  return p1.mean == p2.mean && p1.cov == p2.cov;
569 }
570 
572  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
573 {
574  CPosePDFGaussian res(a);
575  res += b;
576  return res;
577 }
578 
580  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
581 {
582  CPosePDFGaussian res;
583  res.inverseComposition(a, b);
584  return res;
585 }
A namespace of pseudo-random numbers generators of diferent distributions.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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
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.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
T x
X,Y coordinates.
Definition: TPoint2D.h:25
A gaussian distribution for 2D points.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:282
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
void assureMinCovariance(double minStdXY, double minStdPhi)
Substitutes the diagonal elements if (square) they are below some given minimum values (Use this befo...
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)...
double mahalanobisDistanceTo(const CPosePDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
void composePoint(const mrpt::math::TPoint2D &l, CPoint2DPDFGaussian &g) const
Returns the PDF of the 2D point with "q"=this pose and "l" a point without uncertainty.
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:199
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
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.
Virtual base class for "schematic archives" (JSON, XML,...)
#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
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.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#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
CPosePDFGaussian()
Default constructor.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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...
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
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.
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.
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
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
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...
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:415
return_t square(const num_t x)
Inline function for the square of a number.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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.
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
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
This file implements matrix/vector text and binary serialization.
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
#define MRPT_END
Definition: exceptions.h:245
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:264
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CMatrixFixed< double, ROWS, COLS > cast_double() const
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
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
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020