Main MRPT website > C++ reference for MRPT 1.9.9
CPose3DPDFGaussian.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 
12 #include <mrpt/random.h>
17 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/system/os.h>
21 
22 #include <sstream>
23 
24 using namespace mrpt;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace mrpt::random;
28 
29 using namespace mrpt::system;
30 using namespace std;
31 
33 
35 {
37 }
39 {
41 }
42 
44 
45 /*---------------------------------------------------------------
46  Constructor
47  ---------------------------------------------------------------*/
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  TConstructorFlags_Poses constructor_dummy_param)
55 {
56  MRPT_UNUSED_PARAM(constructor_dummy_param);
57 }
58 
59 /*---------------------------------------------------------------
60  Constructor
61  ---------------------------------------------------------------*/
63  const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
64  : mean(init_Mean), cov(init_Cov)
65 {
66 }
67 
68 /*---------------------------------------------------------------
69  Copy Constructor from 2D PDF
70  ---------------------------------------------------------------*/
72  : mean(o.mean.x(), o.mean.y(), 0, o.mean.phi(), 0, 0), cov()
73 {
74  for (size_t i = 0; i < 3; i++)
75  {
76  const size_t ii = (i == 2) ? 3 : i;
77  for (size_t j = 0; j < 3; j++)
78  {
79  const size_t jj = (j == 2) ? 3 : j;
80  cov(ii, jj) = o.cov(i, j);
81  }
82  }
83 }
84 
85 /*---------------------------------------------------------------
86  Constructor
87  ---------------------------------------------------------------*/
89  : mean(init_Mean), cov()
90 {
91 }
92 
93 //#define DO_TEST_JACOB
94 
95 #ifdef DO_TEST_JACOB
96 void ffff(
97  const CVectorDouble& x, const CQuaternionDouble& Q, CVectorDouble& OUT)
98 {
99  OUT.resize(3);
100  CQuaternionDouble q(x[0], x[1], x[2], x[3]);
101  q.normalize();
102  q.rpy(OUT[2], OUT[1], OUT[0]);
103 }
104 #endif
105 
107  const CArrayDouble<7>& x, const double& dummy, CArrayDouble<6>& y)
108 {
109  MRPT_UNUSED_PARAM(dummy);
110  y[0] = x[0];
111  y[1] = x[1];
112  y[2] = x[2];
113  CQuaternionDouble q(x[3], x[4], x[5], x[6]);
114  q.normalize();
115  q.rpy(y[5], y[4], y[3]);
116 }
117 
118 /*---------------------------------------------------------------
119  CPose3DPDFGaussian
120  ---------------------------------------------------------------*/
123 {
124  this->copyFrom(o);
125 }
126 
127 /*---------------------------------------------------------------
128  asString
129  ---------------------------------------------------------------*/
131 {
132  ostringstream ss;
133  ss << *this;
134  s = ss.str();
135 }
136 
137 /*---------------------------------------------------------------
138  copyFrom
139  ---------------------------------------------------------------*/
141 {
142  MRPT_START
144  {
145 // Convert using Jacobians and first order approximation:
146 
147 // [ I_3 | 0 ]
148 // dr_dq = [ -------+------------- ]
149 // [ 0 | dr_dq_angles ]
150 #ifdef DO_TEST_JACOB
151  // Test Jacob:
152  {
153  CVectorDouble x(4);
154  for (int i = 0; i < 4; i++) x[i] = o.mean.quat()[i];
155  CVectorDouble Ax(4);
156  Ax.assign(1e-7);
157  CMatrixDouble H;
158  jacobians::jacob_numeric_estimate(x, ffff, Ax, o.mean.quat(), H);
159  cout << "num:" << endl << H << endl << endl;
160  CMatrixDouble J;
161  double a, b, c;
162  o.mean.quat().rpy_and_jacobian(a, b, c, &J);
163  CMatrixDouble NJ;
165  cout << "lin:" << endl << J * NJ << endl << endl;
166  }
167 #endif
168 
169  double yaw, pitch, roll;
171 
172  o.mean.quat().rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);
173 
175  o.mean.quat().normalizationJacobian(dnorm_dq);
176 
178  dr_dq_sub.multiply(dr_dq_sub_aux, dnorm_dq);
179 
180  // Set the mean:
181  this->mean.setFromValues(
182  o.mean.x(), o.mean.y(), o.mean.z(), yaw, pitch, roll);
183 
184  // Cov:
188  o.cov.extractMatrix(3, 3, cov_Q);
189  o.cov.extractMatrix(0, 0, cov_T);
190  o.cov.extractMatrix(0, 3, cov_TQ);
191 
192  // [ S_T | S_TQ * H^t ]
193  // [ -----------------+---------------- ]
194  // [ (S_TQ * H^t)^t | H * S_Q * H^t ]
195 
196  // top-left:
197  this->cov.insertMatrix(0, 0, cov_T);
198 
199  // diagonals:
201  cov_TR.multiply_ABt(cov_TQ, dr_dq_sub);
202  this->cov.insertMatrix(0, 3, cov_TR);
203  this->cov.insertMatrixTranspose(3, 0, cov_TR);
204 
205  // bottom-right:
207  dr_dq_sub.multiply_HCHt(cov_Q, cov_r);
208  this->cov.insertMatrix(3, 3, cov_r);
209  }
210  else
211  {
212  // Use UT transformation:
213  // f: R^7 => R^6
214  const CArrayDouble<7> x_mean(o.mean);
215  CArrayDouble<6> y_mean;
216  static const bool elements_do_wrapPI[6] = {
217  false, false, false, true, true, true}; // xyz yaw pitch roll
218 
219  static const double dummy = 0;
221  x_mean, o.cov, aux_posequat2poseypr, dummy, y_mean, this->cov,
222  elements_do_wrapPI);
223  this->mean.setFromValues(
224  y_mean[0], y_mean[1], y_mean[2], y_mean[3], y_mean[4], y_mean[5]);
225  }
226  MRPT_END
227 }
228 
231 {
232  out << mean;
234 }
237 {
238  switch (version)
239  {
240  case 1:
241  {
242  in >> mean;
244  }
245  break;
246  default:
248  };
249 }
250 
252 {
253  if (this == &o) return; // It may be used sometimes
254 
255  // Convert to gaussian pdf:
257 }
258 
260 {
261  // Convert to gaussian pdf:
262  CMatrixDouble33 C;
263  CPose2D p;
264  o.getCovarianceAndMean(C, p);
265  mean = CPose3D(p);
266 
267  cov.zeros();
268  cov.get_unsafe(0, 0) = C.get_unsafe(0, 0);
269  cov.get_unsafe(1, 1) = C.get_unsafe(1, 1);
270  cov.get_unsafe(3, 3) = C.get_unsafe(2, 2);
271 
272  cov.get_unsafe(0, 1) = cov.get_unsafe(1, 0) = C.get_unsafe(0, 1);
273 
274  cov.get_unsafe(0, 3) = cov.get_unsafe(3, 0) = C.get_unsafe(0, 2);
275 
276  cov.get_unsafe(1, 3) = cov.get_unsafe(3, 1) = C.get_unsafe(1, 2);
277 }
278 
279 /*---------------------------------------------------------------
280 
281  ---------------------------------------------------------------*/
282 bool CPose3DPDFGaussian::saveToTextFile(const string& file) const
283 {
284  FILE* f = os::fopen(file.c_str(), "wt");
285  if (!f) return false;
286 
287  os::fprintf(
288  f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
289  mean.pitch(), mean.roll());
290 
291  for (unsigned int i = 0; i < 6; i++)
292  os::fprintf(
293  f, "%e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
294  cov(i, 3), cov(i, 4), cov(i, 5));
295 
296  os::fclose(f);
297  return true;
298 }
299 
300 /*---------------------------------------------------------------
301  changeCoordinatesReference
302  ---------------------------------------------------------------*/
304  const CPose3D& newReferenceBase)
305 {
306  MRPT_START
307  // this = p (+) this
308 
309  // COV:
310  const CMatrixDouble66 OLD_COV = this->cov;
312 
314  newReferenceBase, // x
315  this->mean, // u
316  df_dx, df_du);
317 
318  // this->cov = H1*this->cov*~H1 + H2* 0 *~H2;
319  df_du.multiply_HCHt(OLD_COV, cov);
320 
321  // MEAN:
322  this->mean.composeFrom(newReferenceBase, this->mean);
323 
324  MRPT_END
325 }
326 
327 /*---------------------------------------------------------------
328  drawSingleSample
329  ---------------------------------------------------------------*/
331 {
332  MRPT_START
333 
336 
337  outPart.setFromValues(
338  mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
339  mean.pitch() + v[4], mean.roll() + v[5]);
340 
342  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
343 }
344 
345 /*---------------------------------------------------------------
346  drawManySamples
347  ---------------------------------------------------------------*/
349  size_t N, vector<CVectorDouble>& outSamples) const
350 {
351  MRPT_START
352 
354 
355  for (vector<CVectorDouble>::iterator it = outSamples.begin();
356  it != outSamples.end(); ++it)
357  {
358  (*it)[0] += mean.x();
359  (*it)[1] += mean.y();
360  (*it)[2] += mean.z();
361  (*it)[3] = math::wrapToPi((*it)[3] + mean.yaw());
362  (*it)[4] = math::wrapToPi((*it)[4] + mean.pitch());
363  (*it)[5] = math::wrapToPi((*it)[5] + mean.roll());
364  }
365 
366  MRPT_END
367 }
368 
369 /*---------------------------------------------------------------
370  bayesianFusion
371  ---------------------------------------------------------------*/
373  const CPose3DPDF& p1_, const CPose3DPDF& p2_)
374 {
375  MRPT_UNUSED_PARAM(p1_);
376  MRPT_UNUSED_PARAM(p2_);
377  MRPT_START
378 
379  THROW_EXCEPTION("TO DO!!!");
380 
381  /* ASSERT_( p1_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
382  ASSERT_( p2_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
383 
384  CPose3DPDFGaussian *p1 = (CPose3DPDFGaussian*) &p1_;
385  CPose3DPDFGaussian *p2 = (CPose3DPDFGaussian*) &p2_;
386 
387 
388  CMatrixD x1(3,1),x2(3,1),x(3,1);
389  CMatrixD C1( p1->cov );
390  CMatrixD C2( p2->cov );
391  CMatrixD C1_inv = C1.inv();
392  CMatrixD C2_inv = C2.inv();
393  CMatrixD C;
394 
395  x1(0,0) = p1->mean.x; x1(1,0) = p1->mean.y; x1(2,0) = p1->mean.phi;
396  x2(0,0) = p2->mean.x; x2(1,0) = p2->mean.y; x2(2,0) = p2->mean.phi;
397 
398  C = !(C1_inv + C2_inv);
399 
400  this->cov = C;
401  this->assureSymmetry();
402 
403  x = C * ( C1_inv*x1 + C2_inv*x2 );
404 
405  this->mean.x = x(0,0);
406  this->mean.y = x(1,0);
407  this->mean.phi = x(2,0);
408  this->mean.normalizePhi();
409  */
410  MRPT_END
411 }
412 
413 /*---------------------------------------------------------------
414  inverse
415  ---------------------------------------------------------------*/
417 {
419  CPose3DPDFGaussian& out = static_cast<CPose3DPDFGaussian&>(o);
420 
421  // This is like: b=(0,0,0)
422  // OUT = b - THIS
423  CPose3DPDFGaussian p_zero(
424  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
425 
426  out = p_zero - *this;
427 }
428 
429 /*---------------------------------------------------------------
430  +=
431  ---------------------------------------------------------------*/
433 {
434  // COV:
435  const CMatrixDouble66 OLD_COV = this->cov;
437 
439  this->mean, // x
440  Ap, // u
441  df_dx, df_du);
442 
443  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
444  df_dx.multiply_HCHt(OLD_COV, cov);
445  // df_du: Nothing to do, since COV(Ap) = zeros
446 
447  // MEAN:
448  this->mean.composeFrom(this->mean, Ap);
449 }
450 
451 /*---------------------------------------------------------------
452  +=
453  ---------------------------------------------------------------*/
455 {
456  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
457  // Make a way around them and consider instead this path:
458  //
459  // X(6D) U(6D)
460  // | |
461  // v v
462  // X(7D) U(7D)
463  // | |
464  // +--- (+) ---+
465  // |
466  // v
467  // RES(7D)
468  // |
469  // v
470  // RES(6D)
471  //
472  CPose3DQuatPDFGaussian X7(*this);
473  const CPose3DQuatPDFGaussian U7(Ap);
474 
475  X7 += U7;
476 
477  this->copyFrom(X7);
478 }
479 
480 /*---------------------------------------------------------------
481  -=
482  ---------------------------------------------------------------*/
484 {
485  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
486  // Make a way around them and consider instead this path:
487  //
488  // X(6D) U(6D)
489  // | |
490  // v v
491  // X(7D) U(7D)
492  // | |
493  // +--- (-) ---+
494  // |
495  // v
496  // RES(7D)
497  // |
498  // v
499  // RES(6D)
500  //
501  CPose3DQuatPDFGaussian X7(*this);
502  const CPose3DQuatPDFGaussian U7(Ap);
503 
504  X7 -= U7;
505 
506  this->copyFrom(X7);
507 }
508 
509 /*---------------------------------------------------------------
510  evaluatePDF
511  ---------------------------------------------------------------*/
513 {
515  THROW_EXCEPTION("TO DO!!!");
516 
517  /* CMatrixD X(6,1);
518  X(0,0) = x.x;
519  X(1,0) = x.y;
520  X(2,0) = x.z;
521 
522  CMatrixD MU(6,1);
523  MU(0,0) = mean.x;
524  MU(1,0) = mean.y;
525  MU(2,0) = mean.z;
526 
527  return math::normalPDF( X, MU, this->cov );
528  */
529 }
530 
531 /*---------------------------------------------------------------
532  evaluateNormalizedPDF
533  ---------------------------------------------------------------*/
535 {
537  THROW_EXCEPTION("TO DO!!!");
538  /* CMatrixD X(3,1);
539  X(0,0) = x.x;
540  X(1,0) = x.y;
541  X(2,0) = x.phi;
542 
543  CMatrixD MU(3,1);
544  MU(0,0) = mean.x;
545  MU(1,0) = mean.y;
546  MU(2,0) = mean.phi;
547 
548  return math::normalPDF( X, MU, this->cov ) / math::normalPDF( MU, MU,
549  this->cov );
550  */
551 }
552 
553 /*---------------------------------------------------------------
554  assureSymmetry
555  ---------------------------------------------------------------*/
557 {
558  // Differences, when they exist, appear in the ~15'th significant
559  // digit, so... just take one of them arbitrarily!
560  for (unsigned int i = 0; i < cov.rows() - 1; i++)
561  for (unsigned int j = i + 1; j < cov.rows(); j++)
562  cov.get_unsafe(i, j) = cov.get_unsafe(j, i);
563 }
564 
565 /*---------------------------------------------------------------
566  mahalanobisDistanceTo
567  ---------------------------------------------------------------*/
569  const CPose3DPDFGaussian& theOther)
570 {
571  MRPT_START
572 
573  CMatrixDouble66 COV_ = cov + theOther.cov;
575 
576  for (int i = 0; i < 6; i++)
577  {
578  if (COV_.get_unsafe(i, i) == 0)
579  {
580  if (MU.get_unsafe(i, 0) != 0)
581  return std::numeric_limits<double>::infinity();
582  else
583  COV_.get_unsafe(i, i) = 1; // Any arbitrary value since
584  // MU(i)=0, and this value doesn't
585  // affect the result.
586  }
587  }
588 
589  return std::sqrt(MU.multiply_HtCH_scalar(COV_.inv()));
590 
591  MRPT_END
592 }
593 
594 /*---------------------------------------------------------------
595  operator <<
596  ---------------------------------------------------------------*/
597 ostream& mrpt::poses::operator<<(ostream& out, const CPose3DPDFGaussian& obj)
598 {
599  out << "Mean: " << obj.mean << "\n";
600  out << "Covariance:\n" << obj.cov << "\n";
601 
602  return out;
603 }
604 
605 /*---------------------------------------------------------------
606  getCovSubmatrix2D
607  ---------------------------------------------------------------*/
609 {
610  out_cov.setSize(3, 3);
611 
612  for (int i = 0; i < 3; i++)
613  {
614  int a = i == 2 ? 3 : i;
615  for (int j = i; j < 3; j++)
616  {
617  int b = j == 2 ? 3 : j;
618  double f = cov.get_unsafe(a, b);
619  out_cov.set_unsafe(i, j, f);
620  out_cov.set_unsafe(j, i, f);
621  }
622  }
623 }
624 
626  const CPose3DPDFGaussian& p1, const CPose3DPDFGaussian& p2)
627 {
628  return p1.mean == p2.mean && p1.cov == p2.cov;
629 }
A namespace of pseudo-random numbers generators of diferent distributions.
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
Definition: CQuaternion.h:411
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:61
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
Scalar * iterator
Definition: eigen_plugins.h:26
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
CPose3D mean
The mean value.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
Definition: CPose3DPDF.cpp:141
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
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
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:59
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
STL namespace.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:268
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:65
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in ...
A numeric matrix of compile-time fixed size.
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:85
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void USE_SUT_QUAT2EULER_CONVERSION(bool value)
If set to true (false), a Scaled Unscented Transform is used instead of a linear approximation with J...
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:565
const GLubyte * c
Definition: glext.h:6313
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
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...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
CPose3DPDFGaussian()
Default constructor.
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 1x6 vectors, where each row contains a (x,y,phi) datum.
void transform_gaussian_unscented(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const bool *elem_do_wrap2pi=nullptr, const double alpha=1e-3, const double K=0, const double beta=2.0)
Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:540
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
void getCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only...
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:283
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
void aux_posequat2poseypr(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 6 > &y)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
This file implements matrix/vector text and binary serialization.
#define MRPT_END
Definition: exceptions.h:266
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
GLuint in
Definition: glext.h:7274
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void operator-=(const CPose3DPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
GLenum GLint GLint y
Definition: glext.h:3538
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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
GLsizei const GLfloat * value
Definition: glext.h:4117
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
GLenum GLint x
Definition: glext.h:3538
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:46
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
void drawSingleSample(CPose3D &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 USE_SUT_QUAT2EULER_CONVERSION_value
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
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



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