Main MRPT website > C++ reference for MRPT 1.5.6
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-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 
15 #include <mrpt/poses/CPose3DPDF.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/math/CMatrix.h>
19 #include <mrpt/math/wrap2pi.h>
20 #include <mrpt/system/os.h>
21 #include <mrpt/utils/CStream.h>
22 
23 #include <mrpt/random.h>
24 
25 using namespace mrpt;
26 using namespace mrpt::utils;
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 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41 {
42 
43 }
44 
45 /*---------------------------------------------------------------
46  Constructor
47  ---------------------------------------------------------------*/
48 CPosePDFGaussian::CPosePDFGaussian(
49  const CPose2D &init_Mean,
50  const CMatrixDouble33 &init_Cov ) : mean(init_Mean), cov(init_Cov)
51 {
52 
53 }
54 
55 /*---------------------------------------------------------------
56  Constructor
57  ---------------------------------------------------------------*/
58 CPosePDFGaussian::CPosePDFGaussian(const CPose2D &init_Mean ) : mean(init_Mean), cov()
59 {
60  cov.zeros();
61 }
62 
63 /*---------------------------------------------------------------
64  writeToStream
65  ---------------------------------------------------------------*/
67 {
68  if (version)
69  *version = 2;
70  else
71  {
72  out << mean;
73  out << cov(0,0) << cov(1,1) << cov(2,2);
74  out << cov(0,1) << cov(0,2) << cov(1,2);
75  }
76 }
77 
78 /*---------------------------------------------------------------
79  readFromStream
80  ---------------------------------------------------------------*/
82 {
83  switch(version)
84  {
85  case 2:
86  {
87  double x;
88  in >> mean;
89 
90  in >> x; cov(0,0) = x;
91  in >> x; cov(1,1) = x;
92  in >> x; cov(2,2) = x;
93 
94  in >> x; cov(1,0) = x; cov(0,1) = x;
95  in >> x; cov(2,0) = x; cov(0,2) = x;
96  in >> x; cov(1,2) = x; cov(2,1) = x;
97  } break;
98  case 1:
99  {
100  float x;
101  in >> mean;
102 
103  in >> x; cov(0,0) = x;
104  in >> x; cov(1,1) = x;
105  in >> x; cov(2,2) = x;
106 
107  in >> x; cov(1,0) = x; cov(0,1) = x;
108  in >> x; cov(2,0) = x; cov(0,2) = x;
109  in >> x; cov(1,2) = x; cov(2,1) = x;
110  } break;
111  case 0:
112  {
113  CMatrix auxCov;
114  in >> mean >> auxCov;
115  cov = auxCov.cast<double>();
116  } break;
117  default:
119 
120  };
121 }
122 
123 
124 
125 /*---------------------------------------------------------------
126  copyFrom
127  ---------------------------------------------------------------*/
129 {
130  if (this == &o) return; // It may be used sometimes
131 
132  // Convert to gaussian pdf:
133  o.getMean(mean);
134  o.getCovariance(cov);
135 }
136 
137 /*---------------------------------------------------------------
138  copyFrom 3D
139  ---------------------------------------------------------------*/
141 {
142  // Convert to gaussian pdf:
143  mean = CPose2D(o.getMeanVal());
144  CMatrixDouble66 C;
145  o.getCovariance(C);
146 
147  // Clip to 3x3:
148  C(2,0)=C(0,2) = C(0,3);
149  C(2,1)=C(1,2) = C(1,3);
150  C(2,2)= C(3,3);
151  cov = C.block(0,0,3,3);
152 }
153 
154 
155 /*---------------------------------------------------------------
156 
157  ---------------------------------------------------------------*/
159 {
160  FILE *f=os::fopen(file.c_str(),"wt");
161  if (!f) return;
162 
163  os::fprintf(f,"%f %f %f\n", mean.x(), mean.y(), mean.phi() );
164 
165  os::fprintf(f,"%f %f %f\n", cov(0,0),cov(0,1),cov(0,2) );
166  os::fprintf(f,"%f %f %f\n", cov(1,0),cov(1,1),cov(1,2) );
167  os::fprintf(f,"%f %f %f\n", cov(2,0),cov(2,1),cov(2,2) );
168 
169 
170  os::fclose(f);
171 }
172 
173 /*---------------------------------------------------------------
174  changeCoordinatesReference
175  ---------------------------------------------------------------*/
177 {
178  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
179 
180  // The mean:
181  mean.composeFrom(newReferenceBase, mean);
182 
183  // The covariance:
184  rotateCov( newReferenceBase.phi() );
185 }
186 
187 /*---------------------------------------------------------------
188  changeCoordinatesReference
189  ---------------------------------------------------------------*/
191 {
192  // The mean:
193  mean.composeFrom(newReferenceBase, mean);
194  // The covariance:
195  rotateCov( newReferenceBase.phi() );
196 }
197 
198 /*---------------------------------------------------------------
199  changeCoordinatesReference
200  ---------------------------------------------------------------*/
201 void CPosePDFGaussian::rotateCov(const double ang)
202 {
203  const double ccos = cos(ang);
204  const double ssin = sin(ang);
205 
206  MRPT_ALIGN16 const double rot_vals[] = {
207  ccos, -ssin, 0.,
208  ssin, ccos, 0.,
209  0. , 0., 1. };
210 
211  const CMatrixFixedNumeric<double,3,3> rot(rot_vals);
212  cov = (rot * cov * rot.adjoint()).eval();
213 }
214 
215 /*---------------------------------------------------------------
216  drawSingleSample
217  ---------------------------------------------------------------*/
219 {
220  MRPT_START
221 
224 
225  outPart.x( mean.x() + v[0] );
226  outPart.y( mean.y() + v[1] );
227  outPart.phi( mean.phi() + v[2] );
228 
229  // Range -pi,pi
230  outPart.normalizePhi();
231 
233  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"); \
234  );
235 }
236 
237 /*---------------------------------------------------------------
238  drawManySamples
239  ---------------------------------------------------------------*/
241  size_t N,
242  std::vector<CVectorDouble> &outSamples ) const
243 {
244  MRPT_START
245 
246  std::vector<CVectorDouble> rndSamples;
247 
249  outSamples.resize( N );
250  for (unsigned int i=0;i<N;i++)
251  {
252  outSamples[i].resize(3);
253  outSamples[i][0] = mean.x() + rndSamples[i][0] ;
254  outSamples[i][1] = mean.y() + rndSamples[i][1] ;
255  outSamples[i][2] = mean.phi() + rndSamples[i][2] ;
256 
257  wrapToPiInPlace( outSamples[i][2] );
258  }
259 
260  MRPT_END
261 }
262 
263 
264 /*---------------------------------------------------------------
265  bayesianFusion
266  ---------------------------------------------------------------*/
267 void CPosePDFGaussian::bayesianFusion(const CPosePDF &p1_,const CPosePDF &p2_,const double& minMahalanobisDistToDrop )
268 {
269  MRPT_START
270 
271  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
272 
275 
276  const CPosePDFGaussian *p1 = static_cast<const CPosePDFGaussian*>( &p1_ );
277  const CPosePDFGaussian *p2 = static_cast<const CPosePDFGaussian*>( &p2_ );
278 
279 
280  CMatrixDouble33 C1 = p1->cov;
281  CMatrixDouble33 C2 = p2->cov;
282 
283  CMatrixDouble33 C1_inv;
284  C1.inv(C1_inv);
285 
286  CMatrixDouble33 C2_inv;
287  C2.inv(C2_inv);
288 
289  CMatrixDouble31 x1 = CMatrixDouble31(p1->mean);
291 
292 
293  CMatrixDouble33 auxC = C1_inv + C2_inv;
294  auxC.inv(this->cov);
295  this->assureSymmetry();
296 
297  CMatrixDouble31 x = this->cov * ( C1_inv*x1 + C2_inv*x2 );
298 
299  this->mean.x( x(0,0) );
300  this->mean.y( x(1,0) );
301  this->mean.phi( x(2,0) );
302  this->mean.normalizePhi();
303 
304  MRPT_END
305 
306 }
307 
308 /*---------------------------------------------------------------
309  inverse
310  ---------------------------------------------------------------*/
312 {
314  CPosePDFGaussian *out = static_cast<CPosePDFGaussian*>( &o );
315 
316  // The mean:
317  out->mean = CPose2D(0,0,0) - mean;
318 
319  // The covariance:
320  const double ccos = ::cos(mean.phi());
321  const double ssin = ::sin(mean.phi());
322 
323  // jacobian:
324  MRPT_ALIGN16 const double H_values[] = {
325  -ccos, -ssin, mean.x()*ssin-mean.y()*ccos,
326  ssin, -ccos, mean.x()*ccos+mean.y()*ssin,
327  0 , 0, -1
328  };
329  const CMatrixFixedNumeric<double,3,3> H(H_values);
330 
331  out->cov.noalias() = (H * cov * H.adjoint()).eval(); // o.cov = H * cov * Ht
332 }
333 
334 
335 /*---------------------------------------------------------------
336  +=
337  ---------------------------------------------------------------*/
339 {
340  mean = mean + Ap;
341  rotateCov( Ap.phi() );
342 }
343 
344 /*---------------------------------------------------------------
345  evaluatePDF
346  ---------------------------------------------------------------*/
348 {
351 
352  return math::normalPDF( X, MU, this->cov );
353 }
354 
355 /*---------------------------------------------------------------
356  evaluateNormalizedPDF
357  ---------------------------------------------------------------*/
359 {
362 
363  return math::normalPDF( X, MU, this->cov ) / math::normalPDF( MU, MU, this->cov );
364 }
365 
366 /*---------------------------------------------------------------
367  assureSymmetry
368  ---------------------------------------------------------------*/
370 {
371  // Differences, when they exist, appear in the ~15'th significant
372  // digit, so... just take one of them arbitrarily!
373  cov(0,1) = cov(1,0);
374  cov(0,2) = cov(2,0);
375  cov(1,2) = cov(2,1);
376 }
377 
378 /*---------------------------------------------------------------
379  mahalanobisDistanceTo
380  ---------------------------------------------------------------*/
382 {
383  MRPT_START
384 
386  MU-=CArrayDouble<3>(theOther.mean);
387 
388  wrapToPiInPlace(MU[2]);
389 
390  if (MU[0]==0 && MU[1]==0 && MU[2]==0)
391  return 0; // This is the ONLY case where we know the result, whatever COVs are.
392 
393  CMatrixDouble33 COV_=cov;
394  COV_+=theOther.cov;
395 
397  COV_.inv_fast(COV_inv);
398 
399  // (~MU) * (!COV_) * MU
400  return std::sqrt( mrpt::math::multiply_HCHt_scalar(MU,COV_inv) );
401 
402  MRPT_END
403 }
404 
405 /*---------------------------------------------------------------
406  operator <<
407  ---------------------------------------------------------------*/
409  std::ostream &out,
410  const CPosePDFGaussian &obj )
411 {
412  out << "Mean: " << obj.mean << "\n";
413  out << "Covariance:\n" << obj.cov << "\n";
414 
415  return out;
416 }
417 
418 /*---------------------------------------------------------------
419  operator +
420  ---------------------------------------------------------------*/
422 {
425  return ret;
426 }
427 
428 /*---------------------------------------------------------------
429  assureMinCovariance
430  ---------------------------------------------------------------*/
431 void CPosePDFGaussian::assureMinCovariance( const double& minStdXY, const double&minStdPhi )
432 {
433  cov(0,0) = max( cov(0,0), square(minStdXY) );
434  cov(1,1) = max( cov(1,1), square(minStdXY) );
435  cov(2,2) = max( cov(2,2), square(minStdPhi) );
436 }
437 
438 /*---------------------------------------------------------------
439  inverseComposition
440  Set 'this' = 'x' - 'ref', computing the mean using the "-"
441  operator and the covariances through the corresponding Jacobians.
442  ---------------------------------------------------------------*/
444  const CPosePDFGaussian &xv,
445  const CPosePDFGaussian &xi )
446 {
447  // COV:
448  double cpi = cos(xi.mean.phi());
449  double spi = sin(xi.mean.phi());
450 
451  // jacob: dh_xv
452  CMatrixDouble33 dh_xv;
453 
454  dh_xv(0,0) = cpi; dh_xv(0,1) = spi;
455  dh_xv(1,0) = -spi; dh_xv(1,1) = cpi;
456  dh_xv(2,2) = 1;
457 
458  // jacob: dh_xi
459  CMatrixDouble33 dh_xi;
460 
461  double xv_xi = xv.mean.x() - xi.mean.x();
462  double yv_yi = xv.mean.y() - xi.mean.y();
463 
464  dh_xi(0,0) = -cpi; dh_xi(0,1) = -spi; dh_xi(0,2) = -xv_xi*spi+yv_yi*cpi;
465  dh_xi(1,0) = spi; dh_xi(1,1) = -cpi; dh_xi(1,2) = -xv_xi*cpi-yv_yi*spi;
466  dh_xi(2,2) = -1;
467 
468  // Build the cov:
469  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T
470  dh_xv.multiply_HCHt( xv.cov, this->cov );
471  dh_xi.multiply_HCHt( xi.cov, this->cov, true ); // Accum. result
472 
473  // Mean:
474  mean = xv.mean - xi.mean;
475 }
476 
477 
478 /*---------------------------------------------------------------
479  inverseComposition
480  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
481  the "-" operator and the covariances through the corresponding
482  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
483  ---------------------------------------------------------------*/
485  const CPosePDFGaussian &x1,
486  const CPosePDFGaussian &x0,
487  const CMatrixDouble33 &COV_01 )
488 {
489  double cp0 = cos(x0.mean.phi());
490  double sp0 = sin(x0.mean.phi());
491 
492  // jacob: dh_x1
493  CMatrixDouble33 dh_x1;
494 
495  dh_x1(0,0) = cp0; dh_x1(0,1) = sp0;
496  dh_x1(1,0) = -sp0; dh_x1(1,1) = cp0;
497  dh_x1(2,2) = 1;
498 
499  // jacob: dh_x0
500  CMatrixDouble33 dh_x0;
501 
502  double xv_xi = x1.mean.x() - x0.mean.x();
503  double yv_yi = x1.mean.y() - x0.mean.y();
504 
505  dh_x0(0,0) = -cp0; dh_x0(0,1) = -sp0; dh_x0(0,2) = -xv_xi*sp0+yv_yi*cp0;
506  dh_x0(1,0) = sp0; dh_x0(1,1) = -cp0; dh_x0(1,2) = -xv_xi*cp0-yv_yi*sp0;
507  dh_x0(2,2) = -1;
508 
509  // Build the cov:
510  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T + A + At
511  // being A = dh_dx0 * COV01 * dh_dx1^t
512  dh_x0.multiply_HCHt( x0.cov, this->cov );
513  dh_x1.multiply_HCHt( x1.cov, this->cov, true ); // Accum. result
514 
515  CMatrixDouble33 M;
516  M.multiply_ABCt( dh_x0, COV_01, dh_x1 );
517 
518  this->cov.add_AAt(M);
519 
520  //mean
521  mean = x1.mean - x0.mean;
522 }
523 
524 /*---------------------------------------------------------------
525  +=
526  ---------------------------------------------------------------*/
528 {
529  // COV:
530  const CMatrixDouble33 OLD_COV = this->cov;
532 
534  this->mean, // x
535  Ap.mean, // u
536  df_dx,
537  df_du );
538 
539  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
540  df_dx.multiply_HCHt( OLD_COV, cov );
541  df_du.multiply_HCHt( Ap.cov, cov, true); // Accumulate result
542 
543  // MEAN:
544  this->mean = this->mean + Ap.mean;
545 }
546 
547 /** Returns the PDF of the 2D point \f$ g = q \oplus l\f$ with "q"=this pose and "l" a point without uncertainty */
549 {
550  // Mean:
551  double gx,gy;
552  mean.composePoint(l.x,l.y, gx,gy);
553  g.mean.x(gx);
554  g.mean.y(gy);
555 
556  // COV:
559  this->mean, // x
560  this->mean, // u
561  df_dx,
562  df_du,
563  true, // Eval df_dx
564  false // Eval df_du (not needed)
565  );
566 
567  const CMatrixFixedNumeric<double,2,3> dp_dx = df_dx.block<2,3>(0,0);
568  g.cov = dp_dx * this->cov * dp_dx.transpose();
569 }
570 
571 
573 {
574  return p1.mean==p2.mean && p1.cov==p2.cov;
575 }
576 
577 
579 {
581  res+=b;
582  return res;
583 }
584 
586 {
588  res.inverseComposition(a,b);
589  return res;
590 }
A namespace of pseudo-random numbers genrators of diferent distributions.
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
double y
X,Y coordinates.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:53
#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
A gaussian distribution for 2D points.
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.
mrpt::math::TPoint2D BASE_IMPEXP 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:359
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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
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:62
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
STL namespace.
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.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
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:178
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
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...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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.
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
CPosePDFGaussian()
Default constructor.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CPose2D BASE_IMPEXP 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:307
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
GLubyte g
Definition: glext.h:5575
void inverse(CPosePDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
GLubyte GLubyte b
Definition: glext.h:5575
int version
Definition: mrpt_jpeglib.h:898
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLsizei const GLchar ** string
Definition: glext.h:3919
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:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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 1x3 vectors, where each row contains a (x,y,phi) datum.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
#define MRPT_START
const GLdouble * v
Definition: glext.h:3603
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...
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:35
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:122
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
GLuint res
Definition: glext.h:6298
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.
GLenum GLint x
Definition: glext.h:3516
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:30
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
Lightweight 2D point.
double BASE_IMPEXP normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:908
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
#define MRPT_ALIGN16
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:287
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019