Main MRPT website > C++ reference for MRPT 1.9.9
CPosePDFGaussian.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPose3DPDF.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/math/CMatrix.h>
18 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/system/os.h>
22 
23 #include <mrpt/random.h>
24 
25 using namespace mrpt;
26 
27 using namespace mrpt::poses;
28 using namespace mrpt::math;
29 using namespace mrpt::random;
30 using namespace mrpt::system;
31 
32 using namespace std;
33 
35 
36 /*---------------------------------------------------------------
37  Constructor
38  ---------------------------------------------------------------*/
40 /*---------------------------------------------------------------
41  Constructor
42  ---------------------------------------------------------------*/
44  const CPose2D& init_Mean, const CMatrixDouble33& init_Cov)
45  : mean(init_Mean), cov(init_Cov)
46 {
47 }
48 
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  : mean(init_Mean), cov()
54 {
55  cov.zeros();
56 }
57 
60 {
61  out << mean;
63 }
66 {
67  switch (version)
68  {
69  case 2:
70  {
71  in >> mean;
73  }
74  break;
75  case 1:
76  {
77  in >> mean;
80  cov = m.cast<double>();
81  }
82  break;
83  case 0:
84  {
85  CMatrix auxCov;
86  in >> mean >> auxCov;
87  cov = auxCov.cast<double>();
88  }
89  break;
90  default:
92  };
93 }
94 
95 /*---------------------------------------------------------------
96  copyFrom
97  ---------------------------------------------------------------*/
99 {
100  if (this == &o) return; // It may be used sometimes
101 
102  // Convert to gaussian pdf:
103  o.getMean(mean);
104  o.getCovariance(cov);
105 }
106 
107 /*---------------------------------------------------------------
108  copyFrom 3D
109  ---------------------------------------------------------------*/
111 {
112  // Convert to gaussian pdf:
113  mean = CPose2D(o.getMeanVal());
114  CMatrixDouble66 C;
115  o.getCovariance(C);
116 
117  // Clip to 3x3:
118  C(2, 0) = C(0, 2) = C(0, 3);
119  C(2, 1) = C(1, 2) = C(1, 3);
120  C(2, 2) = C(3, 3);
121  cov = C.block(0, 0, 3, 3);
122 }
123 
124 /*---------------------------------------------------------------
125 
126  ---------------------------------------------------------------*/
128 {
129  FILE* f = os::fopen(file.c_str(), "wt");
130  if (!f) return false;
131 
132  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
133 
134  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
135  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
136  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
137 
138  os::fclose(f);
139  return true;
140 }
141 
142 /*---------------------------------------------------------------
143  changeCoordinatesReference
144  ---------------------------------------------------------------*/
146  const CPose3D& newReferenceBase_)
147 {
148  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
149 
150  // The mean:
151  mean.composeFrom(newReferenceBase, mean);
152 
153  // The covariance:
154  rotateCov(newReferenceBase.phi());
155 }
156 
157 /*---------------------------------------------------------------
158  changeCoordinatesReference
159  ---------------------------------------------------------------*/
161  const CPose2D& newReferenceBase)
162 {
163  // The mean:
164  mean.composeFrom(newReferenceBase, mean);
165  // The covariance:
166  rotateCov(newReferenceBase.phi());
167 }
168 
169 /*---------------------------------------------------------------
170  changeCoordinatesReference
171  ---------------------------------------------------------------*/
172 void CPosePDFGaussian::rotateCov(const double ang)
173 {
174  const double ccos = cos(ang);
175  const double ssin = sin(ang);
176 
177  alignas(MRPT_MAX_ALIGN_BYTES)
178  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
179 
180  const CMatrixFixedNumeric<double, 3, 3> rot(rot_vals);
181  cov = (rot * cov * rot.adjoint()).eval();
182 }
183 
184 /*---------------------------------------------------------------
185  drawSingleSample
186  ---------------------------------------------------------------*/
188 {
189  MRPT_START
190 
193 
194  outPart.x(mean.x() + v[0]);
195  outPart.y(mean.y() + v[1]);
196  outPart.phi(mean.phi() + v[2]);
197 
198  // Range -pi,pi
199  outPart.normalizePhi();
200 
202  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
203 }
204 
205 /*---------------------------------------------------------------
206  drawManySamples
207  ---------------------------------------------------------------*/
209  size_t N, std::vector<CVectorDouble>& outSamples) const
210 {
211  MRPT_START
212 
213  std::vector<CVectorDouble> rndSamples;
214 
216  outSamples.resize(N);
217  for (unsigned int i = 0; i < N; i++)
218  {
219  outSamples[i].resize(3);
220  outSamples[i][0] = mean.x() + rndSamples[i][0];
221  outSamples[i][1] = mean.y() + rndSamples[i][1];
222  outSamples[i][2] = mean.phi() + rndSamples[i][2];
223 
224  wrapToPiInPlace(outSamples[i][2]);
225  }
226 
227  MRPT_END
228 }
229 
230 /*---------------------------------------------------------------
231  bayesianFusion
232  ---------------------------------------------------------------*/
234  const CPosePDF& p1_, const CPosePDF& p2_,
235  const double minMahalanobisDistToDrop)
236 {
237  MRPT_START
238 
239  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
240 
243 
244  const CPosePDFGaussian* p1 = static_cast<const CPosePDFGaussian*>(&p1_);
245  const CPosePDFGaussian* p2 = static_cast<const CPosePDFGaussian*>(&p2_);
246 
247  CMatrixDouble33 C1 = p1->cov;
248  CMatrixDouble33 C2 = p2->cov;
249 
250  CMatrixDouble33 C1_inv;
251  C1.inv(C1_inv);
252 
253  CMatrixDouble33 C2_inv;
254  C2.inv(C2_inv);
255 
256  CMatrixDouble31 x1 = CMatrixDouble31(p1->mean);
258 
259  CMatrixDouble33 auxC = C1_inv + C2_inv;
260  auxC.inv(this->cov);
261  this->assureSymmetry();
262 
263  CMatrixDouble31 x = this->cov * (C1_inv * x1 + C2_inv * x2);
264 
265  this->mean.x(x(0, 0));
266  this->mean.y(x(1, 0));
267  this->mean.phi(x(2, 0));
268  this->mean.normalizePhi();
269 
270  MRPT_END
271 }
272 
273 /*---------------------------------------------------------------
274  inverse
275  ---------------------------------------------------------------*/
277 {
279  CPosePDFGaussian* out = static_cast<CPosePDFGaussian*>(&o);
280 
281  // The mean:
282  out->mean = CPose2D(0, 0, 0) - mean;
283 
284  // The covariance:
285  const double ccos = ::cos(mean.phi());
286  const double ssin = ::sin(mean.phi());
287 
288  // jacobian:
289  alignas(MRPT_MAX_ALIGN_BYTES) const double H_values[] = {
290  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
291  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
292  0, 0, -1};
293  const CMatrixFixedNumeric<double, 3, 3> H(H_values);
294 
295  out->cov.noalias() =
296  (H * cov * H.adjoint()).eval(); // o.cov = H * cov * Ht
297 }
298 
299 /*---------------------------------------------------------------
300  +=
301  ---------------------------------------------------------------*/
303 {
304  mean = mean + Ap;
305  rotateCov(Ap.phi());
306 }
307 
308 /*---------------------------------------------------------------
309  evaluatePDF
310  ---------------------------------------------------------------*/
312 {
315 
316  return math::normalPDF(X, MU, this->cov);
317 }
318 
319 /*---------------------------------------------------------------
320  evaluateNormalizedPDF
321  ---------------------------------------------------------------*/
323 {
326 
327  return math::normalPDF(X, MU, this->cov) /
328  math::normalPDF(MU, MU, this->cov);
329 }
330 
331 /*---------------------------------------------------------------
332  assureSymmetry
333  ---------------------------------------------------------------*/
335 {
336  // Differences, when they exist, appear in the ~15'th significant
337  // digit, so... just take one of them arbitrarily!
338  cov(0, 1) = cov(1, 0);
339  cov(0, 2) = cov(2, 0);
340  cov(1, 2) = cov(2, 1);
341 }
342 
343 /*---------------------------------------------------------------
344  mahalanobisDistanceTo
345  ---------------------------------------------------------------*/
347 {
348  MRPT_START
349 
351  MU -= CArrayDouble<3>(theOther.mean);
352 
353  wrapToPiInPlace(MU[2]);
354 
355  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
356  return 0; // This is the ONLY case where we know the result, whatever
357  // COVs are.
358 
359  CMatrixDouble33 COV_ = cov;
360  COV_ += theOther.cov;
361 
363  COV_.inv_fast(COV_inv);
364 
365  // (~MU) * (!COV_) * MU
366  return std::sqrt(mrpt::math::multiply_HCHt_scalar(MU, COV_inv));
367 
368  MRPT_END
369 }
370 
371 /*---------------------------------------------------------------
372  operator <<
373  ---------------------------------------------------------------*/
375  std::ostream& out, const CPosePDFGaussian& obj)
376 {
377  out << "Mean: " << obj.mean << "\n";
378  out << "Covariance:\n" << obj.cov << "\n";
379 
380  return out;
381 }
382 
383 /*---------------------------------------------------------------
384  operator +
385  ---------------------------------------------------------------*/
388 {
391  return ret;
392 }
393 
394 /*---------------------------------------------------------------
395  assureMinCovariance
396  ---------------------------------------------------------------*/
398  const double& minStdXY, const double& minStdPhi)
399 {
400  cov(0, 0) = max(cov(0, 0), square(minStdXY));
401  cov(1, 1) = max(cov(1, 1), square(minStdXY));
402  cov(2, 2) = max(cov(2, 2), square(minStdPhi));
403 }
404 
405 /*---------------------------------------------------------------
406  inverseComposition
407  Set 'this' = 'x' - 'ref', computing the mean using the "-"
408  operator and the covariances through the corresponding Jacobians.
409  ---------------------------------------------------------------*/
411  const CPosePDFGaussian& xv, const CPosePDFGaussian& xi)
412 {
413  // COV:
414  double cpi = cos(xi.mean.phi());
415  double spi = sin(xi.mean.phi());
416 
417  // jacob: dh_xv
418  CMatrixDouble33 dh_xv;
419 
420  dh_xv(0, 0) = cpi;
421  dh_xv(0, 1) = spi;
422  dh_xv(1, 0) = -spi;
423  dh_xv(1, 1) = cpi;
424  dh_xv(2, 2) = 1;
425 
426  // jacob: dh_xi
427  CMatrixDouble33 dh_xi;
428 
429  double xv_xi = xv.mean.x() - xi.mean.x();
430  double yv_yi = xv.mean.y() - xi.mean.y();
431 
432  dh_xi(0, 0) = -cpi;
433  dh_xi(0, 1) = -spi;
434  dh_xi(0, 2) = -xv_xi * spi + yv_yi * cpi;
435  dh_xi(1, 0) = spi;
436  dh_xi(1, 1) = -cpi;
437  dh_xi(1, 2) = -xv_xi * cpi - yv_yi * spi;
438  dh_xi(2, 2) = -1;
439 
440  // Build the cov:
441  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T
442  dh_xv.multiply_HCHt(xv.cov, this->cov);
443  dh_xi.multiply_HCHt(xi.cov, this->cov, true); // Accum. result
444 
445  // Mean:
446  mean = xv.mean - xi.mean;
447 }
448 
449 /*---------------------------------------------------------------
450  inverseComposition
451  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
452  the "-" operator and the covariances through the corresponding
453  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
454  ---------------------------------------------------------------*/
456  const CPosePDFGaussian& x1, const CPosePDFGaussian& x0,
457  const CMatrixDouble33& COV_01)
458 {
459  double cp0 = cos(x0.mean.phi());
460  double sp0 = sin(x0.mean.phi());
461 
462  // jacob: dh_x1
463  CMatrixDouble33 dh_x1;
464 
465  dh_x1(0, 0) = cp0;
466  dh_x1(0, 1) = sp0;
467  dh_x1(1, 0) = -sp0;
468  dh_x1(1, 1) = cp0;
469  dh_x1(2, 2) = 1;
470 
471  // jacob: dh_x0
472  CMatrixDouble33 dh_x0;
473 
474  double xv_xi = x1.mean.x() - x0.mean.x();
475  double yv_yi = x1.mean.y() - x0.mean.y();
476 
477  dh_x0(0, 0) = -cp0;
478  dh_x0(0, 1) = -sp0;
479  dh_x0(0, 2) = -xv_xi * sp0 + yv_yi * cp0;
480  dh_x0(1, 0) = sp0;
481  dh_x0(1, 1) = -cp0;
482  dh_x0(1, 2) = -xv_xi * cp0 - yv_yi * sp0;
483  dh_x0(2, 2) = -1;
484 
485  // Build the cov:
486  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T + A + At
487  // being A = dh_dx0 * COV01 * dh_dx1^t
488  dh_x0.multiply_HCHt(x0.cov, this->cov);
489  dh_x1.multiply_HCHt(x1.cov, this->cov, true); // Accum. result
490 
491  CMatrixDouble33 M;
492  M.multiply_ABCt(dh_x0, COV_01, dh_x1);
493 
494  this->cov.add_AAt(M);
495 
496  // mean
497  mean = x1.mean - x0.mean;
498 }
499 
500 /*---------------------------------------------------------------
501  +=
502  ---------------------------------------------------------------*/
504 {
505  // COV:
506  const CMatrixDouble33 OLD_COV = this->cov;
508 
510  this->mean, // x
511  Ap.mean, // u
512  df_dx, df_du);
513 
514  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
515  df_dx.multiply_HCHt(OLD_COV, cov);
516  df_du.multiply_HCHt(Ap.cov, cov, true); // Accumulate result
517 
518  // MEAN:
519  this->mean = this->mean + Ap.mean;
520 }
521 
522 /** Returns the PDF of the 2D point \f$ g = q \oplus l\f$ with "q"=this pose and
523  * "l" a point without uncertainty */
525  const mrpt::math::TPoint2D& l, CPoint2DPDFGaussian& g) const
526 {
527  // Mean:
528  double gx, gy;
529  mean.composePoint(l.x, l.y, gx, gy);
530  g.mean.x(gx);
531  g.mean.y(gy);
532 
533  // COV:
536  this->mean, // x
537  this->mean, // u
538  df_dx, df_du,
539  true, // Eval df_dx
540  false // Eval df_du (not needed)
541  );
542 
543  const CMatrixFixedNumeric<double, 2, 3> dp_dx = df_dx.block<2, 3>(0, 0);
544  g.cov = dp_dx * this->cov * dp_dx.transpose();
545 }
546 
548  const CPosePDFGaussian& p1, const CPosePDFGaussian& p2)
549 {
550  return p1.mean == p2.mean && p1.cov == p2.cov;
551 }
552 
554  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
555 {
557  res += b;
558  return res;
559 }
560 
562  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
563 {
565  res.inverseComposition(a, b);
566  return res;
567 }
A namespace of pseudo-random numbers generators of diferent distributions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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:262
CPose2D mean
The mean value.
#define MRPT_MAX_ALIGN_BYTES
double x
X,Y coordinates.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.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:273
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in 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:364
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:69
STL namespace.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
TDATA getMeanVal() const
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:268
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:175
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:64
This base provides a set of functions for maths stuff.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:62
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:85
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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...
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:315
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.
GLubyte g
Definition: glext.h:6279
GLubyte GLubyte b
Definition: glext.h:6279
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.
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
GLsizei const GLchar ** string
Definition: glext.h:4101
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:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void assureMinCovariance(const double &minStdXY, const double &minStdPhi)
Substitutes the diagonal elements if (square) they are below some given minimum values (Use this befo...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
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.
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:111
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
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.
Lightweight 2D point.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42
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...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:292
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:140
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019