Main MRPT website > C++ reference for MRPT 1.5.6
CPosePDFGaussianInf.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>
17 #include <mrpt/poses/CPose3D.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  Constructor
46  ---------------------------------------------------------------*/
47 CPosePDFGaussianInf::CPosePDFGaussianInf(
48  const CPose2D &init_Mean,
49  const CMatrixDouble33 &init_CovInv ) : mean(init_Mean), cov_inv(init_CovInv)
50 {
51 }
52 
53 /*---------------------------------------------------------------
54  Constructor
55  ---------------------------------------------------------------*/
56 CPosePDFGaussianInf::CPosePDFGaussianInf(const CPose2D &init_Mean ) : mean(init_Mean), cov_inv()
57 {
58 }
59 
60 /*---------------------------------------------------------------
61  writeToStream
62  ---------------------------------------------------------------*/
64 {
65  if (version)
66  *version = 0;
67  else
68  {
69  out << mean.x() << mean.y() << mean.phi();
70  out << cov_inv(0,0) << cov_inv(1,1) << cov_inv(2,2);
71  out << cov_inv(0,1) << cov_inv(0,2) << cov_inv(1,2);
72  }
73 }
74 
75 /*---------------------------------------------------------------
76  readFromStream
77  ---------------------------------------------------------------*/
79 {
80  switch(version)
81  {
82  case 0:
83  {
84  TPose2D p;
85  in >> p.x >> p.y >> p.phi;
86  mean = CPose2D(p);
87 
88  in >> cov_inv(0,0) >> cov_inv(1,1) >> cov_inv(2,2);
89  in >> cov_inv(0,1) >> cov_inv(0,2) >> cov_inv(1,2);
90  } break;
91  default:
93  };
94 }
95 
96 
97 /*---------------------------------------------------------------
98  copyFrom
99  ---------------------------------------------------------------*/
101 {
102  if (this == &o) return; // It may be used sometimes
103 
104  if (IS_CLASS(&o, CPosePDFGaussianInf))
105  { // It's my same class:
106  const CPosePDFGaussianInf *ptr = static_cast<const CPosePDFGaussianInf*>(&o);
107  mean = ptr->mean;
108  cov_inv = ptr->cov_inv;
109  }
110  else
111  { // Convert to gaussian pdf:
112  o.getMean(mean);
113 
115  o.getCovariance(o_cov);
116  o_cov.inv_fast(this->cov_inv);
117  }
118 }
119 
120 /*---------------------------------------------------------------
121  copyFrom 3D
122  ---------------------------------------------------------------*/
124 {
125  // Convert to gaussian pdf:
126  mean = CPose2D(o.getMeanVal());
127 
129  { // Cov is already in information form:
130  const CPose3DPDFGaussianInf *ptr = static_cast<const CPose3DPDFGaussianInf*>(&o);
131  cov_inv(0,0)=ptr->cov_inv(0,0);
132  cov_inv(1,1)=ptr->cov_inv(1,1);
133  cov_inv(2,2)=ptr->cov_inv(3,3);
134  cov_inv(0,1)=cov_inv(1,0)=ptr->cov_inv(0,1);
135  cov_inv(0,2)=cov_inv(2,0)=ptr->cov_inv(0,3);
136  cov_inv(1,2)=cov_inv(2,1)=ptr->cov_inv(1,3);
137  }
138  else
139  {
141  o.getCovariance(C);
142 
143  // Clip to 3x3:
145  o_cov(0,0)=C(0,0);
146  o_cov(1,1)=C(1,1);
147  o_cov(2,2)=C(3,3);
148  o_cov(0,1)=o_cov(1,0)=C(0,1);
149  o_cov(0,2)=o_cov(2,0)=C(0,3);
150  o_cov(1,2)=o_cov(2,1)=C(1,3);
151 
152  o_cov.inv_fast(this->cov_inv);
153  }
154 }
155 
156 
157 /*---------------------------------------------------------------
158 
159  ---------------------------------------------------------------*/
161 {
162  FILE *f=os::fopen(file.c_str(),"wt");
163  if (!f) return;
164 
165  os::fprintf(f,"%f %f %f\n", mean.x(), mean.y(), mean.phi() );
166 
167  for (unsigned int i=0;i<3;i++)
168  os::fprintf(f,"%f %f %f\n", cov_inv(i,0),cov_inv(i,1),cov_inv(i,2) );
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 /*---------------------------------------------------------------
200  changeCoordinatesReference
201  ---------------------------------------------------------------*/
202 void CPosePDFGaussianInf::rotateCov(const double ang)
203 {
204  const double ccos = cos(ang);
205  const double ssin = sin(ang);
206 
207  MRPT_ALIGN16 const double rot_vals[] = {
208  ccos, -ssin, 0.,
209  ssin, ccos, 0.,
210  0. , 0., 1. };
211 
212  const CMatrixFixedNumeric<double,3,3> rot(rot_vals);
213 
214  // NEW_COV = H C H^T
215  // NEW_COV^(-1) = (H C H^T)^(-1) = (H^T)^(-1) C^(-1) H^(-1)
216  // rot: Inverse of a rotation matrix is its trasposed.
217  // But we need H^t^-1 -> H !! so rot stays unchanged:
218  cov_inv = (rot * cov_inv * rot.adjoint()).eval();
219 }
220 
221 /*---------------------------------------------------------------
222  drawSingleSample
223  ---------------------------------------------------------------*/
225 {
226  MRPT_START
227 
229  this->cov_inv.inv(cov);
230 
233 
234  outPart.x( mean.x() + v[0] );
235  outPart.y( mean.y() + v[1] );
236  outPart.phi( mean.phi() + v[2] );
237 
238  // Range -pi,pi
239  outPart.normalizePhi();
240 
242  cov_inv.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"); \
243  );
244 }
245 
246 /*---------------------------------------------------------------
247  drawManySamples
248  ---------------------------------------------------------------*/
250  size_t N,
251  std::vector<CVectorDouble> &outSamples ) const
252 {
253  MRPT_START
254 
256  this->cov_inv.inv(cov);
257 
258  std::vector<CVectorDouble> rndSamples;
259 
261  outSamples.resize( N );
262  for (unsigned int i=0;i<N;i++)
263  {
264  outSamples[i].resize(3);
265  outSamples[i][0] = mean.x() + rndSamples[i][0] ;
266  outSamples[i][1] = mean.y() + rndSamples[i][1] ;
267  outSamples[i][2] = mean.phi() + rndSamples[i][2] ;
268 
269  wrapToPiInPlace( outSamples[i][2] );
270  }
271 
272  MRPT_END
273 }
274 
275 
276 /*---------------------------------------------------------------
277  bayesianFusion
278  ---------------------------------------------------------------*/
279 void CPosePDFGaussianInf::bayesianFusion(const CPosePDF &p1_,const CPosePDF &p2_,const double& minMahalanobisDistToDrop )
280 {
281  MRPT_START
282 
283  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
284 
287 
288  const CPosePDFGaussianInf *p1 = static_cast<const CPosePDFGaussianInf*>( &p1_ );
289  const CPosePDFGaussianInf *p2 = static_cast<const CPosePDFGaussianInf*>( &p2_ );
290 
291  const CMatrixDouble33& C1_inv = p1->cov_inv;
292  const CMatrixDouble33& C2_inv = p2->cov_inv;
293 
294  CMatrixDouble31 x1 = CMatrixDouble31(p1->mean);
296 
297  this->cov_inv = C1_inv + C2_inv;
298 
300  this->cov_inv.inv(cov);
301 
302  CMatrixDouble31 x = cov * ( C1_inv*x1 + C2_inv*x2 );
303 
304  this->mean.x( x(0,0) );
305  this->mean.y( x(1,0) );
306  this->mean.phi( x(2,0) );
307  this->mean.normalizePhi();
308 
309  MRPT_END
310 
311 }
312 
313 /*---------------------------------------------------------------
314  inverse
315  ---------------------------------------------------------------*/
317 {
319  CPosePDFGaussianInf *out = static_cast<CPosePDFGaussianInf*>( &o );
320 
321  // The mean:
322  out->mean = CPose2D(0,0,0) - mean;
323 
324  // The covariance:
325  const double ccos = ::cos(mean.phi());
326  const double ssin = ::sin(mean.phi());
327 
328  // jacobian:
329  MRPT_ALIGN16 const double H_values[] = {
330  -ccos, -ssin, mean.x()*ssin-mean.y()*ccos,
331  ssin, -ccos, mean.x()*ccos+mean.y()*ssin,
332  0 , 0, -1
333  };
334  const CMatrixFixedNumeric<double,3,3> H(H_values);
335 
336  out->cov_inv.noalias() = (H * cov_inv * H.adjoint()).eval(); // o.cov = H * cov * Ht. It's the same with inverse covariances.
337 }
338 
339 
340 /*---------------------------------------------------------------
341  +=
342  ---------------------------------------------------------------*/
344 {
345  mean = mean + Ap;
346  rotateCov( Ap.phi() );
347 }
348 
349 /*---------------------------------------------------------------
350  evaluatePDF
351  ---------------------------------------------------------------*/
353 {
356 
357  return math::normalPDF( X, MU, cov_inv.inverse() );
358 }
359 
360 /*---------------------------------------------------------------
361  evaluateNormalizedPDF
362  ---------------------------------------------------------------*/
364 {
367 
369  this->cov_inv.inv(cov);
370 
371  return math::normalPDF( X, MU, cov ) / math::normalPDF( MU, MU, cov );
372 }
373 
374 /*---------------------------------------------------------------
375  assureSymmetry
376  ---------------------------------------------------------------*/
378 {
379  // Differences, when they exist, appear in the ~15'th significant
380  // digit, so... just take one of them arbitrarily!
381  cov_inv(0,1) = cov_inv(1,0);
382  cov_inv(0,2) = cov_inv(2,0);
383  cov_inv(1,2) = cov_inv(2,1);
384 }
385 
386 /*---------------------------------------------------------------
387  mahalanobisDistanceTo
388  ---------------------------------------------------------------*/
390 {
391  MRPT_START
392 
394  MU-=CArrayDouble<3>(theOther.mean);
395 
396  wrapToPiInPlace(MU[2]);
397 
398  if (MU[0]==0 && MU[1]==0 && MU[2]==0)
399  return 0; // This is the ONLY case where we know the result, whatever COVs are.
400 
402  this->cov_inv.inv(COV_);
403  theOther.cov_inv.inv(cov2);
404 
405  COV_+=cov2; // COV_ = cov1+cov2
406 
408  COV_.inv_fast(COV_inv);
409 
410  // (~MU) * (!COV_) * MU
411  return std::sqrt( mrpt::math::multiply_HCHt_scalar(MU,COV_inv) );
412 
413  MRPT_END
414 }
415 
416 /*---------------------------------------------------------------
417  operator <<
418  ---------------------------------------------------------------*/
420  std::ostream &out,
421  const CPosePDFGaussianInf &obj )
422 {
423  out << "Mean: " << obj.mean << "\n";
424  out << "Inverse cov:\n" << obj.cov_inv << "\n";
425 
426  return out;
427 }
428 
429 /*---------------------------------------------------------------
430  operator +
431  ---------------------------------------------------------------*/
433 {
436  return ret;
437 }
438 
439 /*---------------------------------------------------------------
440  inverseComposition
441  Set 'this' = 'x' - 'ref', computing the mean using the "-"
442  operator and the covariances through the corresponding Jacobians.
443  ---------------------------------------------------------------*/
445  const CPosePDFGaussianInf &xv,
446  const CPosePDFGaussianInf &xi )
447 {
448  // Use implementation in CPosePDFGaussian:
450  xv.cov_inv.inv(xv_cov);
451  xi.cov_inv.inv(xi_cov);
452 
453  const CPosePDFGaussian xv_(xv.mean,xv_cov);
454  const CPosePDFGaussian xi_(xi.mean,xi_cov);
455 
456  CPosePDFGaussian RET;
457  RET.inverseComposition(xv_,xi_);
458 
459  // Copy result to "this":
460  this->mean = RET.mean;
461  RET.cov.inv(this->cov_inv);
462 }
463 
464 /*---------------------------------------------------------------
465  inverseComposition
466  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
467  the "-" operator and the covariances through the corresponding
468  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
469  ---------------------------------------------------------------*/
471  const CPosePDFGaussianInf &x1,
472  const CPosePDFGaussianInf &x0,
473  const CMatrixDouble33 &COV_01 )
474 {
475  // Use implementation in CPosePDFGaussian:
477  x1.cov_inv.inv(x1_cov);
478  x0.cov_inv.inv(x0_cov);
479 
480  const CPosePDFGaussian x1_(x1.mean,x1_cov);
481  const CPosePDFGaussian x0_(x0.mean,x0_cov);
482 
483  CPosePDFGaussian RET;
484  RET.inverseComposition(x1_,x0_,COV_01);
485 
486  // Copy result to "this":
487  this->mean = RET.mean;
488  RET.cov.inv(this->cov_inv);
489 }
490 
491 
492 /*---------------------------------------------------------------
493  +=
494  ---------------------------------------------------------------*/
496 {
497  // COV:
499  this->cov_inv.inv(OLD_COV);
500 
501  CMatrixDouble33 df_dx, df_du;
502 
504  this->mean, // x
505  Ap.mean, // u
506  df_dx,
507  df_du );
508 
509  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
511 
513  Ap.cov_inv.inv(Ap_cov);
514 
515  df_dx.multiply_HCHt( OLD_COV, cov );
516  df_du.multiply_HCHt( Ap_cov, cov, true); // Accumulate result
517 
518  cov.inv_fast(this->cov_inv);
519 
520  // MEAN:
521  this->mean = this->mean + Ap.mean;
522 }
523 
525 {
526  return p1.mean==p2.mean && p1.cov_inv==p2.cov_inv;
527 }
528 
529 /** Pose compose operator: RES = A (+) B , computing both the mean and the covariance */
532  res+=b;
533  return res;
534 }
535 
536 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and the covariance */
539  res.inverseComposition(a,b);
540  return res;
541 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
const GLdouble * v
Definition: glew.h:1296
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void inverse(CPosePDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
CPose2D mean
The mean value.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:53
#define MRPT_END_WITH_CLEAN_UP(stuff)
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
#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.
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
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
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...
GLuint in
Definition: glew.h:7146
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
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.
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
GLhandleARB obj
Definition: glew.h:3276
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jaco...
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
GLuint res
Definition: glew.h:7143
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
double y
X,Y coordinates.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
CPosePDFGaussianInf()
Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!) ...
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
#define MRPT_START
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
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.
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
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:122
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 3D pose as a Gaussian des...
Lightweight 2D pose.
#define ASSERT_(f)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
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.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given 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
void inverseComposition(const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jaco...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
double phi
Orientation (rads)
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
#define MRPT_ALIGN16
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:287
double mahalanobisDistanceTo(const CPosePDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018