MRPT  1.9.9
CPosePDFGaussian.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/CMatrixF.h>
15 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/CPose3DPDF.h>
20 #include <mrpt/random.h>
23 #include <mrpt/system/os.h>
24 #include <Eigen/Dense>
25 
26 using namespace mrpt;
27 using namespace mrpt::poses;
28 using namespace mrpt::math;
29 using namespace mrpt::random;
30 using namespace mrpt::system;
31 using namespace std;
32 
34 
35 /*---------------------------------------------------------------
36  Constructor
37  ---------------------------------------------------------------*/
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  const CPose2D& init_Mean, const CMatrixDouble33& init_Cov)
44  : mean(init_Mean), cov(init_Cov)
45 {
46 }
47 
48 /*---------------------------------------------------------------
49  Constructor
50  ---------------------------------------------------------------*/
52  : mean(init_Mean), cov()
53 {
54  cov.setZero();
55 }
56 
59 {
60  out << mean;
62 }
65 {
66  switch (version)
67  {
68  case 2:
69  {
70  in >> mean;
72  }
73  break;
74  case 1:
75  {
76  in >> mean;
79  cov = m.cast_double();
80  }
81  break;
82  case 0:
83  {
84  CMatrixF auxCov;
85  in >> mean >> auxCov;
86  cov = auxCov.cast_double();
87  }
88  break;
89  default:
91  };
92 }
93 
96 {
98  out["mean"] = mean;
99  out["cov"] = CMatrixD(cov);
100 }
103 {
104  uint8_t version;
106  switch (version)
107  {
108  case 1:
109  {
110  in["mean"].readTo(mean);
111  CMatrixD m;
112  in["cov"].readTo(m);
113  cov = m;
114  }
115  break;
116  default:
118  }
119 }
120 
121 /*---------------------------------------------------------------
122  copyFrom
123  ---------------------------------------------------------------*/
125 {
126  if (this == &o) return; // It may be used sometimes
127 
128  // Convert to gaussian pdf:
129  o.getMean(mean);
130  o.getCovariance(cov);
131 }
132 
133 /*---------------------------------------------------------------
134  copyFrom 3D
135  ---------------------------------------------------------------*/
137 {
138  // Convert to gaussian pdf:
139  mean = CPose2D(o.getMeanVal());
140  CMatrixDouble66 C;
141  o.getCovariance(C);
142 
143  // Clip to 3x3:
144  C(2, 0) = C(0, 2) = C(0, 3);
145  C(2, 1) = C(1, 2) = C(1, 3);
146  C(2, 2) = C(3, 3);
147  cov = C.block<3, 3>(0, 0);
148 }
149 
150 /*---------------------------------------------------------------
151 
152  ---------------------------------------------------------------*/
154 {
155  FILE* f = os::fopen(file.c_str(), "wt");
156  if (!f) return false;
157 
158  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.phi());
159 
160  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
161  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
162  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
163 
164  os::fclose(f);
165  return true;
166 }
167 
168 /*---------------------------------------------------------------
169  changeCoordinatesReference
170  ---------------------------------------------------------------*/
172  const CPose3D& newReferenceBase_)
173 {
174  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
175 
176  // The mean:
177  mean.composeFrom(newReferenceBase, mean);
178 
179  // The covariance:
180  rotateCov(newReferenceBase.phi());
181 }
182 
183 /*---------------------------------------------------------------
184  changeCoordinatesReference
185  ---------------------------------------------------------------*/
187  const CPose2D& newReferenceBase)
188 {
189  // The mean:
190  mean.composeFrom(newReferenceBase, mean);
191  // The covariance:
192  rotateCov(newReferenceBase.phi());
193 }
194 
195 /*---------------------------------------------------------------
196  changeCoordinatesReference
197  ---------------------------------------------------------------*/
198 void CPosePDFGaussian::rotateCov(const double ang)
199 {
200  const double ccos = cos(ang);
201  const double ssin = sin(ang);
202 
203  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
204  const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
205 
206  const CMatrixFixed<double, 3, 3> rot(rot_vals);
207  cov = (rot.asEigen() * cov.asEigen() * rot.asEigen().transpose()).eval();
208 }
209 
210 /*---------------------------------------------------------------
211  drawSingleSample
212  ---------------------------------------------------------------*/
214 {
215  MRPT_START
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.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
229 }
230 
231 /*---------------------------------------------------------------
232  drawManySamples
233  ---------------------------------------------------------------*/
235  size_t N, std::vector<CVectorDouble>& outSamples) const
236 {
237  MRPT_START
238 
239  std::vector<CVectorDouble> rndSamples;
240 
242  outSamples.resize(N);
243  for (unsigned int i = 0; i < N; i++)
244  {
245  outSamples[i].resize(3);
246  outSamples[i][0] = mean.x() + rndSamples[i][0];
247  outSamples[i][1] = mean.y() + rndSamples[i][1];
248  outSamples[i][2] = mean.phi() + rndSamples[i][2];
249 
250  wrapToPiInPlace(outSamples[i][2]);
251  }
252 
253  MRPT_END
254 }
255 
256 /*---------------------------------------------------------------
257  bayesianFusion
258  ---------------------------------------------------------------*/
260  const CPosePDF& p1_, const CPosePDF& p2_,
261  const double minMahalanobisDistToDrop)
262 {
263  MRPT_START
264 
265  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop); // Not used in this class!
266 
269 
270  const auto* p1 = dynamic_cast<const CPosePDFGaussian*>(&p1_);
271  const auto* p2 = dynamic_cast<const CPosePDFGaussian*>(&p2_);
272 
273  const CMatrixDouble33 C1 = p1->cov;
274  const CMatrixDouble33 C2 = p2->cov;
275 
276  const CMatrixDouble33 C1_inv = C1.inverse_LLt();
277  const CMatrixDouble33 C2_inv = C2.inverse_LLt();
278 
279  auto x1 = CMatrixDouble31(p1->mean);
280  auto x2 = CMatrixDouble31(p2->mean);
281 
282  CMatrixDouble33 auxC = C1_inv + C2_inv;
283  this->cov = auxC.inverse_LLt();
284  this->enforceCovSymmetry();
285 
286  auto x = CMatrixDouble31(this->cov.asEigen() * (C1_inv * x1 + C2_inv * x2));
287 
288  this->mean.x(x(0, 0));
289  this->mean.y(x(1, 0));
290  this->mean.phi(x(2, 0));
291  this->mean.normalizePhi();
292 
293  MRPT_END
294 }
295 
296 /*---------------------------------------------------------------
297  inverse
298  ---------------------------------------------------------------*/
300 {
302  auto* out = dynamic_cast<CPosePDFGaussian*>(&o);
303 
304  // The mean:
305  out->mean = CPose2D(0, 0, 0) - mean;
306 
307  // The covariance:
308  const double ccos = ::cos(mean.phi());
309  const double ssin = ::sin(mean.phi());
310 
311  // jacobian:
312  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double H_values[] = {
313  -ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
314  ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
315  0, 0, -1};
316  const CMatrixFixed<double, 3, 3> H(H_values);
317 
318  out->cov.asEigen().noalias() =
319  (H.asEigen() * cov.asEigen() * H.transpose()).eval();
320 }
321 
322 /*---------------------------------------------------------------
323  +=
324  ---------------------------------------------------------------*/
326 {
327  mean = mean + Ap;
328  rotateCov(Ap.phi());
329 }
330 
331 /*---------------------------------------------------------------
332  evaluatePDF
333  ---------------------------------------------------------------*/
335 {
336  auto X = CMatrixDouble31(x);
337  auto MU = CMatrixDouble31(mean);
338 
339  return math::normalPDF(X, MU, this->cov);
340 }
341 
342 /*---------------------------------------------------------------
343  evaluateNormalizedPDF
344  ---------------------------------------------------------------*/
346 {
347  auto X = CMatrixDouble31(x);
348  auto MU = CMatrixDouble31(mean);
349 
350  return math::normalPDF(X, MU, this->cov) /
351  math::normalPDF(MU, MU, this->cov);
352 }
353 
354 /*---------------------------------------------------------------
355  enforceCovSymmetry
356  ---------------------------------------------------------------*/
358 {
359  // Differences, when they exist, appear in the ~15'th significant
360  // digit, so... just take one of them arbitrarily!
361  cov(0, 1) = cov(1, 0);
362  cov(0, 2) = cov(2, 0);
363  cov(1, 2) = cov(2, 1);
364 }
365 
366 /*---------------------------------------------------------------
367  mahalanobisDistanceTo
368  ---------------------------------------------------------------*/
370 {
371  MRPT_START
372 
373  auto MU = CVectorFixedDouble<3>(mean);
374  MU -= CVectorFixedDouble<3>(theOther.mean);
375 
376  wrapToPiInPlace(MU[2]);
377 
378  if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
379  return 0; // This is the ONLY case where we know the result, whatever
380  // COVs are.
381 
382  CMatrixDouble33 COV_ = cov;
383  COV_ += theOther.cov;
384 
385  const CMatrixDouble33 COV_inv = COV_.inverse_LLt();
386 
387  // (~MU) * (!COV_) * MU
388  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU.asEigen(), COV_inv));
389 
390  MRPT_END
391 }
392 
393 /*---------------------------------------------------------------
394  operator <<
395  ---------------------------------------------------------------*/
397  std::ostream& out, const CPosePDFGaussian& obj)
398 {
399  out << "Mean: " << obj.mean << "\n";
400  out << "Covariance:\n" << obj.cov << "\n";
401 
402  return out;
403 }
404 
405 /*---------------------------------------------------------------
406  operator +
407  ---------------------------------------------------------------*/
410 {
413  return ret;
414 }
415 
416 /*---------------------------------------------------------------
417  assureMinCovariance
418  ---------------------------------------------------------------*/
420  const double& minStdXY, const double& minStdPhi)
421 {
422  cov(0, 0) = max(cov(0, 0), square(minStdXY));
423  cov(1, 1) = max(cov(1, 1), square(minStdXY));
424  cov(2, 2) = max(cov(2, 2), square(minStdPhi));
425 }
426 
427 /*---------------------------------------------------------------
428  inverseComposition
429  Set 'this' = 'x' - 'ref', computing the mean using the "-"
430  operator and the covariances through the corresponding Jacobians.
431  ---------------------------------------------------------------*/
433  const CPosePDFGaussian& xv, const CPosePDFGaussian& xi)
434 {
435  // COV:
436  double cpi = cos(xi.mean.phi());
437  double spi = sin(xi.mean.phi());
438 
439  // jacob: dh_xv
440  CMatrixDouble33 dh_xv;
441 
442  dh_xv(0, 0) = cpi;
443  dh_xv(0, 1) = spi;
444  dh_xv(1, 0) = -spi;
445  dh_xv(1, 1) = cpi;
446  dh_xv(2, 2) = 1;
447 
448  // jacob: dh_xi
449  CMatrixDouble33 dh_xi;
450 
451  double xv_xi = xv.mean.x() - xi.mean.x();
452  double yv_yi = xv.mean.y() - xi.mean.y();
453 
454  dh_xi(0, 0) = -cpi;
455  dh_xi(0, 1) = -spi;
456  dh_xi(0, 2) = -xv_xi * spi + yv_yi * cpi;
457  dh_xi(1, 0) = spi;
458  dh_xi(1, 1) = -cpi;
459  dh_xi(1, 2) = -xv_xi * cpi - yv_yi * spi;
460  dh_xi(2, 2) = -1;
461 
462  // Build the cov:
463  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T
464  this->cov = mrpt::math::multiply_HCHt(dh_xv, xv.cov) +
465  mrpt::math::multiply_HCHt(dh_xi, xi.cov);
466 
467  // Mean:
468  mean = xv.mean - xi.mean;
469 }
470 
471 /*---------------------------------------------------------------
472  inverseComposition
473  Set \f$ this = x1 \ominus x0 \f$ , computing the mean using
474  the "-" operator and the covariances through the corresponding
475  Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x0).
476  ---------------------------------------------------------------*/
478  const CPosePDFGaussian& x1, const CPosePDFGaussian& x0,
479  const CMatrixDouble33& COV_01)
480 {
481  double cp0 = cos(x0.mean.phi());
482  double sp0 = sin(x0.mean.phi());
483 
484  // jacob: dh_x1
485  CMatrixDouble33 dh_x1;
486 
487  dh_x1(0, 0) = cp0;
488  dh_x1(0, 1) = sp0;
489  dh_x1(1, 0) = -sp0;
490  dh_x1(1, 1) = cp0;
491  dh_x1(2, 2) = 1;
492 
493  // jacob: dh_x0
494  CMatrixDouble33 dh_x0;
495 
496  double xv_xi = x1.mean.x() - x0.mean.x();
497  double yv_yi = x1.mean.y() - x0.mean.y();
498 
499  dh_x0(0, 0) = -cp0;
500  dh_x0(0, 1) = -sp0;
501  dh_x0(0, 2) = -xv_xi * sp0 + yv_yi * cp0;
502  dh_x0(1, 0) = sp0;
503  dh_x0(1, 1) = -cp0;
504  dh_x0(1, 2) = -xv_xi * cp0 - yv_yi * sp0;
505  dh_x0(2, 2) = -1;
506 
507  // Build the cov:
508  // Y = dh_xv * XV * dh_xv^T + dh_xi * XI * dh_xi^T + A + At
509  // being A = dh_dx0 * COV01 * dh_dx1^t
510  this->cov = mrpt::math::multiply_HCHt(dh_x0, x0.cov) +
511  mrpt::math::multiply_HCHt(dh_x1, x1.cov);
512 
513  auto M =
514  CMatrixDouble33(dh_x0.asEigen() * COV_01.asEigen() * dh_x1.asEigen());
515 
516  this->cov.asEigen() += (M.asEigen() * M.transpose()).eval();
517 
518  // mean
519  mean = x1.mean - x0.mean;
520 }
521 
522 /*---------------------------------------------------------------
523  +=
524  ---------------------------------------------------------------*/
526 {
527  // COV:
528  const CMatrixDouble33 OLD_COV = this->cov;
530 
532  this->mean, // x
533  Ap.mean, // u
534  df_dx, df_du);
535 
536  this->cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV) +
537  mrpt::math::multiply_HCHt(df_du, Ap.cov);
538 
539  // MEAN:
540  this->mean = this->mean + Ap.mean;
541 }
542 
543 /** Returns the PDF of the 2D point \f$ g = q \oplus l\f$ with "q"=this pose and
544  * "l" a point without uncertainty */
546  const mrpt::math::TPoint2D& l, CPoint2DPDFGaussian& g) const
547 {
548  // Mean:
549  double gx, gy;
550  mean.composePoint(l.x, l.y, gx, gy);
551  g.mean.x(gx);
552  g.mean.y(gy);
553 
554  // COV:
557  this->mean, // x
558  this->mean, // u
559  df_dx, df_du,
560  true, // Eval df_dx
561  false // Eval df_du (not needed)
562  );
563 
564  const auto dp_dx = CMatrixFixed<double, 2, 3>(df_dx.block<2, 3>(0, 0));
565  g.cov = dp_dx * this->cov * dp_dx.transpose();
566 }
567 
569  const CPosePDFGaussian& p1, const CPosePDFGaussian& p2)
570 {
571  return p1.mean == p2.mean && p1.cov == p2.cov;
572 }
573 
575  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
576 {
578  res += b;
579  return res;
580 }
581 
583  const CPosePDFGaussian& a, const CPosePDFGaussian& b)
584 {
586  res.inverseComposition(a, b);
587  return res;
588 }
A namespace of pseudo-random numbers generators of diferent distributions.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
#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
A gaussian distribution for 2D points.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
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)...
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.
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
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:199
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
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
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.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#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
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
CPosePDFGaussian()
Default constructor.
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...
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
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.
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.
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
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
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
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
void assureMinCovariance(const double &minStdXY, const double &minStdPhi)
Substitutes the diagonal elements if (square) they are below some given minimum values (Use this befo...
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
This file implements matrix/vector text and binary serialization.
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
GLuint in
Definition: glext.h:7391
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
#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
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Lightweight 2D point.
Definition: TPoint2D.h:31
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:34
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CMatrixFixed< double, ROWS, COLS > cast_double() const
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
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...
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
#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
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019