Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
16 #include "mrpt/poses/CPose3D.h" // for CPose3D
17 #include "mrpt/poses/CPose3DQuat.h" // for CPose3D...
18 #include "mrpt/poses/CPose3DQuatPDF.h" // for CPose3D...
19 #include <mrpt/system/os.h>
20 #include <mrpt/utils/CStream.h>
21 
22 using namespace mrpt;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace mrpt::utils;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
32 
34 
35 /** Default constructor - set all values to zero. */
37  mean(), cov()
38 {
39 }
40 
41 // Un-initialized constructor:
44 {
45  MRPT_UNUSED_PARAM(constructor_dummy_param);
46 }
47 
48 /** Constructor from a default mean value, covariance equals to zero. */
50  mean(init_Mean), cov()
51 {
52 }
53 
54 /** Constructor with mean and covariance. */
56  mean(init_Mean), cov(init_Cov)
57 {
58 }
59 
60 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables). */
63 {
64  this->copyFrom(CPose3DPDFGaussian(o));
65 }
66 
67 /** Constructor from an equivalent Gaussian in Euler angles representation. */
70 {
71  this->copyFrom(o);
72 }
73 
74 /*---------------------------------------------------------------
75  getMean
76  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
77  ---------------------------------------------------------------*/
79 {
80  p=mean;
81 }
82 
83 /*---------------------------------------------------------------
84  getCovarianceAndMean
85  ---------------------------------------------------------------*/
87 {
88  C=cov;
89  p=mean;
90 }
91 
92 /*---------------------------------------------------------------
93  writeToStream
94  ---------------------------------------------------------------*/
96 {
97  if (version)
98  *version = 0;
99  else
100  {
101  out << mean;
102 
103  for (size_t r=0;r<size(cov,1);r++)
104  out << cov.get_unsafe(r,r);
105  for (size_t r=0;r<size(cov,1);r++)
106  for (size_t c=r+1;c<size(cov,2);c++)
107  out << cov.get_unsafe(r,c);
108  }
109 }
110 
111 /*---------------------------------------------------------------
112  readFromStream
113  ---------------------------------------------------------------*/
115 {
116  switch(version)
117  {
118  case 0:
119  {
120  in >> mean;
121 
122  for (size_t r=0;r<size(cov,1);r++)
123  in >> cov.get_unsafe(r,r);
124  for (size_t r=0;r<size(cov,1);r++)
125  for (size_t c=r+1;c<size(cov,2);c++)
126  {
127  double x;
128  in >> x;
129  cov.get_unsafe(r,c) = cov.get_unsafe(c,r) = x;
130  }
131  } break;
132  default:
134 
135  };
136 }
137 
139 {
140  if (this == &o) return; // It may be used sometimes
141 
142  // Convert to gaussian pdf:
144 }
145 
147 {
148  CPose3DPDFGaussian aux;
149  aux.copyFrom(o);
150  this->copyFrom(aux);
151 }
152 
153 
154 void aux_poseypr2posequat(const CArrayDouble<6> &x,const double&dummy, CArrayDouble<7> &y)
155 {
156  MRPT_UNUSED_PARAM(dummy);
157  y[0]=x[0];
158  y[1]=x[1];
159  y[2]=x[2];
160 
161  CPose3D p(0,0,0, x[3],x[4],x[5]);
163  p.getAsQuaternion(q);
164  y[3] = q[0];
165  y[4] = q[1];
166  y[5] = q[2];
167  y[6] = q[3];
168 }
169 
171 {
173  { // Use Jacobians
175 
176  // Mean:
177  mean.x(o.mean.x());
178  mean.y(o.mean.y());
179  mean.z(o.mean.z());
180 
181  o.mean.getAsQuaternion(mean.quat(), &dq_dr_sub);
182 
183  // Cov:
184  #if 1
186  dq_dr.get_unsafe(0,0)=dq_dr.get_unsafe(1,1)=dq_dr.get_unsafe(2,2)=1;
187  dq_dr.insertMatrix(3,3,dq_dr_sub);
188  // Now for the covariance:
189  dq_dr.multiply_HCHt( o.cov, this->cov);
190  #else
194  o.cov.extractMatrix(3,3,cov_R);
195  o.cov.extractMatrix(0,0,cov_T);
196  o.cov.extractMatrix(0,3,cov_TR);
197 
198  // [ S_T | S_TR * H^t ]
199  // [ -----------------+---------------- ]
200  // [ (S_TR * H^t)^t | H * S_R * H^t ]
201 
202  // top-left:
203  this->cov.insertMatrix(0,0,cov_T);
204 
205  // diagonals:
207  cov_TQ.multiply_ABt(cov_TR,dq_dr_sub);
208  this->cov.insertMatrix (0,3,cov_TQ);
209  this->cov.insertMatrixTranspose(3,0,cov_TQ);
210 
211  // bottom-right:
213  dq_dr_sub.multiply_HCHt(cov_R,cov_q);
214  this->cov.insertMatrix(3,3,cov_q);
215  #endif
216  }
217  else
218  {
219  // Use UT transformation:
220  // f: R^6 => R^7
221  const CArrayDouble<6> x_mean(o.mean);
222 
223  static const double dummy=0;
225  x_mean, o.cov,
227  dummy,
228  this->mean,
229  this->cov
230  );
231  }
232 }
233 
234 /*---------------------------------------------------------------
235  saveToTextFile
236  ---------------------------------------------------------------*/
237 void CPose3DQuatPDFGaussian::saveToTextFile(const string &file) const
238 {
239  FILE *f=os::fopen(file.c_str(),"wt");
240  if (!f) return;
241 
242  os::fprintf(f,"%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
243 
244  for (unsigned int i=0;i<7;i++)
245  os::fprintf(f,"%e %e %e %e %e %e %e\n", cov(i,0),cov(i,1),cov(i,2),cov(i,3),cov(i,4),cov(i,5),cov(i,6));
246 
247  os::fclose(f);
248 }
249 
250 /*---------------------------------------------------------------
251  changeCoordinatesReference
252  ---------------------------------------------------------------*/
254 {
255  MRPT_START
256  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
257  MRPT_END
258 }
259 
260 /*---------------------------------------------------------------
261  changeCoordinatesReference
262  ---------------------------------------------------------------*/
264 {
265  MRPT_START
266 
267  // COV:
268  const CMatrixDouble77 OLD_COV = this->cov;
270 
272  newReferenceBaseQuat, // x
273  this->mean, // u
274  df_dx,
275  df_du,
276  &this->mean // Output: newReferenceBaseQuat + this->mean;
277  );
278 
279  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
280  //df_dx: not used, since its COV are all zeros... // df_dx.multiply_HCHt( OLD_COV, cov );
281  df_du.multiply_HCHt( OLD_COV, cov);
282 
283  MRPT_END
284 }
285 
286 /*---------------------------------------------------------------
287  drawSingleSample
288  ---------------------------------------------------------------*/
290 {
291  MRPT_START
293  MRPT_END
294 }
295 
296 /*---------------------------------------------------------------
297  drawManySamples
298  ---------------------------------------------------------------*/
300  size_t N,
301  vector<CVectorDouble> &outSamples ) const
302 {
303  MRPT_START
304 
306 
307  for (vector<CVectorDouble>::iterator it=outSamples.begin();it!=outSamples.end();++it)
308  for (unsigned int k=0;k<7;k++)
309  (*it)[k] += mean[k];
310 
311  MRPT_END
312 }
313 
314 /*---------------------------------------------------------------
315  inverse
316  ---------------------------------------------------------------*/
318 {
320  CPose3DQuatPDFGaussian &out = static_cast<CPose3DQuatPDFGaussian&>(o);
321 
322  // COV:
324  double lx,ly,lz;
325  mean.inverseComposePoint(0,0,0,lx,ly,lz, NULL, &df_dpose);
326 
327 
329  jacob.insertMatrix(0,0, df_dpose );
330  jacob.set_unsafe(3,3, 1);
331  jacob.set_unsafe(4,4, -1);
332  jacob.set_unsafe(5,5, -1);
333  jacob.set_unsafe(6,6, -1);
334 
335  // C(0:2,0:2): H C H^t
336  jacob.multiply_HCHt( this->cov, out.cov );
337 
338  // Mean:
339  out.mean.x(lx);
340  out.mean.y(ly);
341  out.mean.z(lz);
342  this->mean.quat().conj( out.mean.quat() );
343 }
344 
345 
346 /*---------------------------------------------------------------
347  +=
348  ---------------------------------------------------------------*/
350 {
351  // COV:
352  const CMatrixDouble77 OLD_COV = this->cov;
354 
356  this->mean, // x
357  Ap, // u
358  df_dx,
359  df_du,
360  &this->mean // Output: this->mean + Ap;
361  );
362 
363  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
364  df_dx.multiply_HCHt( OLD_COV, cov );
365  // df_du: Nothing to do, since COV(Ap) = zeros
366 
367 }
368 
369 /*---------------------------------------------------------------
370  +=
371  ---------------------------------------------------------------*/
373 {
374  // COV:
375  const CMatrixDouble77 OLD_COV = this->cov;
377 
379  this->mean, // x
380  Ap.mean, // u
381  df_dx,
382  df_du,
383  &this->mean // Output: this->mean + Ap.mean;
384  );
385 
386  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
387  df_dx.multiply_HCHt( OLD_COV, cov );
388  df_du.multiply_HCHt( Ap.cov, cov, true); // Accumulate result
389 }
390 
391 /*---------------------------------------------------------------
392  -=
393  ---------------------------------------------------------------*/
395 {
396  // THIS = THIS (-) Ap -->
397  // THIS = inverse(Ap) (+) THIS
398  CPose3DQuatPDFGaussian inv_Ap = -Ap;
399  *this = inv_Ap + *this;
400 }
401 
402 /*---------------------------------------------------------------
403  evaluatePDF
404  ---------------------------------------------------------------*/
406 {
408 }
409 
410 /*---------------------------------------------------------------
411  evaluateNormalizedPDF
412  ---------------------------------------------------------------*/
414 {
415  return mrpt::math::normalPDF(CMatrixDouble71(x),CMatrixDouble71(this->mean),this->cov, true);
416 }
417 
418 /*---------------------------------------------------------------
419  assureSymmetry
420  ---------------------------------------------------------------*/
422 {
423  // Differences, when they exist, appear in the ~15'th significant
424  // digit, so... just take one of them arbitrarily!
425  for (unsigned int i=0;i<size(cov,1)-1;i++)
426  for (unsigned int j=i+1;j<size(cov,1);j++)
427  cov.get_unsafe(i,j) = cov.get_unsafe(j,i);
428 }
429 
430 /*---------------------------------------------------------------
431  mahalanobisDistanceTo
432  ---------------------------------------------------------------*/
434 {
435  MRPT_START
436  const CMatrixDouble77 COV2 = cov + theOther.cov;
437  return mrpt::math::mahalanobisDistance( CMatrixDouble71(this->mean) - CMatrixDouble71(theOther.mean), COV2);
438  MRPT_END
439 }
440 
441 /*---------------------------------------------------------------
442  operator <<
443  ---------------------------------------------------------------*/
445  ostream &out,
446  const CPose3DQuatPDFGaussian &obj )
447 {
448  out << "Mean: " << obj.mean << "\n";
449  out << "Covariance:\n" << obj.cov << "\n";
450  return out;
451 }
452 
453 
455 {
456  return p1.mean==p2.mean && p1.cov==p2.cov;
457 }
458 
460 {
462  res+=u;
463  return res;
464 }
465 
467 {
469  res-=u;
470  return res;
471 }
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:52
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
CPose3D mean
The mean value.
void copyFrom(const CPose3DQuatPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getCovarianceAndMean(mrpt::math::CMatrixDouble77 &cov, CPose3DQuat &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (7x7 cov matrix) and the mean, both at once...
BASE_IMPEXP bool USE_SUT_EULER2QUAT_CONVERSION
If set to true (default), a Scaled Unscented Transform is used instead of a linear approximation with...
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPoint2D BASE_IMPEXP 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:359
const GLfloat * c
Definition: glew.h:10088
double evaluatePDF(const CPose3DQuat &x) const
Evaluates the PDF at a given point.
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Scalar * iterator
Definition: eigen_plugins.h:23
double mahalanobisDistanceTo(const CPose3DQuatPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
void conj(CQuaternion &q_out) const
Return the conjugate quaternion.
Definition: CQuaternion.h:297
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save the PDF to a text file, containing the 3D pose in the first line (x y z qr qx qy qz)...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
void operator+=(const CPose3DQuat &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void getMean(CPose3DQuat &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=NULL)
Generate a given number of multidimensional random samples according to a given covariance matrix...
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
GLuint in
Definition: glew.h:7146
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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:135
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CPose2D BASE_IMPEXP 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:307
void aux_poseypr2posequat(const CArrayDouble< 6 > &x, const double &dummy, CArrayDouble< 7 > &y)
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
GLhandleARB obj
Definition: glew.h:3276
void inverse(CPose3DQuatPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
double evaluateNormalizedPDF(const CPose3DQuat &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
int version
Definition: mrpt_jpeglib.h:898
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
GLfloat GLfloat p
Definition: glew.h:10113
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
GLuint res
Definition: glew.h:7143
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
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:72
void operator-=(const CPose3DQuatPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
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=NULL, 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...
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:58
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=NULL)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:553
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...
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_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.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
CMatrixFixedNumeric< double, 7, 1 > CMatrixDouble71
Definition: eigen_frwds.h:58
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=NULL)
Generate multidimensional random samples according to a given covariance matrix.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
double BASE_IMPEXP normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:908
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point L such as .
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
void drawSingleSample(CPose3DQuat &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
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:106
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018