MRPT  1.9.9
CPosePDFGaussianInf.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/math/ops_matrices.h>
15 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPose3DPDF.h>
21 #include <mrpt/random.h>
24 #include <mrpt/system/os.h>
25 #include <Eigen/Dense>
26 
27 using namespace mrpt;
28 using namespace mrpt::poses;
29 using namespace mrpt::math;
30 using namespace mrpt::random;
31 using namespace mrpt::system;
32 
33 using namespace std;
34 
36 
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
40 CPosePDFGaussianInf::CPosePDFGaussianInf() : mean(0, 0, 0), cov_inv() {}
41 /*---------------------------------------------------------------
42  Constructor
43  ---------------------------------------------------------------*/
45  const CPose2D& init_Mean, const CMatrixDouble33& init_CovInv)
46  : mean(init_Mean), cov_inv(init_CovInv)
47 {
48 }
49 
50 /*---------------------------------------------------------------
51  Constructor
52  ---------------------------------------------------------------*/
54  : mean(init_Mean), cov_inv()
55 {
56 }
57 
60 {
61  out << mean.x() << mean.y() << mean.phi();
62  out << cov_inv(0, 0) << cov_inv(1, 1) << cov_inv(2, 2);
63  out << cov_inv(0, 1) << cov_inv(0, 2) << cov_inv(1, 2);
64 }
67 {
68  switch (version)
69  {
70  case 0:
71  {
72  TPose2D p;
73  in >> p.x >> p.y >> p.phi;
74  mean = CPose2D(p);
75 
76  in >> cov_inv(0, 0) >> cov_inv(1, 1) >> cov_inv(2, 2);
77  in >> cov_inv(0, 1) >> cov_inv(0, 2) >> cov_inv(1, 2);
78  }
79  break;
80  default:
82  };
83 }
84 
87 {
89  out["mean"] = mean;
90  out["cov_inv"] = CMatrixD(cov_inv);
91 }
94 {
95  uint8_t version;
97  switch (version)
98  {
99  case 1:
100  {
101  in["mean"].readTo(mean);
102  CMatrixD m;
103  in["cov_inv"].readTo(m);
104  cov_inv = m;
105  }
106  break;
107  default:
109  }
110 }
111 
112 /*---------------------------------------------------------------
113  copyFrom
114  ---------------------------------------------------------------*/
116 {
117  if (this == &o) return; // It may be used sometimes
118 
120  { // It's my same class:
121  const auto* ptr = dynamic_cast<const CPosePDFGaussianInf*>(&o);
122  mean = ptr->mean;
123  cov_inv = ptr->cov_inv;
124  }
125  else
126  { // Convert to gaussian pdf:
127  o.getMean(mean);
128 
130  o.getCovariance(o_cov);
131  this->cov_inv = o_cov.inverse_LLt();
132  }
133 }
134 
135 /*---------------------------------------------------------------
136  copyFrom 3D
137  ---------------------------------------------------------------*/
139 {
140  // Convert to gaussian pdf:
141  mean = CPose2D(o.getMeanVal());
142 
144  { // Cov is already in information form:
145  const auto* ptr = dynamic_cast<const CPose3DPDFGaussianInf*>(&o);
146  cov_inv(0, 0) = ptr->cov_inv(0, 0);
147  cov_inv(1, 1) = ptr->cov_inv(1, 1);
148  cov_inv(2, 2) = ptr->cov_inv(3, 3);
149  cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
150  cov_inv(0, 2) = cov_inv(2, 0) = ptr->cov_inv(0, 3);
151  cov_inv(1, 2) = cov_inv(2, 1) = ptr->cov_inv(1, 3);
152  }
153  else
154  {
156  o.getCovariance(C);
157 
158  // Clip to 3x3:
160  o_cov(0, 0) = C(0, 0);
161  o_cov(1, 1) = C(1, 1);
162  o_cov(2, 2) = C(3, 3);
163  o_cov(0, 1) = o_cov(1, 0) = C(0, 1);
164  o_cov(0, 2) = o_cov(2, 0) = C(0, 3);
165  o_cov(1, 2) = o_cov(2, 1) = C(1, 3);
166 
167  this->cov_inv = o_cov.inverse_LLt();
168  }
169 }
170 
171 /*---------------------------------------------------------------
172 
173  ---------------------------------------------------------------*/
175 {
176  FILE* f = os::fopen(file.c_str(), "wt");
177  if (!f) return false;
178 
179  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
180 
181  for (unsigned int i = 0; i < 3; i++)
182  os::fprintf(
183  f, "%f %f %f\n", cov_inv(i, 0), cov_inv(i, 1), cov_inv(i, 2));
184 
185  os::fclose(f);
186  return true;
187 }
188 
189 /*---------------------------------------------------------------
190  changeCoordinatesReference
191  ---------------------------------------------------------------*/
193  const CPose3D& newReferenceBase_)
194 {
195  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
196 
197  // The mean:
198  mean.composeFrom(newReferenceBase, mean);
199 
200  // The covariance:
201  rotateCov(newReferenceBase.phi());
202 }
203 
204 /*---------------------------------------------------------------
205  changeCoordinatesReference
206  ---------------------------------------------------------------*/
208  const CPose2D& newReferenceBase)
209 {
210  // The mean:
211  mean.composeFrom(newReferenceBase, mean);
212  // The covariance:
213  rotateCov(newReferenceBase.phi());
214 }
215 
216 /*---------------------------------------------------------------
217  changeCoordinatesReference
218  ---------------------------------------------------------------*/
219 void CPosePDFGaussianInf::rotateCov(const double ang)
220 {
221  const double ccos = cos(ang);
222  const double ssin = sin(ang);
223 
224  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
225  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
226 
227  const CMatrixFixed<double, 3, 3> rot(rot_vals);
228 
229  // NEW_COV = H C H^T
230  // NEW_COV^(-1) = (H C H^T)^(-1) = (H^T)^(-1) C^(-1) H^(-1)
231  // rot: Inverse of a rotation matrix is its trasposed.
232  // But we need H^t^-1 -> H !! so rot stays unchanged:
233  cov_inv = rot.asEigen() * cov_inv.asEigen() * rot.asEigen().transpose();
234 }
235 
237 {
238  MRPT_START
239 
240  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
241 
244 
245  outPart.x(mean.x() + v[0]);
246  outPart.y(mean.y() + v[1]);
247  outPart.phi(mean.phi() + v[2]);
248 
249  // Range -pi,pi
250  outPart.normalizePhi();
251 
253  "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
254 }
255 
257  size_t N, std::vector<CVectorDouble>& outSamples) const
258 {
259  MRPT_START
260 
261  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
262 
263  std::vector<CVectorDouble> rndSamples;
264 
266  outSamples.resize(N);
267  for (unsigned int i = 0; i < N; i++)
268  {
269  outSamples[i].resize(3);
270  outSamples[i][0] = mean.x() + rndSamples[i][0];
271  outSamples[i][1] = mean.y() + rndSamples[i][1];
272  outSamples[i][2] = mean.phi() + rndSamples[i][2];
273 
274  wrapToPiInPlace(outSamples[i][2]);
275  }
276 
277  MRPT_END
278 }
279 
280 /*---------------------------------------------------------------
281  bayesianFusion
282  ---------------------------------------------------------------*/
284  const CPosePDF& p1_, const CPosePDF& p2_,
285  const double minMahalanobisDistToDrop)
286 {
287  MRPT_START
288 
289  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
290 
293 
294  const auto* p1 = dynamic_cast<const CPosePDFGaussianInf*>(&p1_);
295  const auto* p2 = dynamic_cast<const CPosePDFGaussianInf*>(&p2_);
296 
297  const CMatrixDouble33& C1_inv = p1->cov_inv;
298  const CMatrixDouble33& C2_inv = p2->cov_inv;
299 
300  auto x1 = CMatrixDouble31(p1->mean);
301  auto x2 = CMatrixDouble31(p2->mean);
302 
303  this->cov_inv = C1_inv + C2_inv;
304 
305  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
306 
307  auto x = CMatrixDouble31(cov.asEigen() * (C1_inv * x1 + C2_inv * x2));
308 
309  this->mean.x(x(0, 0));
310  this->mean.y(x(1, 0));
311  this->mean.phi(x(2, 0));
312  this->mean.normalizePhi();
313 
314  MRPT_END
315 }
316 
317 /*---------------------------------------------------------------
318  inverse
319  ---------------------------------------------------------------*/
321 {
323  auto* out = dynamic_cast<CPosePDFGaussianInf*>(&o);
324 
325  // The mean:
326  out->mean = CPose2D(0, 0, 0) - mean;
327 
328  // The covariance:
329  const double ccos = ::cos(mean.phi());
330  const double ssin = ::sin(mean.phi());
331 
332  // jacobian:
333  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double H_values[] = {
334  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
335  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
336  0, 0, -1};
337  const CMatrixFixed<double, 3, 3> H(H_values);
338 
339  // o.cov = H * cov * Ht. It's the same with inverse covariances.
340  out->cov_inv.asEigen().noalias() =
341  (H.asEigen() * cov_inv.asEigen() * H.transpose()).eval();
342 }
343 
344 /*---------------------------------------------------------------
345  +=
346  ---------------------------------------------------------------*/
348 {
349  mean = mean + Ap;
350  rotateCov(Ap.phi());
351 }
352 
353 /*---------------------------------------------------------------
354  evaluatePDF
355  ---------------------------------------------------------------*/
357 {
358  auto X = CMatrixDouble31(x);
359  auto MU = CMatrixDouble31(mean);
360 
361  return math::normalPDF(X, MU, cov_inv.inverse());
362 }
363 
364 /*---------------------------------------------------------------
365  evaluateNormalizedPDF
366  ---------------------------------------------------------------*/
368 {
369  auto X = CMatrixDouble31(x);
370  auto MU = CMatrixDouble31(mean);
371 
372  const CMatrixDouble33 cov = this->cov_inv.inverse_LLt();
373 
374  return math::normalPDF(X, MU, cov) / math::normalPDF(MU, MU, cov);
375 }
376 
377 /*---------------------------------------------------------------
378  enforceCovSymmetry
379  ---------------------------------------------------------------*/
381 {
382  // Differences, when they exist, appear in the ~15'th significant
383  // digit, so... just take one of them arbitrarily!
384  cov_inv(0, 1) = cov_inv(1, 0);
385  cov_inv(0, 2) = cov_inv(2, 0);
386  cov_inv(1, 2) = cov_inv(2, 1);
387 }
388 
389 /*---------------------------------------------------------------
390  mahalanobisDistanceTo
391  ---------------------------------------------------------------*/
393  const CPosePDFGaussianInf& theOther)
394 {
395  MRPT_START
396 
397  auto MU = CVectorFixedDouble<3>(mean);
398  MU -= CVectorFixedDouble<3>(theOther.mean);
399 
400  wrapToPiInPlace(MU[2]);
401 
402  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
403  return 0; // This is the ONLY case where we know the result, whatever
404  // COVs are.
405 
406  CMatrixDouble33 COV_ = this->cov_inv.inverse_LLt();
407  const CMatrixDouble33 cov2 = theOther.cov_inv.inverse_LLt();
408  COV_ += cov2; // COV_ = cov1+cov2
409 
410  const CMatrixDouble33 COV_inv = COV_.inverse_LLt();
411 
412  // (~MU) * (!COV_) * MU
413  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU.asEigen(), COV_inv));
414 
415  MRPT_END
416 }
417 
418 /*---------------------------------------------------------------
419  operator <<
420  ---------------------------------------------------------------*/
422  std::ostream& out, const CPosePDFGaussianInf& obj)
423 {
424  out << "Mean: " << obj.mean << "\n";
425  out << "Inverse cov:\n" << obj.cov_inv << "\n";
426 
427  return out;
428 }
429 
430 /*---------------------------------------------------------------
431  operator +
432  ---------------------------------------------------------------*/
435 {
438  return ret;
439 }
440 
441 /*---------------------------------------------------------------
442  inverseComposition
443  Set 'this' = 'x' - 'ref', computing the mean using the "-"
444  operator and the covariances through the corresponding Jacobians.
445  ---------------------------------------------------------------*/
447  const CPosePDFGaussianInf& xv, const CPosePDFGaussianInf& xi)
448 {
449  // Use implementation in CPosePDFGaussian:
450  const CMatrixDouble33 xv_cov = xv.cov_inv.inverse_LLt();
451  const CMatrixDouble33 xi_cov = xi.cov_inv.inverse_LLt();
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  this->cov_inv = RET.cov.inverse_LLt();
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, const CPosePDFGaussianInf& x0,
472  const CMatrixDouble33& COV_01)
473 {
474  // Use implementation in CPosePDFGaussian:
475  const CMatrixDouble33 x1_cov = x1.cov_inv.inverse_LLt();
476  const CMatrixDouble33 x0_cov = x0.cov_inv.inverse_LLt();
477 
478  const CPosePDFGaussian x1_(x1.mean, x1_cov);
479  const CPosePDFGaussian x0_(x0.mean, x0_cov);
480 
481  CPosePDFGaussian RET;
482  RET.inverseComposition(x1_, x0_, COV_01);
483 
484  // Copy result to "this":
485  this->mean = RET.mean;
486  this->cov_inv = RET.cov.inverse_LLt();
487 }
488 
489 /*---------------------------------------------------------------
490  +=
491  ---------------------------------------------------------------*/
493 {
494  // COV:
495  const CMatrixDouble33 OLD_COV = this->cov_inv.inverse_LLt();
496 
497  CMatrixDouble33 df_dx, df_du;
498 
500  this->mean, // x
501  Ap.mean, // u
502  df_dx, df_du);
503 
504  const CMatrixDouble33 Ap_cov = Ap.cov_inv.inverse_LLt();
505 
506  this->cov_inv =
507  (multiply_HCHt(df_dx, OLD_COV) + multiply_HCHt(df_du, Ap_cov))
508  .inverse_LLt();
509 
510  // MEAN:
511  this->mean = this->mean + Ap.mean;
512 }
513 
515  const CPosePDFGaussianInf& p1, const CPosePDFGaussianInf& p2)
516 {
517  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
518 }
519 
520 /** Pose compose operator: RES = A (+) B , computing both the mean and the
521  * covariance */
524 {
526  res += b;
527  return res;
528 }
529 
530 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and
531  * the covariance */
534 {
536  res.inverseComposition(a, b);
537  return res;
538 }
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.
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a column vector H and a symmetric matrix C)
Definition: ops_matrices.h:56
#define MRPT_START
Definition: exceptions.h:241
CPose2D mean
The mean value.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to 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:394
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
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...
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
virtual void getMean(type_value &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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:247
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
unsigned char uint8_t
Definition: rptypes.h:44
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
This base provides a set of functions for maths stuff.
Derived inverse() const
Returns the inverse of a general matrix using LU.
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:89
CMatrixFixed< double, 3, 1 > CMatrixDouble31
Definition: CMatrixFixed.h:357
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:356
GLubyte GLubyte b
Definition: glext.h:6372
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
CMatrixDouble 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
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLsizei const GLchar ** string
Definition: glext.h:4116
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:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
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:3684
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...
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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:86
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
#define MRPT_END
Definition: exceptions.h:245
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
double mean(const CONTAINER &v)
Computes the mean value of a vector.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
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:257
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLuint res
Definition: glext.h:7385
GLenum GLint x
Definition: glext.h:3542
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:34
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
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:6372
GLfloat GLfloat p
Definition: glext.h:6398
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:39
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
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:333
double mahalanobisDistanceTo(const CPosePDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019