MRPT  1.9.9
CPose3DPDFGaussian.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-2020, 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 
14 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/random.h>
20 #include <mrpt/system/os.h>
21 #include <Eigen/Dense>
22 
23 #include <sstream>
24 
25 using namespace mrpt;
26 using namespace mrpt::poses;
27 using namespace mrpt::math;
28 using namespace mrpt::random;
29 using namespace mrpt::system;
30 using namespace std;
31 
33 
35 {
37 }
39 {
41 }
42 
44 
45 /*---------------------------------------------------------------
46  Constructor
47  ---------------------------------------------------------------*/
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  [maybe_unused]] TConstructorFlags_Poses constructor_dummy_param)
55 {
56 }
57 
58 /*---------------------------------------------------------------
59  Constructor
60  ---------------------------------------------------------------*/
62  const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
63  : mean(init_Mean), cov(init_Cov)
64 {
65 }
66 
67 /*---------------------------------------------------------------
68  Copy Constructor from 2D PDF
69  ---------------------------------------------------------------*/
71  : mean(o.mean.x(), o.mean.y(), 0, o.mean.phi(), 0, 0), cov()
72 {
73  for (size_t i = 0; i < 3; i++)
74  {
75  const size_t ii = (i == 2) ? 3 : i;
76  for (size_t j = 0; j < 3; j++)
77  {
78  const size_t jj = (j == 2) ? 3 : j;
79  cov(ii, jj) = o.cov(i, j);
80  }
81  }
82 }
83 
84 /*---------------------------------------------------------------
85  Constructor
86  ---------------------------------------------------------------*/
88  : mean(init_Mean), cov()
89 {
90 }
91 
92 //#define DO_TEST_JACOB
93 
94 #ifdef DO_TEST_JACOB
95 void ffff(
96  const CVectorDouble& x, const CQuaternionDouble& Q, CVectorDouble& OUT)
97 {
98  OUT.resize(3);
99  CQuaternionDouble q(x[0], x[1], x[2], x[3]);
100  q.normalize();
101  q.rpy(OUT[2], OUT[1], OUT[0]);
102 }
103 #endif
104 
106  const CVectorFixedDouble<7>& x, [[maybe_unused]] const double& dummy,
108 {
109  y[0] = x[0];
110  y[1] = x[1];
111  y[2] = x[2];
112  CQuaternionDouble q(x[3], x[4], x[5], x[6]);
113  q.normalize();
114  q.rpy(y[5], y[4], y[3]);
115 }
116 
117 /*---------------------------------------------------------------
118  CPose3DPDFGaussian
119  ---------------------------------------------------------------*/
122 {
123  this->copyFrom(o);
124 }
125 
126 /*---------------------------------------------------------------
127  asString
128  ---------------------------------------------------------------*/
129 void CPose3DPDFGaussian::asString(std::string& s) const
130 {
131  ostringstream ss;
132  ss << *this;
133  s = ss.str();
134 }
135 
136 /*---------------------------------------------------------------
137  copyFrom
138  ---------------------------------------------------------------*/
140 {
141  MRPT_START
143  {
144 // Convert using Jacobians and first order approximation:
145 
146 // [ I_3 | 0 ]
147 // dr_dq = [ -------+------------- ]
148 // [ 0 | dr_dq_angles ]
149 #ifdef DO_TEST_JACOB
150  // Test Jacob:
151  {
152  CVectorDouble x(4);
153  for (int i = 0; i < 4; i++) x[i] = o.mean.quat()[i];
154  CVectorDouble Ax(4);
155  Ax.assign(1e-7);
156  CMatrixDouble H;
157  jacobians::jacob_numeric_estimate(x, ffff, Ax, o.mean.quat(), H);
158  cout << "num:" << endl << H << endl << endl;
159  CMatrixDouble J;
160  double a, b, c;
161  o.mean.quat().rpy_and_jacobian(a, b, c, &J);
162  CMatrixDouble NJ;
164  cout << "lin:" << endl << J * NJ << endl << endl;
165  }
166 #endif
167 
168  double yaw, pitch, roll;
170 
171  o.mean.quat().rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);
172 
174  o.mean.quat().normalizationJacobian(dnorm_dq);
175 
177  dr_dq_sub.asEigen() = dr_dq_sub_aux * dnorm_dq;
178 
179  // Set the mean:
180  this->mean.setFromValues(
181  o.mean.x(), o.mean.y(), o.mean.z(), yaw, pitch, roll);
182 
183  // Cov:
184  const CMatrixDouble44 cov_Q = o.cov.blockCopy<4, 4>(3, 3);
185  const CMatrixDouble33 cov_T = o.cov.blockCopy<3, 3>(0, 0);
186  const CMatrixFixed<double, 3, 4> cov_TQ = o.cov.blockCopy<3, 4>(0, 3);
187 
188  // [ S_T | S_TQ * H^t ]
189  // [ -----------------+---------------- ]
190  // [ (S_TQ * H^t)^t | H * S_Q * H^t ]
191 
192  // top-left:
193  this->cov.insertMatrix(0, 0, cov_T);
194 
195  // diagonals:
196  const Eigen::Matrix3d cov_TR = cov_TQ.asEigen() * dr_dq_sub.transpose();
197  this->cov.block<3, 3>(0, 3) = cov_TR;
198  this->cov.block<3, 3>(3, 0) = cov_TR.transpose();
199 
200  // bottom-right:
201  CMatrixDouble33 cov_r = mrpt::math::multiply_HCHt(dr_dq_sub, cov_Q);
202  this->cov.insertMatrix(3, 3, cov_r);
203  }
204  else
205  {
206  // Use UT transformation:
207  // f: R^7 => R^6
208  const CVectorFixedDouble<7> x_mean(o.mean);
209  CVectorFixedDouble<6> y_mean;
210  static const bool elements_do_wrapPI[6] = {
211  false, false, false, true, true, true}; // xyz yaw pitch roll
212 
213  const double dummy = 0;
215  x_mean, o.cov, aux_posequat2poseypr, dummy, y_mean, this->cov,
216  elements_do_wrapPI);
217  this->mean.setFromValues(
218  y_mean[0], y_mean[1], y_mean[2], y_mean[3], y_mean[4], y_mean[5]);
219  }
220  MRPT_END
221 }
222 
223 uint8_t CPose3DPDFGaussian::serializeGetVersion() const { return 1; }
225 {
226  out << mean;
228 }
230  mrpt::serialization::CArchive& in, uint8_t version)
231 {
232  switch (version)
233  {
234  case 1:
235  {
236  in >> mean;
238  }
239  break;
240  default:
242  };
243 }
244 
246 {
247  if (this == &o) return; // It may be used sometimes
248 
249  // Convert to gaussian pdf:
251 }
252 
254 {
255  // Convert to gaussian pdf:
256  CMatrixDouble33 C;
257  CPose2D p;
258  o.getCovarianceAndMean(C, p);
259  mean = CPose3D(p);
260 
261  cov.setZero();
262  cov(0, 0) = C(0, 0);
263  cov(1, 1) = C(1, 1);
264  cov(3, 3) = C(2, 2);
265 
266  cov(0, 1) = cov(1, 0) = C(0, 1);
267 
268  cov(0, 3) = cov(3, 0) = C(0, 2);
269 
270  cov(1, 3) = cov(3, 1) = C(1, 2);
271 }
272 
273 /*---------------------------------------------------------------
274 
275  ---------------------------------------------------------------*/
276 bool CPose3DPDFGaussian::saveToTextFile(const string& file) const
277 {
278  FILE* f = os::fopen(file.c_str(), "wt");
279  if (!f) return false;
280 
281  os::fprintf(
282  f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
283  mean.pitch(), mean.roll());
284 
285  for (unsigned int i = 0; i < 6; i++)
286  os::fprintf(
287  f, "%e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
288  cov(i, 3), cov(i, 4), cov(i, 5));
289 
290  os::fclose(f);
291  return true;
292 }
293 
294 /*---------------------------------------------------------------
295  changeCoordinatesReference
296  ---------------------------------------------------------------*/
298  const CPose3D& newReferenceBase)
299 {
300  MRPT_START
301  // this = p (+) this
302 
303  // COV:
304  const CMatrixDouble66 OLD_COV = this->cov;
306 
308  newReferenceBase, // x
309  this->mean, // u
310  df_dx, df_du);
311 
312  // this->cov = H1*this->cov*H1' + H2* 0 *H2';
313  cov = mrpt::math::multiply_HCHt(df_du, OLD_COV);
314 
315  // MEAN:
316  this->mean.composeFrom(newReferenceBase, this->mean);
317 
318  MRPT_END
319 }
320 
321 /*---------------------------------------------------------------
322  drawSingleSample
323  ---------------------------------------------------------------*/
325 {
326  MRPT_START
327 
328  CVectorDouble v;
330 
331  outPart.setFromValues(
332  mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
333  mean.pitch() + v[4], mean.roll() + v[5]);
334 
336  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
337 }
338 
339 /*---------------------------------------------------------------
340  drawManySamples
341  ---------------------------------------------------------------*/
343  size_t N, vector<CVectorDouble>& outSamples) const
344 {
345  MRPT_START
346 
348 
349  for (auto& outSample : outSamples)
350  {
351  outSample[0] += mean.x();
352  outSample[1] += mean.y();
353  outSample[2] += mean.z();
354  outSample[3] = math::wrapToPi(outSample[3] + mean.yaw());
355  outSample[4] = math::wrapToPi(outSample[4] + mean.pitch());
356  outSample[5] = math::wrapToPi(outSample[5] + mean.roll());
357  }
358 
359  MRPT_END
360 }
361 
362 /*---------------------------------------------------------------
363  bayesianFusion
364  ---------------------------------------------------------------*/
366  [[maybe_unused]] const CPose3DPDF& p1_,
367  [[maybe_unused]] const CPose3DPDF& p2_)
368 {
369  MRPT_START
370 
371  THROW_EXCEPTION("TO DO!!!");
372 
373  /* ASSERT_( p1_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
374  ASSERT_( p2_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
375 
376  CPose3DPDFGaussian *p1 = (CPose3DPDFGaussian*) &p1_;
377  CPose3DPDFGaussian *p2 = (CPose3DPDFGaussian*) &p2_;
378 
379 
380  CMatrixD x1(3,1),x2(3,1),x(3,1);
381  CMatrixD C1( p1->cov );
382  CMatrixD C2( p2->cov );
383  CMatrixD C1_inv = C1.inverse_LLt();
384  CMatrixD C2_inv = C2.inverse_LLt();
385  CMatrixD C;
386 
387  x1(0,0) = p1->mean.x; x1(1,0) = p1->mean.y; x1(2,0) = p1->mean.phi;
388  x2(0,0) = p2->mean.x; x2(1,0) = p2->mean.y; x2(2,0) = p2->mean.phi;
389 
390  C = !(C1_inv + C2_inv);
391 
392  this->cov = C;
393  this->enforceCovSymmetry();
394 
395  x = C * ( C1_inv*x1 + C2_inv*x2 );
396 
397  this->mean.x = x(0,0);
398  this->mean.y = x(1,0);
399  this->mean.phi = x(2,0);
400  this->mean.normalizePhi();
401  */
402  MRPT_END
403 }
404 
405 /*---------------------------------------------------------------
406  inverse
407  ---------------------------------------------------------------*/
409 {
411  auto& out = dynamic_cast<CPose3DPDFGaussian&>(o);
412 
413  // This is like: b=(0,0,0)
414  // OUT = b - THIS
415  CPose3DPDFGaussian p_zero(
416  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
417 
418  out = p_zero - *this;
419 }
420 
421 /*---------------------------------------------------------------
422  +=
423  ---------------------------------------------------------------*/
425 {
426  // COV:
427  const CMatrixDouble66 OLD_COV = this->cov;
429 
431  this->mean, // x
432  Ap, // u
433  df_dx, df_du);
434 
435  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
436  cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV);
437  // df_du: Nothing to do, since COV(Ap) = zeros
438 
439  // MEAN:
440  this->mean.composeFrom(this->mean, Ap);
441 }
442 
443 /*---------------------------------------------------------------
444  +=
445  ---------------------------------------------------------------*/
447 {
448  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
449  // Make a way around them and consider instead this path:
450  //
451  // X(6D) U(6D)
452  // | |
453  // v v
454  // X(7D) U(7D)
455  // | |
456  // +--- (+) ---+
457  // |
458  // v
459  // RES(7D)
460  // |
461  // v
462  // RES(6D)
463  //
464  CPose3DQuatPDFGaussian X7(*this);
465  const CPose3DQuatPDFGaussian U7(Ap);
466 
467  X7 += U7;
468 
469  this->copyFrom(X7);
470 }
471 
472 /*---------------------------------------------------------------
473  -=
474  ---------------------------------------------------------------*/
476 {
477  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
478  // Make a way around them and consider instead this path:
479  //
480  // X(6D) U(6D)
481  // | |
482  // v v
483  // X(7D) U(7D)
484  // | |
485  // +--- (-) ---+
486  // |
487  // v
488  // RES(7D)
489  // |
490  // v
491  // RES(6D)
492  //
493  CPose3DQuatPDFGaussian X7(*this);
494  const CPose3DQuatPDFGaussian U7(Ap);
495 
496  X7 -= U7;
497 
498  this->copyFrom(X7);
499 }
500 
501 /*---------------------------------------------------------------
502  evaluatePDF
503  ---------------------------------------------------------------*/
504 double CPose3DPDFGaussian::evaluatePDF([[maybe_unused]] const CPose3D& x) const
505 {
506  THROW_EXCEPTION("TO DO!!!");
507 
508  /* CMatrixD X(6,1);
509  X(0,0) = x.x;
510  X(1,0) = x.y;
511  X(2,0) = x.z;
512 
513  CMatrixD MU(6,1);
514  MU(0,0) = mean.x;
515  MU(1,0) = mean.y;
516  MU(2,0) = mean.z;
517 
518  return math::normalPDF( X, MU, this->cov );
519  */
520 }
521 
522 /*---------------------------------------------------------------
523  evaluateNormalizedPDF
524  ---------------------------------------------------------------*/
526  [maybe_unused]] const CPose3D& x) const
527 {
528  THROW_EXCEPTION("TO DO!!!");
529  /* CMatrixD X(3,1);
530  X(0,0) = x.x;
531  X(1,0) = x.y;
532  X(2,0) = x.phi;
533 
534  CMatrixD MU(3,1);
535  MU(0,0) = mean.x;
536  MU(1,0) = mean.y;
537  MU(2,0) = mean.phi;
538 
539  return math::normalPDF( X, MU, this->cov ) / math::normalPDF( MU, MU,
540  this->cov );
541  */
542 }
543 
545 {
546  // Differences, when they exist, appear in the ~15'th significant
547  // digit, so... just take one of them arbitrarily!
548  for (int i = 0; i < cov.rows() - 1; i++)
549  for (int j = i + 1; j < cov.rows(); j++) cov(i, j) = cov(j, i);
550 }
551 
552 /*---------------------------------------------------------------
553  mahalanobisDistanceTo
554  ---------------------------------------------------------------*/
556  const CPose3DPDFGaussian& theOther)
557 {
558  MRPT_START
559 
560  CMatrixDouble66 COV_ = cov + theOther.cov;
562 
563  for (int i = 0; i < 6; i++)
564  {
565  if (COV_(i, i) == 0)
566  {
567  if (MU(i, 0) != 0)
568  return std::numeric_limits<double>::infinity();
569  else
570  COV_(i, i) = 1; // Any arbitrary value since
571  // MU(i)=0, and this value doesn't
572  // affect the result.
573  }
574  }
575 
576  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU, COV_.inverse_LLt()));
577 
578  MRPT_END
579 }
580 
581 /*---------------------------------------------------------------
582  operator <<
583  ---------------------------------------------------------------*/
584 ostream& mrpt::poses::operator<<(ostream& out, const CPose3DPDFGaussian& obj)
585 {
586  out << "Mean: " << obj.mean << "\n";
587  out << "Covariance:\n" << obj.cov << "\n";
588 
589  return out;
590 }
591 
592 /*---------------------------------------------------------------
593  getCovSubmatrix2D
594  ---------------------------------------------------------------*/
596 {
597  out_cov.setSize(3, 3);
598 
599  for (int i = 0; i < 3; i++)
600  {
601  int a = i == 2 ? 3 : i;
602  for (int j = i; j < 3; j++)
603  {
604  int b = j == 2 ? 3 : j;
605  double f = cov(a, b);
606  out_cov(i, j) = f;
607  out_cov(j, i) = f;
608  }
609  }
610 }
611 
613  const CPose3DPDFGaussian& p1, const CPose3DPDFGaussian& p2)
614 {
615  return p1.mean == p2.mean && p1.cov == p2.cov;
616 }
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
A namespace of pseudo-random numbers generators of diferent distributions.
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
Definition: CQuaternion.h:449
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
Definition: ops_matrices.h:54
static void aux_posequat2poseypr(const CVectorFixedDouble< 7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &y)
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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
CPose3D mean
The mean value.
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
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.
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
Definition: CPose3DPDF.cpp:129
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
STL namespace.
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
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.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
#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
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in ...
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void USE_SUT_QUAT2EULER_CONVERSION(bool value)
If set to true (false), a Scaled Unscented Transform is used instead of a linear approximation with J...
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:556
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
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...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CMatrixFixed< double, 6, 1 > CMatrixDouble61
Definition: CMatrixFixed.h:377
void assign(const std::size_t N, const Scalar value)
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
CPose3DPDFGaussian()
Default constructor.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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 1x6 vectors, where each row contains a (x,y,phi) datum.
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:149
void transform_gaussian_unscented(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const bool *elem_do_wrap2pi=nullptr, const double alpha=1e-3, const double K=0, const double beta=2.0)
Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
void getCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only...
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
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
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.
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
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:85
mrpt::vision::TStereoCalibResults out
This file implements matrix/vector text and binary serialization.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:369
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
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void operator-=(const CPose3DPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
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.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void resize(std::size_t N, bool zeroNewElements=false)
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:433
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
bool USE_SUT_QUAT2EULER_CONVERSION_value
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020