MRPT  1.9.9
CPose3DQuatPDFGaussian.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/CPose3DQuat.h>
20 #include <mrpt/system/os.h>
22 
23 using namespace mrpt;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::random;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
33 {
35 }
37 {
39 }
40 
42 
43 /** Default constructor - set all values to zero. */
45 // Un-initialized constructor:
47  TConstructorFlags_Quaternions constructor_dummy_param)
49 {
50  MRPT_UNUSED_PARAM(constructor_dummy_param);
51 }
52 
53 /** Constructor from a default mean value, covariance equals to zero. */
55  : mean(init_Mean), cov()
56 {
57 }
58 
59 /** Constructor with mean and covariance. */
61  const CPose3DQuat& init_Mean, const CMatrixDouble77& init_Cov)
62  : mean(init_Mean), cov(init_Cov)
63 {
64 }
65 
66 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables).
67  */
70 {
71  this->copyFrom(CPose3DPDFGaussian(o));
72 }
73 
74 /** Constructor from an equivalent Gaussian in Euler angles representation. */
77 {
78  this->copyFrom(o);
79 }
80 
81 /*---------------------------------------------------------------
82  getMean
83  Returns an estimate of the pose, (the mean, or mathematical expectation of the
84  PDF)
85  ---------------------------------------------------------------*/
87 /*---------------------------------------------------------------
88  getCovarianceAndMean
89  ---------------------------------------------------------------*/
91  CMatrixDouble77& C, CPose3DQuat& p) const
92 {
93  C = cov;
94  p = mean;
95 }
96 
100 {
101  out << mean;
103 }
106 {
107  switch (version)
108  {
109  case 0:
110  {
111  in >> mean;
113  }
114  break;
115  default:
117  };
118 }
119 
121 {
122  if (this == &o) return; // It may be used sometimes
123 
124  // Convert to gaussian pdf:
126 }
127 
129 {
130  CPose3DPDFGaussian aux;
131  aux.copyFrom(o);
132  this->copyFrom(aux);
133 }
134 
136  const CArrayDouble<6>& x, const double& dummy, CArrayDouble<7>& y)
137 {
138  MRPT_UNUSED_PARAM(dummy);
139  y[0] = x[0];
140  y[1] = x[1];
141  y[2] = x[2];
142 
143  CPose3D p(0, 0, 0, x[3], x[4], x[5]);
145  p.getAsQuaternion(q);
146  y[3] = q[0];
147  y[4] = q[1];
148  y[5] = q[2];
149  y[6] = q[3];
150 }
151 
153 {
155  { // Use Jacobians
157 
158  // Mean:
159  mean.x(o.mean.x());
160  mean.y(o.mean.y());
161  mean.z(o.mean.z());
162 
163  o.mean.getAsQuaternion(mean.quat(), &dq_dr_sub);
164 
165 // Cov:
166 #if 1
168  dq_dr.get_unsafe(0, 0) = dq_dr.get_unsafe(1, 1) =
169  dq_dr.get_unsafe(2, 2) = 1;
170  dq_dr.insertMatrix(3, 3, dq_dr_sub);
171  // Now for the covariance:
172  dq_dr.multiply_HCHt(o.cov, this->cov);
173 #else
177  o.cov.extractMatrix(3, 3, cov_R);
178  o.cov.extractMatrix(0, 0, cov_T);
179  o.cov.extractMatrix(0, 3, cov_TR);
180 
181  // [ S_T | S_TR * H^t ]
182  // [ -----------------+---------------- ]
183  // [ (S_TR * H^t)^t | H * S_R * H^t ]
184 
185  // top-left:
186  this->cov.insertMatrix(0, 0, cov_T);
187 
188  // diagonals:
190  cov_TQ.multiply_ABt(cov_TR, dq_dr_sub);
191  this->cov.insertMatrix(0, 3, cov_TQ);
192  this->cov.insertMatrixTranspose(3, 0, cov_TQ);
193 
194  // bottom-right:
196  dq_dr_sub.multiply_HCHt(cov_R, cov_q);
197  this->cov.insertMatrix(3, 3, cov_q);
198 #endif
199  }
200  else
201  {
202  // Use UT transformation:
203  // f: R^6 => R^7
204  const CArrayDouble<6> x_mean(o.mean);
205 
206  static const double dummy = 0;
208  x_mean, o.cov, &aux_poseypr2posequat, dummy, this->mean, this->cov);
209  }
210 }
211 
212 /*---------------------------------------------------------------
213  saveToTextFile
214  ---------------------------------------------------------------*/
215 bool CPose3DQuatPDFGaussian::saveToTextFile(const string& file) const
216 {
217  FILE* f = os::fopen(file.c_str(), "wt");
218  if (!f) return false;
219 
220  os::fprintf(
221  f, "%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(),
222  mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
223 
224  for (unsigned int i = 0; i < 7; i++)
225  os::fprintf(
226  f, "%e %e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
227  cov(i, 3), cov(i, 4), cov(i, 5), cov(i, 6));
228 
229  os::fclose(f);
230  return true;
231 }
232 
233 /*---------------------------------------------------------------
234  changeCoordinatesReference
235  ---------------------------------------------------------------*/
237  const CPose3D& newReferenceBase)
238 {
239  MRPT_START
240  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
241  MRPT_END
242 }
243 
244 /*---------------------------------------------------------------
245  changeCoordinatesReference
246  ---------------------------------------------------------------*/
248  const CPose3DQuat& newReferenceBaseQuat)
249 {
250  MRPT_START
251 
252  // COV:
253  const CMatrixDouble77 OLD_COV = this->cov;
255 
257  newReferenceBaseQuat, // x
258  this->mean, // u
259  df_dx, df_du,
260  &this->mean // Output: newReferenceBaseQuat + this->mean;
261  );
262 
263  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
264  // df_dx: not used, since its COV are all zeros... // df_dx.multiply_HCHt(
265  // OLD_COV, cov );
266  df_du.multiply_HCHt(OLD_COV, cov);
267 
268  MRPT_END
269 }
270 
271 /*---------------------------------------------------------------
272  drawSingleSample
273  ---------------------------------------------------------------*/
275 {
276  MRPT_START
278  MRPT_END
279 }
280 
281 /*---------------------------------------------------------------
282  drawManySamples
283  ---------------------------------------------------------------*/
285  size_t N, vector<CVectorDouble>& outSamples) const
286 {
287  MRPT_START
288 
290 
291  for (vector<CVectorDouble>::iterator it = outSamples.begin();
292  it != outSamples.end(); ++it)
293  for (unsigned int k = 0; k < 7; k++) (*it)[k] += mean[k];
294 
295  MRPT_END
296 }
297 
298 /*---------------------------------------------------------------
299  inverse
300  ---------------------------------------------------------------*/
302 {
304  CPose3DQuatPDFGaussian& out = static_cast<CPose3DQuatPDFGaussian&>(o);
305 
306  // COV:
308  double lx, ly, lz;
309  mean.inverseComposePoint(0, 0, 0, lx, ly, lz, nullptr, &df_dpose);
310 
312  jacob.insertMatrix(0, 0, df_dpose);
313  jacob.set_unsafe(3, 3, 1);
314  jacob.set_unsafe(4, 4, -1);
315  jacob.set_unsafe(5, 5, -1);
316  jacob.set_unsafe(6, 6, -1);
317 
318  // C(0:2,0:2): H C H^t
319  jacob.multiply_HCHt(this->cov, out.cov);
320 
321  // Mean:
322  out.mean.x(lx);
323  out.mean.y(ly);
324  out.mean.z(lz);
325  this->mean.quat().conj(out.mean.quat());
326 }
327 
328 /*---------------------------------------------------------------
329  +=
330  ---------------------------------------------------------------*/
332 {
333  // COV:
334  const CMatrixDouble77 OLD_COV = this->cov;
336 
338  this->mean, // x
339  Ap, // u
340  df_dx, df_du,
341  &this->mean // Output: this->mean + Ap;
342  );
343 
344  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
345  df_dx.multiply_HCHt(OLD_COV, cov);
346  // df_du: Nothing to do, since COV(Ap) = zeros
347 }
348 
349 /*---------------------------------------------------------------
350  +=
351  ---------------------------------------------------------------*/
353 {
354  // COV:
355  const CMatrixDouble77 OLD_COV = this->cov;
357 
359  this->mean, // x
360  Ap.mean, // u
361  df_dx, df_du,
362  &this->mean // Output: this->mean + Ap.mean;
363  );
364 
365  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
366  df_dx.multiply_HCHt(OLD_COV, cov);
367  df_du.multiply_HCHt(Ap.cov, cov, true); // Accumulate result
368 }
369 
370 /*---------------------------------------------------------------
371  -=
372  ---------------------------------------------------------------*/
374 {
375  // THIS = THIS (-) Ap -->
376  // THIS = inverse(Ap) (+) THIS
377  CPose3DQuatPDFGaussian inv_Ap = -Ap;
378  *this = inv_Ap + *this;
379 }
380 
381 /*---------------------------------------------------------------
382  evaluatePDF
383  ---------------------------------------------------------------*/
385 {
386  return mrpt::math::normalPDF(
387  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov);
388 }
389 
390 /*---------------------------------------------------------------
391  evaluateNormalizedPDF
392  ---------------------------------------------------------------*/
394 {
395  return mrpt::math::normalPDF(
396  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov, true);
397 }
398 
399 /*---------------------------------------------------------------
400  assureSymmetry
401  ---------------------------------------------------------------*/
403 {
404  // Differences, when they exist, appear in the ~15'th significant
405  // digit, so... just take one of them arbitrarily!
406  for (int i = 0; i < cov.rows() - 1; i++)
407  for (int j = i + 1; j < cov.rows(); j++)
408  cov.get_unsafe(i, j) = cov.get_unsafe(j, i);
409 }
410 
411 /*---------------------------------------------------------------
412  mahalanobisDistanceTo
413  ---------------------------------------------------------------*/
415  const CPose3DQuatPDFGaussian& theOther)
416 {
417  MRPT_START
418  const CMatrixDouble77 COV2 = cov + theOther.cov;
420  CMatrixDouble71(this->mean) - CMatrixDouble71(theOther.mean), COV2);
421  MRPT_END
422 }
423 
424 /*---------------------------------------------------------------
425  operator <<
426  ---------------------------------------------------------------*/
428  ostream& out, const CPose3DQuatPDFGaussian& obj)
429 {
430  out << "Mean: " << obj.mean << "\n";
431  out << "Covariance:\n" << obj.cov << "\n";
432  return out;
433 }
434 
436  const CPose3DQuatPDFGaussian& p1, const CPose3DQuatPDFGaussian& p2)
437 {
438  return p1.mean == p2.mean && p1.cov == p2.cov;
439 }
440 
443 {
445  res += u;
446  return res;
447 }
448 
451 {
453  res -= u;
454  return res;
455 }
A namespace of pseudo-random numbers generators of diferent distributions.
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:59
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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 1x7 vectors, where each row contains a (x,y,z,qr,qx,qy,qz) datum.
Scalar * iterator
Definition: eigen_plugins.h:26
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
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:262
CPose3D mean
The mean value.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:364
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
double mahalanobisDistanceTo(const CPose3DQuatPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
double evaluatePDF(const CPose3DQuat &x) const
Evaluates the PDF at a given point.
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:510
STL namespace.
void operator+=(const CPose3DQuat &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
CMatrixFixedNumeric< double, 7, 1 > CMatrixDouble71
Definition: eigen_frwds.h:67
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
unsigned char uint8_t
Definition: rptypes.h:41
void USE_SUT_EULER2QUAT_CONVERSION(bool value)
If set to true (default), a Scaled Unscented Transform is used instead of a linear approximation with...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double evaluateNormalizedPDF(const CPose3DQuat &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
A numeric matrix of compile-time fixed size.
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:84
void getCovarianceAndMean(mrpt::math::CMatrixDouble77 &cov, CPose3DQuat &mean_point) const override
Returns an estimate of the pose covariance matrix (7x7 cov matrix) and the mean, both at once...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void conj(CQuaternion &q_out) const
Return the conjugate quaternion.
Definition: CQuaternion.h:372
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix...
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:315
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void aux_poseypr2posequat(const CArrayDouble< 6 > &x, const double &dummy, CArrayDouble< 7 > &y)
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
void drawSingleSample(CPose3DQuat &outPart) const override
Draws a single sample from the distribution.
void copyFrom(const CPose3DQuatPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:46
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...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line (x y z qr qx qy qz)...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:164
CPose3DQuatPDFGaussian()
Default constructor - set all values to zero.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
bool USE_SUT_EULER2QUAT_CONVERSION_value
void operator-=(const CPose3DQuatPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
This file implements matrix/vector text and binary serialization.
VECTORLIKE1::Scalar mahalanobisDistance(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the mahalanobis distance of a vector X given the mean MU and the covariance inverse COV_inv ...
Definition: data_utils.h:57
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3538
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
void inverse(CPose3DQuatPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
GLsizei const GLfloat * value
Definition: glext.h:4117
void getMean(CPose3DQuat &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=nullptr)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
GLfloat GLfloat p
Definition: glext.h:6305
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:138
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020