Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPose3DPDF.h>
16 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/system/os.h>
21 #include <mrpt/random.h>
22 
23 using namespace mrpt;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::random;
27 using namespace mrpt::system;
28 
29 using namespace std;
30 
32 
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
36 CPosePDFGaussianInf::CPosePDFGaussianInf() : mean(0, 0, 0), cov_inv() {}
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41  const CPose2D& init_Mean, const CMatrixDouble33& init_CovInv)
42  : mean(init_Mean), cov_inv(init_CovInv)
43 {
44 }
45 
46 /*---------------------------------------------------------------
47  Constructor
48  ---------------------------------------------------------------*/
50  : mean(init_Mean), cov_inv()
51 {
52 }
53 
56 {
57  out << mean.x() << mean.y() << mean.phi();
58  out << cov_inv(0, 0) << cov_inv(1, 1) << cov_inv(2, 2);
59  out << cov_inv(0, 1) << cov_inv(0, 2) << cov_inv(1, 2);
60 }
63 {
64  switch (version)
65  {
66  case 0:
67  {
68  TPose2D p;
69  in >> p.x >> p.y >> p.phi;
70  mean = CPose2D(p);
71 
72  in >> cov_inv(0, 0) >> cov_inv(1, 1) >> cov_inv(2, 2);
73  in >> cov_inv(0, 1) >> cov_inv(0, 2) >> cov_inv(1, 2);
74  }
75  break;
76  default:
78  };
79 }
80 
81 /*---------------------------------------------------------------
82  copyFrom
83  ---------------------------------------------------------------*/
85 {
86  if (this == &o) return; // It may be used sometimes
87 
89  { // It's my same class:
90  const CPosePDFGaussianInf* ptr =
91  static_cast<const CPosePDFGaussianInf*>(&o);
92  mean = ptr->mean;
93  cov_inv = ptr->cov_inv;
94  }
95  else
96  { // Convert to gaussian pdf:
97  o.getMean(mean);
98 
100  o.getCovariance(o_cov);
101  o_cov.inv_fast(this->cov_inv);
102  }
103 }
104 
105 /*---------------------------------------------------------------
106  copyFrom 3D
107  ---------------------------------------------------------------*/
109 {
110  // Convert to gaussian pdf:
111  mean = CPose2D(o.getMeanVal());
112 
114  { // Cov is already in information form:
115  const CPose3DPDFGaussianInf* ptr =
116  static_cast<const CPose3DPDFGaussianInf*>(&o);
117  cov_inv(0, 0) = ptr->cov_inv(0, 0);
118  cov_inv(1, 1) = ptr->cov_inv(1, 1);
119  cov_inv(2, 2) = ptr->cov_inv(3, 3);
120  cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
121  cov_inv(0, 2) = cov_inv(2, 0) = ptr->cov_inv(0, 3);
122  cov_inv(1, 2) = cov_inv(2, 1) = ptr->cov_inv(1, 3);
123  }
124  else
125  {
127  o.getCovariance(C);
128 
129  // Clip to 3x3:
131  o_cov(0, 0) = C(0, 0);
132  o_cov(1, 1) = C(1, 1);
133  o_cov(2, 2) = C(3, 3);
134  o_cov(0, 1) = o_cov(1, 0) = C(0, 1);
135  o_cov(0, 2) = o_cov(2, 0) = C(0, 3);
136  o_cov(1, 2) = o_cov(2, 1) = C(1, 3);
137 
138  o_cov.inv_fast(this->cov_inv);
139  }
140 }
141 
142 /*---------------------------------------------------------------
143 
144  ---------------------------------------------------------------*/
146 {
147  FILE* f = os::fopen(file.c_str(), "wt");
148  if (!f) return false;
149 
150  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
151 
152  for (unsigned int i = 0; i < 3; i++)
153  os::fprintf(
154  f, "%f %f %f\n", cov_inv(i, 0), cov_inv(i, 1), cov_inv(i, 2));
155 
156  os::fclose(f);
157  return true;
158 }
159 
160 /*---------------------------------------------------------------
161  changeCoordinatesReference
162  ---------------------------------------------------------------*/
164  const CPose3D& newReferenceBase_)
165 {
166  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
167 
168  // The mean:
169  mean.composeFrom(newReferenceBase, mean);
170 
171  // The covariance:
172  rotateCov(newReferenceBase.phi());
173 }
174 
175 /*---------------------------------------------------------------
176  changeCoordinatesReference
177  ---------------------------------------------------------------*/
179  const CPose2D& newReferenceBase)
180 {
181  // The mean:
182  mean.composeFrom(newReferenceBase, mean);
183  // The covariance:
184  rotateCov(newReferenceBase.phi());
185 }
186 
187 /*---------------------------------------------------------------
188  changeCoordinatesReference
189  ---------------------------------------------------------------*/
190 void CPosePDFGaussianInf::rotateCov(const double ang)
191 {
192  const double ccos = cos(ang);
193  const double ssin = sin(ang);
194 
195  alignas(MRPT_MAX_ALIGN_BYTES)
196  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
197 
198  const CMatrixFixedNumeric<double, 3, 3> rot(rot_vals);
199 
200  // NEW_COV = H C H^T
201  // NEW_COV^(-1) = (H C H^T)^(-1) = (H^T)^(-1) C^(-1) H^(-1)
202  // rot: Inverse of a rotation matrix is its trasposed.
203  // But we need H^t^-1 -> H !! so rot stays unchanged:
204  cov_inv = (rot * cov_inv * rot.adjoint()).eval();
205 }
206 
207 /*---------------------------------------------------------------
208  drawSingleSample
209  ---------------------------------------------------------------*/
211 {
212  MRPT_START
213 
215  this->cov_inv.inv(cov);
216 
219 
220  outPart.x(mean.x() + v[0]);
221  outPart.y(mean.y() + v[1]);
222  outPart.phi(mean.phi() + v[2]);
223 
224  // Range -pi,pi
225  outPart.normalizePhi();
226 
228  cov_inv.saveToTextFile(
229  "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
230 }
231 
232 /*---------------------------------------------------------------
233  drawManySamples
234  ---------------------------------------------------------------*/
236  size_t N, std::vector<CVectorDouble>& outSamples) const
237 {
238  MRPT_START
239 
241  this->cov_inv.inv(cov);
242 
243  std::vector<CVectorDouble> rndSamples;
244 
246  outSamples.resize(N);
247  for (unsigned int i = 0; i < N; i++)
248  {
249  outSamples[i].resize(3);
250  outSamples[i][0] = mean.x() + rndSamples[i][0];
251  outSamples[i][1] = mean.y() + rndSamples[i][1];
252  outSamples[i][2] = mean.phi() + rndSamples[i][2];
253 
254  wrapToPiInPlace(outSamples[i][2]);
255  }
256 
257  MRPT_END
258 }
259 
260 /*---------------------------------------------------------------
261  bayesianFusion
262  ---------------------------------------------------------------*/
264  const CPosePDF& p1_, const CPosePDF& p2_,
265  const double minMahalanobisDistToDrop)
266 {
267  MRPT_START
268 
269  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
270 
273 
274  const CPosePDFGaussianInf* p1 =
275  static_cast<const CPosePDFGaussianInf*>(&p1_);
276  const CPosePDFGaussianInf* p2 =
277  static_cast<const CPosePDFGaussianInf*>(&p2_);
278 
279  const CMatrixDouble33& C1_inv = p1->cov_inv;
280  const CMatrixDouble33& C2_inv = p2->cov_inv;
281 
282  CMatrixDouble31 x1 = CMatrixDouble31(p1->mean);
284 
285  this->cov_inv = C1_inv + C2_inv;
286 
288  this->cov_inv.inv(cov);
289 
290  CMatrixDouble31 x = cov * (C1_inv * x1 + C2_inv * x2);
291 
292  this->mean.x(x(0, 0));
293  this->mean.y(x(1, 0));
294  this->mean.phi(x(2, 0));
295  this->mean.normalizePhi();
296 
297  MRPT_END
298 }
299 
300 /*---------------------------------------------------------------
301  inverse
302  ---------------------------------------------------------------*/
304 {
306  CPosePDFGaussianInf* out = static_cast<CPosePDFGaussianInf*>(&o);
307 
308  // The mean:
309  out->mean = CPose2D(0, 0, 0) - mean;
310 
311  // The covariance:
312  const double ccos = ::cos(mean.phi());
313  const double ssin = ::sin(mean.phi());
314 
315  // jacobian:
316  alignas(MRPT_MAX_ALIGN_BYTES) const double H_values[] = {
317  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
318  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
319  0, 0, -1};
320  const CMatrixFixedNumeric<double, 3, 3> H(H_values);
321 
322  out->cov_inv.noalias() =
323  (H * cov_inv * H.adjoint()).eval(); // o.cov = H * cov * Ht. It's the
324  // same with inverse covariances.
325 }
326 
327 /*---------------------------------------------------------------
328  +=
329  ---------------------------------------------------------------*/
331 {
332  mean = mean + Ap;
333  rotateCov(Ap.phi());
334 }
335 
336 /*---------------------------------------------------------------
337  evaluatePDF
338  ---------------------------------------------------------------*/
340 {
343 
344  return math::normalPDF(X, MU, cov_inv.inverse());
345 }
346 
347 /*---------------------------------------------------------------
348  evaluateNormalizedPDF
349  ---------------------------------------------------------------*/
351 {
354 
356  this->cov_inv.inv(cov);
357 
358  return math::normalPDF(X, MU, cov) / math::normalPDF(MU, MU, cov);
359 }
360 
361 /*---------------------------------------------------------------
362  assureSymmetry
363  ---------------------------------------------------------------*/
365 {
366  // Differences, when they exist, appear in the ~15'th significant
367  // digit, so... just take one of them arbitrarily!
368  cov_inv(0, 1) = cov_inv(1, 0);
369  cov_inv(0, 2) = cov_inv(2, 0);
370  cov_inv(1, 2) = cov_inv(2, 1);
371 }
372 
373 /*---------------------------------------------------------------
374  mahalanobisDistanceTo
375  ---------------------------------------------------------------*/
377  const CPosePDFGaussianInf& theOther)
378 {
379  MRPT_START
380 
382  MU -= CArrayDouble<3>(theOther.mean);
383 
384  wrapToPiInPlace(MU[2]);
385 
386  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
387  return 0; // This is the ONLY case where we know the result, whatever
388  // COVs are.
389 
391  this->cov_inv.inv(COV_);
392  theOther.cov_inv.inv(cov2);
393 
394  COV_ += cov2; // COV_ = cov1+cov2
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, const CPosePDFGaussianInf& obj)
410 {
411  out << "Mean: " << obj.mean << "\n";
412  out << "Inverse cov:\n" << obj.cov_inv << "\n";
413 
414  return out;
415 }
416 
417 /*---------------------------------------------------------------
418  operator +
419  ---------------------------------------------------------------*/
422 {
425  return ret;
426 }
427 
428 /*---------------------------------------------------------------
429  inverseComposition
430  Set 'this' = 'x' - 'ref', computing the mean using the "-"
431  operator and the covariances through the corresponding Jacobians.
432  ---------------------------------------------------------------*/
434  const CPosePDFGaussianInf& xv, const CPosePDFGaussianInf& xi)
435 {
436  // Use implementation in CPosePDFGaussian:
438  xv.cov_inv.inv(xv_cov);
439  xi.cov_inv.inv(xi_cov);
440 
441  const CPosePDFGaussian xv_(xv.mean, xv_cov);
442  const CPosePDFGaussian xi_(xi.mean, xi_cov);
443 
444  CPosePDFGaussian RET;
445  RET.inverseComposition(xv_, xi_);
446 
447  // Copy result to "this":
448  this->mean = RET.mean;
449  RET.cov.inv(this->cov_inv);
450 }
451 
452 /*---------------------------------------------------------------
453  inverseComposition
454  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
455  the "-" operator and the covariances through the corresponding
456  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
457  ---------------------------------------------------------------*/
459  const CPosePDFGaussianInf& x1, const CPosePDFGaussianInf& x0,
460  const CMatrixDouble33& COV_01)
461 {
462  // Use implementation in CPosePDFGaussian:
464  x1.cov_inv.inv(x1_cov);
465  x0.cov_inv.inv(x0_cov);
466 
467  const CPosePDFGaussian x1_(x1.mean, x1_cov);
468  const CPosePDFGaussian x0_(x0.mean, x0_cov);
469 
470  CPosePDFGaussian RET;
471  RET.inverseComposition(x1_, x0_, COV_01);
472 
473  // Copy result to "this":
474  this->mean = RET.mean;
475  RET.cov.inv(this->cov_inv);
476 }
477 
478 /*---------------------------------------------------------------
479  +=
480  ---------------------------------------------------------------*/
482 {
483  // COV:
485  this->cov_inv.inv(OLD_COV);
486 
487  CMatrixDouble33 df_dx, df_du;
488 
490  this->mean, // x
491  Ap.mean, // u
492  df_dx, df_du);
493 
494  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
496 
498  Ap.cov_inv.inv(Ap_cov);
499 
500  df_dx.multiply_HCHt(OLD_COV, cov);
501  df_du.multiply_HCHt(Ap_cov, cov, true); // Accumulate result
502 
503  cov.inv_fast(this->cov_inv);
504 
505  // MEAN:
506  this->mean = this->mean + Ap.mean;
507 }
508 
510  const CPosePDFGaussianInf& p1, const CPosePDFGaussianInf& p2)
511 {
512  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
513 }
514 
515 /** Pose compose operator: RES = A (+) B , computing both the mean and the
516  * covariance */
519 {
521  res += b;
522  return res;
523 }
524 
525 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and
526  * the covariance */
529 {
531  res.inverseComposition(a, b);
532  return res;
533 }
A namespace of pseudo-random numbers generators of diferent distributions.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
#define MRPT_START
Definition: exceptions.h:262
CPose2D mean
The mean value.
#define MRPT_MAX_ALIGN_BYTES
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:364
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:69
STL namespace.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:268
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:64
This base provides a set of functions for maths stuff.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
Definition: eigen_frwds.h:62
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix...
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:315
GLubyte GLubyte b
Definition: glext.h:6279
GLsizei const GLchar ** string
Definition: glext.h:4101
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
CPosePDFGaussianInf()
Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!) ...
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
static void jacobiansPoseComposition(const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true)
This static method computes the pose composition Jacobians, with these formulas:
Definition: CPosePDF.cpp:32
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:111
#define MRPT_END
Definition: exceptions.h:266
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
void inverseComposition(const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:292
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: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