Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/random.h>
17 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/system/os.h>
19 #include <mrpt/utils/CStream.h>
20 
21 #include <sstream>
22 
23 using namespace mrpt;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::random;
27 using namespace mrpt::utils;
28 using namespace mrpt::system;
29 using namespace std;
30 
32 
33 
35 
36 
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41 {
42 }
43 
44 /*---------------------------------------------------------------
45  Constructor
46  ---------------------------------------------------------------*/
48 {
49  MRPT_UNUSED_PARAM(constructor_dummy_param);
50 }
51 
52 /*---------------------------------------------------------------
53  Constructor
54  ---------------------------------------------------------------*/
55 CPose3DPDFGaussian::CPose3DPDFGaussian( const CPose3D &init_Mean, const CMatrixDouble66 &init_Cov ) :
56  mean(init_Mean), cov(init_Cov)
57 {
58 }
59 
60 /*---------------------------------------------------------------
61  Copy Constructor from 2D PDF
62  ---------------------------------------------------------------*/
64  : mean( o.mean.x(),o.mean.y(),0,o.mean.phi(),0,0 ),
65  cov()
66 {
67  for (size_t i=0;i<3;i++)
68  {
69  const size_t ii= (i==2) ? 3 : i;
70  for (size_t j=0;j<3;j++)
71  {
72  const size_t jj= (j==2) ? 3 : j;
73  cov(ii,jj) = o.cov(i,j);
74  }
75  }
76 }
77 
78 
79 /*---------------------------------------------------------------
80  Constructor
81  ---------------------------------------------------------------*/
83  const CPose3D &init_Mean ) : mean(init_Mean), cov()
84 {
85 }
86 
87 
88 //#define DO_TEST_JACOB
89 
90 #ifdef DO_TEST_JACOB
91 void ffff(const CVectorDouble &x,const CQuaternionDouble &Q, CVectorDouble &OUT)
92 {
93  OUT.resize(3);
94  CQuaternionDouble q(x[0],x[1],x[2],x[3]);
95  q.normalize();
96  q.rpy(OUT[2],OUT[1],OUT[0]);
97 }
98 #endif
99 
100 void aux_posequat2poseypr(const CArrayDouble<7> &x,const double&dummy, CArrayDouble<6> &y)
101 {
102  MRPT_UNUSED_PARAM(dummy);
103  y[0]=x[0]; y[1]=x[1]; y[2]=x[2];
104  CQuaternionDouble q(x[3],x[4],x[5],x[6]);
105  q.normalize();
106  q.rpy(y[5],y[4],y[3]);
107 }
108 
109 /*---------------------------------------------------------------
110  CPose3DPDFGaussian
111  ---------------------------------------------------------------*/
114 {
115  this->copyFrom(o);
116 }
117 
118 /*---------------------------------------------------------------
119  asString
120  ---------------------------------------------------------------*/
122 {
123  ostringstream ss;
124  ss << *this;
125  s = ss.str();
126 }
127 
128 /*---------------------------------------------------------------
129  copyFrom
130  ---------------------------------------------------------------*/
132 {
133  MRPT_START
135  {
136  // Convert using Jacobians and first order approximation:
137 
138  // [ I_3 | 0 ]
139  // dr_dq = [ -------+------------- ]
140  // [ 0 | dr_dq_angles ]
141 #ifdef DO_TEST_JACOB
142  // Test Jacob:
143  {
144  CVectorDouble x(4);
145  for (int i=0;i<4;i++) x[i] = o.mean.quat()[i];
146  CVectorDouble Ax(4); Ax.assign(1e-7);
147  CMatrixDouble H;
148  jacobians::jacob_numeric_estimate(x,ffff,Ax, o.mean.quat(),H);
149  cout << "num:" <<endl <<H << endl << endl;
150  CMatrixDouble J;
151  double a,b,c;
152  o.mean.quat().rpy_and_jacobian(a,b,c,&J);
153  CMatrixDouble NJ;
155  cout << "lin:" <<endl<< J*NJ << endl << endl;
156  }
157 #endif
158 
159  double yaw,pitch,roll;
161 
162  o.mean.quat().rpy_and_jacobian(roll,pitch,yaw,&dr_dq_sub_aux,false);
163 
165  o.mean.quat().normalizationJacobian(dnorm_dq);
166 
168  dr_dq_sub.multiply(dr_dq_sub_aux,dnorm_dq);
169 
170  // Set the mean:
171  this->mean.setFromValues(o.mean.x(),o.mean.y(),o.mean.z(),yaw,pitch,roll);
172 
173  // Cov:
177  o.cov.extractMatrix(3,3,cov_Q);
178  o.cov.extractMatrix(0,0,cov_T);
179  o.cov.extractMatrix(0,3,cov_TQ);
180 
181  // [ S_T | S_TQ * H^t ]
182  // [ -----------------+---------------- ]
183  // [ (S_TQ * H^t)^t | H * S_Q * H^t ]
184 
185  // top-left:
186  this->cov.insertMatrix(0,0,cov_T);
187 
188  // diagonals:
190  cov_TR.multiply_ABt(cov_TQ,dr_dq_sub);
191  this->cov.insertMatrix (0,3,cov_TR);
192  this->cov.insertMatrixTranspose(3,0,cov_TR);
193 
194  // bottom-right:
196  dr_dq_sub.multiply_HCHt(cov_Q,cov_r);
197  this->cov.insertMatrix(3,3,cov_r);
198  }
199  else
200  {
201  // Use UT transformation:
202  // f: R^7 => R^6
203  const CArrayDouble<7> x_mean(o.mean);
204  CArrayDouble<6> y_mean;
205  static const bool elements_do_wrapPI[6] = {false,false,false,true,true,true}; // xyz yaw pitch roll
206 
207  static const double dummy=0;
209  x_mean, o.cov,
211  dummy,
212  y_mean,
213  this->cov,
214  elements_do_wrapPI
215  );
216  this->mean.setFromValues(y_mean[0],y_mean[1],y_mean[2],y_mean[3],y_mean[4],y_mean[5]);
217  }
218  MRPT_END
219 }
220 
221 /*---------------------------------------------------------------
222  writeToStream
223  ---------------------------------------------------------------*/
225 {
226  if (version)
227  *version = 1;
228  else
229  {
230  out << mean;
231  for (size_t r=0;r<size(cov,1);r++)
232  out << cov.get_unsafe(r,r);
233  for (size_t r=0;r<size(cov,1);r++)
234  for (size_t c=r+1;c<size(cov,2);c++)
235  out << cov.get_unsafe(r,c);
236  }
237 }
238 
239 /*---------------------------------------------------------------
240  readFromStream
241  ---------------------------------------------------------------*/
243 {
244  switch(version)
245  {
246  case 0:
247  {
248  in >> mean;
249 
250  for (size_t r=0;r<size(cov,1);r++)
251  in >> cov.get_unsafe(r,r);
252  for (size_t r=0;r<size(cov,1);r++)
253  for (size_t c=r+1;c<size(cov,2);c++)
254  {
255  float x;
256  in >> x;
257  cov.get_unsafe(r,c) = cov.get_unsafe(c,r) = x;
258  }
259 
260  } break;
261  case 1:
262  {
263  in >> mean;
264 
265  for (size_t r=0;r<size(cov,1);r++)
266  in >> cov.get_unsafe(r,r);
267  for (size_t r=0;r<size(cov,1);r++)
268  for (size_t c=r+1;c<size(cov,2);c++)
269  {
270  double x;
271  in >> x;
272  cov.get_unsafe(r,c) = cov.get_unsafe(c,r) = x;
273  }
274  } break;
275  default:
277 
278  };
279 }
280 
282 {
283  if (this == &o) return; // It may be used sometimes
284 
285  // Convert to gaussian pdf:
287 }
288 
290 {
291  // Convert to gaussian pdf:
292  CMatrixDouble33 C;
293  CPose2D p;
294  o.getCovarianceAndMean(C,p);
295  mean = CPose3D(p);
296 
297  cov.zeros();
298  cov.get_unsafe(0,0)= C.get_unsafe(0,0);
299  cov.get_unsafe(1,1)= C.get_unsafe(1,1);
300  cov.get_unsafe(3,3)= C.get_unsafe(2,2);
301 
302  cov.get_unsafe(0,1)=
303  cov.get_unsafe(1,0)= C.get_unsafe(0,1);
304 
305  cov.get_unsafe(0,3)=
306  cov.get_unsafe(3,0)= C.get_unsafe(0,2);
307 
308  cov.get_unsafe(1,3)=
309  cov.get_unsafe(3,1)= C.get_unsafe(1,2);
310 }
311 
312 
313 /*---------------------------------------------------------------
314 
315  ---------------------------------------------------------------*/
316 void CPose3DPDFGaussian::saveToTextFile(const string &file) const
317 {
318  FILE *f=os::fopen(file.c_str(),"wt");
319  if (!f) return;
320 
321  os::fprintf(f,"%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(), mean.pitch(), mean.roll() );
322 
323  for (unsigned int i=0;i<6;i++)
324  os::fprintf(f,"%e %e %e %e %e %e\n", cov(i,0),cov(i,1),cov(i,2),cov(i,3),cov(i,4),cov(i,5));
325 
326  os::fclose(f);
327 }
328 
329 /*---------------------------------------------------------------
330  changeCoordinatesReference
331  ---------------------------------------------------------------*/
333 {
334  MRPT_START
335  // this = p (+) this
336 
337  // COV:
338  const CMatrixDouble66 OLD_COV = this->cov;
340 
342  newReferenceBase, // x
343  this->mean, // u
344  df_dx,
345  df_du );
346 
347  // this->cov = H1*this->cov*~H1 + H2* 0 *~H2;
348  df_du.multiply_HCHt( OLD_COV, cov );
349 
350  // MEAN:
351  this->mean.composeFrom(newReferenceBase, this->mean);
352 
353  MRPT_END
354 }
355 
356 /*---------------------------------------------------------------
357  drawSingleSample
358  ---------------------------------------------------------------*/
360 {
361  MRPT_START
362 
365 
366  outPart.setFromValues(
367  mean.x() + v[0],
368  mean.y() + v[1],
369  mean.z() + v[2],
370  mean.yaw() + v[3],
371  mean.pitch() + v[4],
372  mean.roll() + v[5] );
373 
375  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"); \
376  );
377 }
378 
379 /*---------------------------------------------------------------
380  drawManySamples
381  ---------------------------------------------------------------*/
383  size_t N,
384  vector<CVectorDouble> &outSamples ) const
385 {
386  MRPT_START
387 
389 
390  for (vector<CVectorDouble>::iterator it=outSamples.begin();it!=outSamples.end();++it)
391  {
392  (*it)[0] += mean.x();
393  (*it)[1] += mean.y();
394  (*it)[2] += mean.z();
395  (*it)[3] = math::wrapToPi( (*it)[3] + mean.yaw() );
396  (*it)[4] = math::wrapToPi( (*it)[4] + mean.pitch() );
397  (*it)[5] = math::wrapToPi( (*it)[5] + mean.roll() );
398  }
399 
400  MRPT_END
401 }
402 
403 
404 /*---------------------------------------------------------------
405  bayesianFusion
406  ---------------------------------------------------------------*/
408 {
410  MRPT_START
411 
412  THROW_EXCEPTION("TO DO!!!");
413 
414 /* ASSERT_( p1_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
415  ASSERT_( p2_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
416 
417  CPose3DPDFGaussian *p1 = (CPose3DPDFGaussian*) &p1_;
418  CPose3DPDFGaussian *p2 = (CPose3DPDFGaussian*) &p2_;
419 
420 
421  CMatrixD x1(3,1),x2(3,1),x(3,1);
422  CMatrixD C1( p1->cov );
423  CMatrixD C2( p2->cov );
424  CMatrixD C1_inv = C1.inv();
425  CMatrixD C2_inv = C2.inv();
426  CMatrixD C;
427 
428  x1(0,0) = p1->mean.x; x1(1,0) = p1->mean.y; x1(2,0) = p1->mean.phi;
429  x2(0,0) = p2->mean.x; x2(1,0) = p2->mean.y; x2(2,0) = p2->mean.phi;
430 
431  C = !(C1_inv + C2_inv);
432 
433  this->cov = C;
434  this->assureSymmetry();
435 
436  x = C * ( C1_inv*x1 + C2_inv*x2 );
437 
438  this->mean.x = x(0,0);
439  this->mean.y = x(1,0);
440  this->mean.phi = x(2,0);
441  this->mean.normalizePhi();
442 */
443  MRPT_END
444 
445 }
446 
447 /*---------------------------------------------------------------
448  inverse
449  ---------------------------------------------------------------*/
451 {
453  CPose3DPDFGaussian &out = static_cast<CPose3DPDFGaussian&>(o);
454 
455  // This is like: b=(0,0,0)
456  // OUT = b - THIS
457  CPose3DPDFGaussian p_zero( CPose3D(0,0,0, 0,0,0), CMatrixDouble66() ); // COV=All zeros
458 
459  out = p_zero - *this;
460 }
461 
462 
463 /*---------------------------------------------------------------
464  +=
465  ---------------------------------------------------------------*/
467 {
468  // COV:
469  const CMatrixDouble66 OLD_COV = this->cov;
471 
473  this->mean, // x
474  Ap, // u
475  df_dx,
476  df_du );
477 
478  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
479  df_dx.multiply_HCHt( OLD_COV, cov );
480  // df_du: Nothing to do, since COV(Ap) = zeros
481 
482  // MEAN:
483  this->mean.composeFrom(this->mean, Ap);
484 }
485 
486 /*---------------------------------------------------------------
487  +=
488  ---------------------------------------------------------------*/
490 {
491  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
492  // Make a way around them and consider instead this path:
493  //
494  // X(6D) U(6D)
495  // | |
496  // v v
497  // X(7D) U(7D)
498  // | |
499  // +--- (+) ---+
500  // |
501  // v
502  // RES(7D)
503  // |
504  // v
505  // RES(6D)
506  //
507  CPose3DQuatPDFGaussian X7(*this);
508  const CPose3DQuatPDFGaussian U7(Ap);
509 
510  X7+=U7;
511 
512  this->copyFrom(X7);
513 }
514 
515 /*---------------------------------------------------------------
516  -=
517  ---------------------------------------------------------------*/
519 {
520  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
521  // Make a way around them and consider instead this path:
522  //
523  // X(6D) U(6D)
524  // | |
525  // v v
526  // X(7D) U(7D)
527  // | |
528  // +--- (-) ---+
529  // |
530  // v
531  // RES(7D)
532  // |
533  // v
534  // RES(6D)
535  //
536  CPose3DQuatPDFGaussian X7(*this);
537  const CPose3DQuatPDFGaussian U7(Ap);
538 
539  X7-=U7;
540 
541  this->copyFrom(X7);
542 }
543 
544 /*---------------------------------------------------------------
545  evaluatePDF
546  ---------------------------------------------------------------*/
547 double CPose3DPDFGaussian::evaluatePDF( const CPose3D &x ) const
548 {
550  THROW_EXCEPTION("TO DO!!!");
551 
552 /* CMatrixD X(6,1);
553  X(0,0) = x.x;
554  X(1,0) = x.y;
555  X(2,0) = x.z;
556 
557  CMatrixD MU(6,1);
558  MU(0,0) = mean.x;
559  MU(1,0) = mean.y;
560  MU(2,0) = mean.z;
561 
562  return math::normalPDF( X, MU, this->cov );
563 */
564 }
565 
566 /*---------------------------------------------------------------
567  evaluateNormalizedPDF
568  ---------------------------------------------------------------*/
570 {
572  THROW_EXCEPTION("TO DO!!!");
573 /* CMatrixD X(3,1);
574  X(0,0) = x.x;
575  X(1,0) = x.y;
576  X(2,0) = x.phi;
577 
578  CMatrixD MU(3,1);
579  MU(0,0) = mean.x;
580  MU(1,0) = mean.y;
581  MU(2,0) = mean.phi;
582 
583  return math::normalPDF( X, MU, this->cov ) / math::normalPDF( MU, MU, this->cov );
584 */
585 }
586 
587 /*---------------------------------------------------------------
588  assureSymmetry
589  ---------------------------------------------------------------*/
591 {
592  // Differences, when they exist, appear in the ~15'th significant
593  // digit, so... just take one of them arbitrarily!
594  for (unsigned int i=0;i<size(cov,1)-1;i++)
595  for (unsigned int j=i+1;j<size(cov,1);j++)
596  cov.get_unsafe(i,j) = cov.get_unsafe(j,i);
597 }
598 
599 /*---------------------------------------------------------------
600  mahalanobisDistanceTo
601  ---------------------------------------------------------------*/
603 {
604  MRPT_START
605 
606  CMatrixDouble66 COV_ = cov + theOther.cov;
608 
609  for (int i=0;i<6;i++)
610  {
611  if (COV_.get_unsafe(i,i)==0)
612  {
613  if (MU.get_unsafe(i,0)!=0)
614  return std::numeric_limits<double>::infinity();
615  else COV_.get_unsafe(i,i) = 1; // Any arbitrary value since MU(i)=0, and this value doesn't affect the result.
616  }
617  }
618 
619  return std::sqrt( MU.multiply_HtCH_scalar(COV_.inv()) );
620 
621  MRPT_END
622 }
623 
624 /*---------------------------------------------------------------
625  operator <<
626  ---------------------------------------------------------------*/
628  ostream &out,
629  const CPose3DPDFGaussian &obj )
630 {
631  out << "Mean: " << obj.mean << "\n";
632  out << "Covariance:\n" << obj.cov << "\n";
633 
634  return out;
635 }
636 
637 /*---------------------------------------------------------------
638  getCovSubmatrix2D
639  ---------------------------------------------------------------*/
641 {
642  out_cov.setSize(3,3);
643 
644  for (int i=0;i<3;i++)
645  {
646  int a = i==2 ? 3:i;
647  for (int j=i;j<3;j++)
648  {
649  int b = j==2 ? 3:j;
650  double f = cov.get_unsafe(a,b);
651  out_cov.set_unsafe(i,j, f);
652  out_cov.set_unsafe(j,i, f);
653  }
654  }
655 }
656 
658 {
659  return p1.mean==p2.mean && p1.cov==p2.cov;
660 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
A namespace of pseudo-random numbers genrators of diferent distributions.
const GLdouble * v
Definition: glew.h:1296
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3D mean
The mean value.
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:220
#define MRPT_END_WITH_CLEAN_UP(stuff)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
void copyFrom(const CPose3DPDF &o) MRPT_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 BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_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.
const GLfloat * c
Definition: glew.h:10088
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:133
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
Scalar * iterator
Definition: eigen_plugins.h:23
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
STL namespace.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=NULL)
Generate a given number of multidimensional random samples according to a given covariance matrix...
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
GLuint in
Definition: glew.h:7146
void getCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
BASE_IMPEXP bool USE_SUT_QUAT2EULER_CONVERSION
If set to true (false), a Scaled Unscented Transform is used instead of a linear approximation with J...
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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:595
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:135
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:317
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
CPose3DPDFGaussian()
Default constructor.
GLhandleARB obj
Definition: glew.h:3276
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:72
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=NULL, 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...
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:254
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=NULL, 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:328
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
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).
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
Definition: jacobians.h:119
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...
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void drawSingleSample(CPose3D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=NULL)
Generate multidimensional random samples according to a given covariance matrix.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:50
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:56
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in ...
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:106
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018