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-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 
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 
30 using namespace mrpt::system;
31 using namespace std;
32 
34 
36 {
38 }
40 {
42 }
43 
45 
46 /*---------------------------------------------------------------
47  Constructor
48  ---------------------------------------------------------------*/
50 /*---------------------------------------------------------------
51  Constructor
52  ---------------------------------------------------------------*/
54  TConstructorFlags_Poses constructor_dummy_param)
56 {
57  MRPT_UNUSED_PARAM(constructor_dummy_param);
58 }
59 
60 /*---------------------------------------------------------------
61  Constructor
62  ---------------------------------------------------------------*/
64  const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
65  : mean(init_Mean), cov(init_Cov)
66 {
67 }
68 
69 /*---------------------------------------------------------------
70  Copy Constructor from 2D PDF
71  ---------------------------------------------------------------*/
73  : mean(o.mean.x(), o.mean.y(), 0, o.mean.phi(), 0, 0), cov()
74 {
75  for (size_t i = 0; i < 3; i++)
76  {
77  const size_t ii = (i == 2) ? 3 : i;
78  for (size_t j = 0; j < 3; j++)
79  {
80  const size_t jj = (j == 2) ? 3 : j;
81  cov(ii, jj) = o.cov(i, j);
82  }
83  }
84 }
85 
86 /*---------------------------------------------------------------
87  Constructor
88  ---------------------------------------------------------------*/
90  : mean(init_Mean), cov()
91 {
92 }
93 
94 //#define DO_TEST_JACOB
95 
96 #ifdef DO_TEST_JACOB
97 void ffff(
98  const CVectorDouble& x, const CQuaternionDouble& Q, CVectorDouble& OUT)
99 {
100  OUT.resize(3);
101  CQuaternionDouble q(x[0], x[1], x[2], x[3]);
102  q.normalize();
103  q.rpy(OUT[2], OUT[1], OUT[0]);
104 }
105 #endif
106 
108  const CVectorFixedDouble<7>& x, const double& dummy,
110 {
111  MRPT_UNUSED_PARAM(dummy);
112  y[0] = x[0];
113  y[1] = x[1];
114  y[2] = x[2];
115  CQuaternionDouble q(x[3], x[4], x[5], x[6]);
116  q.normalize();
117  q.rpy(y[5], y[4], y[3]);
118 }
119 
120 /*---------------------------------------------------------------
121  CPose3DPDFGaussian
122  ---------------------------------------------------------------*/
125 {
126  this->copyFrom(o);
127 }
128 
129 /*---------------------------------------------------------------
130  asString
131  ---------------------------------------------------------------*/
133 {
134  ostringstream ss;
135  ss << *this;
136  s = ss.str();
137 }
138 
139 /*---------------------------------------------------------------
140  copyFrom
141  ---------------------------------------------------------------*/
143 {
144  MRPT_START
146  {
147 // Convert using Jacobians and first order approximation:
148 
149 // [ I_3 | 0 ]
150 // dr_dq = [ -------+------------- ]
151 // [ 0 | dr_dq_angles ]
152 #ifdef DO_TEST_JACOB
153  // Test Jacob:
154  {
155  CVectorDouble x(4);
156  for (int i = 0; i < 4; i++) x[i] = o.mean.quat()[i];
157  CVectorDouble Ax(4);
158  Ax.assign(1e-7);
159  CMatrixDouble H;
160  jacobians::jacob_numeric_estimate(x, ffff, Ax, o.mean.quat(), H);
161  cout << "num:" << endl << H << endl << endl;
162  CMatrixDouble J;
163  double a, b, c;
164  o.mean.quat().rpy_and_jacobian(a, b, c, &J);
165  CMatrixDouble NJ;
167  cout << "lin:" << endl << J * NJ << endl << endl;
168  }
169 #endif
170 
171  double yaw, pitch, roll;
173 
174  o.mean.quat().rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);
175 
177  o.mean.quat().normalizationJacobian(dnorm_dq);
178 
180  dr_dq_sub.asEigen() = dr_dq_sub_aux * dnorm_dq;
181 
182  // Set the mean:
183  this->mean.setFromValues(
184  o.mean.x(), o.mean.y(), o.mean.z(), yaw, pitch, roll);
185 
186  // Cov:
187  const CMatrixDouble44 cov_Q = o.cov.blockCopy<4, 4>(3, 3);
188  const CMatrixDouble33 cov_T = o.cov.blockCopy<3, 3>(0, 0);
189  const CMatrixFixed<double, 3, 4> cov_TQ = o.cov.blockCopy<3, 4>(0, 3);
190 
191  // [ S_T | S_TQ * H^t ]
192  // [ -----------------+---------------- ]
193  // [ (S_TQ * H^t)^t | H * S_Q * H^t ]
194 
195  // top-left:
196  this->cov.insertMatrix(0, 0, cov_T);
197 
198  // diagonals:
199  const Eigen::Matrix3d cov_TR = cov_TQ.asEigen() * dr_dq_sub.transpose();
200  this->cov.block<3, 3>(0, 3) = cov_TR;
201  this->cov.block<3, 3>(3, 0) = cov_TR.transpose();
202 
203  // bottom-right:
204  CMatrixDouble33 cov_r = mrpt::math::multiply_HCHt(dr_dq_sub, cov_Q);
205  this->cov.insertMatrix(3, 3, cov_r);
206  }
207  else
208  {
209  // Use UT transformation:
210  // f: R^7 => R^6
211  const CVectorFixedDouble<7> x_mean(o.mean);
212  CVectorFixedDouble<6> y_mean;
213  static const bool elements_do_wrapPI[6] = {
214  false, false, false, true, true, true}; // xyz yaw pitch roll
215 
216  static const double dummy = 0;
218  x_mean, o.cov, aux_posequat2poseypr, dummy, y_mean, this->cov,
219  elements_do_wrapPI);
220  this->mean.setFromValues(
221  y_mean[0], y_mean[1], y_mean[2], y_mean[3], y_mean[4], y_mean[5]);
222  }
223  MRPT_END
224 }
225 
226 uint8_t CPose3DPDFGaussian::serializeGetVersion() const { return 1; }
228 {
229  out << mean;
231 }
233  mrpt::serialization::CArchive& in, uint8_t version)
234 {
235  switch (version)
236  {
237  case 1:
238  {
239  in >> mean;
241  }
242  break;
243  default:
245  };
246 }
247 
249 {
250  if (this == &o) return; // It may be used sometimes
251 
252  // Convert to gaussian pdf:
254 }
255 
257 {
258  // Convert to gaussian pdf:
259  CMatrixDouble33 C;
260  CPose2D p;
261  o.getCovarianceAndMean(C, p);
262  mean = CPose3D(p);
263 
264  cov.setZero();
265  cov(0, 0) = C(0, 0);
266  cov(1, 1) = C(1, 1);
267  cov(3, 3) = C(2, 2);
268 
269  cov(0, 1) = cov(1, 0) = C(0, 1);
270 
271  cov(0, 3) = cov(3, 0) = C(0, 2);
272 
273  cov(1, 3) = cov(3, 1) = C(1, 2);
274 }
275 
276 /*---------------------------------------------------------------
277 
278  ---------------------------------------------------------------*/
279 bool CPose3DPDFGaussian::saveToTextFile(const string& file) const
280 {
281  FILE* f = os::fopen(file.c_str(), "wt");
282  if (!f) return false;
283 
284  os::fprintf(
285  f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
286  mean.pitch(), mean.roll());
287 
288  for (unsigned int i = 0; i < 6; i++)
289  os::fprintf(
290  f, "%e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
291  cov(i, 3), cov(i, 4), cov(i, 5));
292 
293  os::fclose(f);
294  return true;
295 }
296 
297 /*---------------------------------------------------------------
298  changeCoordinatesReference
299  ---------------------------------------------------------------*/
301  const CPose3D& newReferenceBase)
302 {
303  MRPT_START
304  // this = p (+) this
305 
306  // COV:
307  const CMatrixDouble66 OLD_COV = this->cov;
309 
311  newReferenceBase, // x
312  this->mean, // u
313  df_dx, df_du);
314 
315  // this->cov = H1*this->cov*H1' + H2* 0 *H2';
316  cov = mrpt::math::multiply_HCHt(df_du, OLD_COV);
317 
318  // MEAN:
319  this->mean.composeFrom(newReferenceBase, this->mean);
320 
321  MRPT_END
322 }
323 
324 /*---------------------------------------------------------------
325  drawSingleSample
326  ---------------------------------------------------------------*/
328 {
329  MRPT_START
330 
333 
334  outPart.setFromValues(
335  mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
336  mean.pitch() + v[4], mean.roll() + v[5]);
337 
339  cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
340 }
341 
342 /*---------------------------------------------------------------
343  drawManySamples
344  ---------------------------------------------------------------*/
346  size_t N, vector<CVectorDouble>& outSamples) const
347 {
348  MRPT_START
349 
351 
352  for (auto& outSample : outSamples)
353  {
354  outSample[0] += mean.x();
355  outSample[1] += mean.y();
356  outSample[2] += mean.z();
357  outSample[3] = math::wrapToPi(outSample[3] + mean.yaw());
358  outSample[4] = math::wrapToPi(outSample[4] + mean.pitch());
359  outSample[5] = math::wrapToPi(outSample[5] + mean.roll());
360  }
361 
362  MRPT_END
363 }
364 
365 /*---------------------------------------------------------------
366  bayesianFusion
367  ---------------------------------------------------------------*/
369  const CPose3DPDF& p1_, const CPose3DPDF& p2_)
370 {
371  MRPT_UNUSED_PARAM(p1_);
372  MRPT_UNUSED_PARAM(p2_);
373  MRPT_START
374 
375  THROW_EXCEPTION("TO DO!!!");
376 
377  /* ASSERT_( p1_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
378  ASSERT_( p2_.GetRuntimeClass() == CLASS_ID( CPose3DPDFGaussian ) );
379 
380  CPose3DPDFGaussian *p1 = (CPose3DPDFGaussian*) &p1_;
381  CPose3DPDFGaussian *p2 = (CPose3DPDFGaussian*) &p2_;
382 
383 
384  CMatrixD x1(3,1),x2(3,1),x(3,1);
385  CMatrixD C1( p1->cov );
386  CMatrixD C2( p2->cov );
387  CMatrixD C1_inv = C1.inverse_LLt();
388  CMatrixD C2_inv = C2.inverse_LLt();
389  CMatrixD C;
390 
391  x1(0,0) = p1->mean.x; x1(1,0) = p1->mean.y; x1(2,0) = p1->mean.phi;
392  x2(0,0) = p2->mean.x; x2(1,0) = p2->mean.y; x2(2,0) = p2->mean.phi;
393 
394  C = !(C1_inv + C2_inv);
395 
396  this->cov = C;
397  this->enforceCovSymmetry();
398 
399  x = C * ( C1_inv*x1 + C2_inv*x2 );
400 
401  this->mean.x = x(0,0);
402  this->mean.y = x(1,0);
403  this->mean.phi = x(2,0);
404  this->mean.normalizePhi();
405  */
406  MRPT_END
407 }
408 
409 /*---------------------------------------------------------------
410  inverse
411  ---------------------------------------------------------------*/
413 {
415  auto& out = dynamic_cast<CPose3DPDFGaussian&>(o);
416 
417  // This is like: b=(0,0,0)
418  // OUT = b - THIS
419  CPose3DPDFGaussian p_zero(
420  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
421 
422  out = p_zero - *this;
423 }
424 
425 /*---------------------------------------------------------------
426  +=
427  ---------------------------------------------------------------*/
429 {
430  // COV:
431  const CMatrixDouble66 OLD_COV = this->cov;
433 
435  this->mean, // x
436  Ap, // u
437  df_dx, df_du);
438 
439  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
440  cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV);
441  // df_du: Nothing to do, since COV(Ap) = zeros
442 
443  // MEAN:
444  this->mean.composeFrom(this->mean, Ap);
445 }
446 
447 /*---------------------------------------------------------------
448  +=
449  ---------------------------------------------------------------*/
451 {
452  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
453  // Make a way around them and consider instead this path:
454  //
455  // X(6D) U(6D)
456  // | |
457  // v v
458  // X(7D) U(7D)
459  // | |
460  // +--- (+) ---+
461  // |
462  // v
463  // RES(7D)
464  // |
465  // v
466  // RES(6D)
467  //
468  CPose3DQuatPDFGaussian X7(*this);
469  const CPose3DQuatPDFGaussian U7(Ap);
470 
471  X7 += U7;
472 
473  this->copyFrom(X7);
474 }
475 
476 /*---------------------------------------------------------------
477  -=
478  ---------------------------------------------------------------*/
480 {
481  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
482  // Make a way around them and consider instead this path:
483  //
484  // X(6D) U(6D)
485  // | |
486  // v v
487  // X(7D) U(7D)
488  // | |
489  // +--- (-) ---+
490  // |
491  // v
492  // RES(7D)
493  // |
494  // v
495  // RES(6D)
496  //
497  CPose3DQuatPDFGaussian X7(*this);
498  const CPose3DQuatPDFGaussian U7(Ap);
499 
500  X7 -= U7;
501 
502  this->copyFrom(X7);
503 }
504 
505 /*---------------------------------------------------------------
506  evaluatePDF
507  ---------------------------------------------------------------*/
509 {
511  THROW_EXCEPTION("TO DO!!!");
512 
513  /* CMatrixD X(6,1);
514  X(0,0) = x.x;
515  X(1,0) = x.y;
516  X(2,0) = x.z;
517 
518  CMatrixD MU(6,1);
519  MU(0,0) = mean.x;
520  MU(1,0) = mean.y;
521  MU(2,0) = mean.z;
522 
523  return math::normalPDF( X, MU, this->cov );
524  */
525 }
526 
527 /*---------------------------------------------------------------
528  evaluateNormalizedPDF
529  ---------------------------------------------------------------*/
531 {
533  THROW_EXCEPTION("TO DO!!!");
534  /* CMatrixD X(3,1);
535  X(0,0) = x.x;
536  X(1,0) = x.y;
537  X(2,0) = x.phi;
538 
539  CMatrixD MU(3,1);
540  MU(0,0) = mean.x;
541  MU(1,0) = mean.y;
542  MU(2,0) = mean.phi;
543 
544  return math::normalPDF( X, MU, this->cov ) / math::normalPDF( MU, MU,
545  this->cov );
546  */
547 }
548 
550 {
551  // Differences, when they exist, appear in the ~15'th significant
552  // digit, so... just take one of them arbitrarily!
553  for (int i = 0; i < cov.rows() - 1; i++)
554  for (int j = i + 1; j < cov.rows(); j++) cov(i, j) = cov(j, i);
555 }
556 
557 /*---------------------------------------------------------------
558  mahalanobisDistanceTo
559  ---------------------------------------------------------------*/
561  const CPose3DPDFGaussian& theOther)
562 {
563  MRPT_START
564 
565  CMatrixDouble66 COV_ = cov + theOther.cov;
567 
568  for (int i = 0; i < 6; i++)
569  {
570  if (COV_(i, i) == 0)
571  {
572  if (MU(i, 0) != 0)
573  return std::numeric_limits<double>::infinity();
574  else
575  COV_(i, i) = 1; // Any arbitrary value since
576  // MU(i)=0, and this value doesn't
577  // affect the result.
578  }
579  }
580 
581  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU, COV_.inverse_LLt()));
582 
583  MRPT_END
584 }
585 
586 /*---------------------------------------------------------------
587  operator <<
588  ---------------------------------------------------------------*/
590 {
591  out << "Mean: " << obj.mean << "\n";
592  out << "Covariance:\n" << obj.cov << "\n";
593 
594  return out;
595 }
596 
597 /*---------------------------------------------------------------
598  getCovSubmatrix2D
599  ---------------------------------------------------------------*/
601 {
602  out_cov.setSize(3, 3);
603 
604  for (int i = 0; i < 3; i++)
605  {
606  int a = i == 2 ? 3 : i;
607  for (int j = i; j < 3; j++)
608  {
609  int b = j == 2 ? 3 : j;
610  double f = cov(a, b);
611  out_cov(i, j) = f;
612  out_cov(j, i) = f;
613  }
614  }
615 }
616 
618  const CPose3DPDFGaussian& p1, const CPose3DPDFGaussian& p2)
619 {
620  return p1.mean == p2.mean && p1.cov == p2.cov;
621 }
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:414
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
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.
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
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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
GLdouble s
Definition: glext.h:3682
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.
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:572
const GLubyte * c
Definition: glext.h:6406
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:362
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.
GLubyte GLubyte b
Definition: glext.h:6372
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...
GLsizei const GLchar ** string
Definition: glext.h:4116
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:410
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...
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
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:286
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
GLuint in
Definition: glext.h:7391
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:354
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).
GLenum GLint GLint y
Definition: glext.h:3542
static void aux_posequat2poseypr(const CVectorFixedDouble< 7 > &x, const double &dummy, CVectorFixedDouble< 6 > &y)
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.
GLsizei const GLfloat * value
Definition: glext.h:4134
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void resize(std::size_t N, bool zeroNewElements=false)
GLenum GLint x
Definition: glext.h:3542
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.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
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
#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: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019