Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DPDFGaussianInf.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 
12 #include <mrpt/random.h>
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/utils/CStream.h>
15 #include <mrpt/poses/CPose3D.h>
20 #include <mrpt/system/os.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 
30 
32 
33 
34 /*---------------------------------------------------------------
35  Constructor
36  ---------------------------------------------------------------*/
38 {
39 }
40 
41 /*---------------------------------------------------------------
42  Constructor
43  ---------------------------------------------------------------*/
45 {
46  MRPT_UNUSED_PARAM(constructor_dummy_param);
47 }
48 
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  mean(init_Mean), cov_inv(init_Cov)
54 {
55 }
56 
57 /*---------------------------------------------------------------
58  Constructor
59  ---------------------------------------------------------------*/
61  const CPose3D &init_Mean ) : mean(init_Mean), cov_inv()
62 {
63 }
64 
65 /*---------------------------------------------------------------
66  CPose3DPDFGaussianInf
67  ---------------------------------------------------------------*/
70 {
71  this->copyFrom(o);
72 }
73 
74 /*---------------------------------------------------------------
75  copyFrom
76  ---------------------------------------------------------------*/
78 {
79  const CPose3DPDFGaussian p(o);
80  this->copyFrom(p);
81 }
82 
83 /*---------------------------------------------------------------
84  writeToStream
85  ---------------------------------------------------------------*/
87 {
88  if (version)
89  *version = 0;
90  else
91  {
92  out << mean;
93  for (size_t r=0;r<size(cov_inv,1);r++)
94  out << cov_inv.get_unsafe(r,r);
95  for (size_t r=0;r<size(cov_inv,1);r++)
96  for (size_t c=r+1;c<size(cov_inv,2);c++)
97  out << cov_inv.get_unsafe(r,c);
98  }
99 }
100 
101 /*---------------------------------------------------------------
102  readFromStream
103  ---------------------------------------------------------------*/
105 {
106  switch(version)
107  {
108  case 0:
109  {
110  in >> mean;
111 
112  for (size_t r=0;r<size(cov_inv,1);r++)
113  in >> cov_inv.get_unsafe(r,r);
114  for (size_t r=0;r<size(cov_inv,1);r++)
115  for (size_t c=r+1;c<size(cov_inv,2);c++)
116  {
117  double x;
118  in >> x;
119  cov_inv.get_unsafe(r,c) = cov_inv.get_unsafe(c,r) = x;
120  }
121  } break;
122  default:
124 
125  };
126 }
127 
129 {
130  if (this == &o) return; // It may be used sometimes
131 
133  { // It's my same class:
134  const CPose3DPDFGaussianInf *ptr = static_cast<const CPose3DPDFGaussianInf*>(&o);
135  mean = ptr->mean;
136  cov_inv = ptr->cov_inv;
137  }
138  else
139  {
140  // Convert to gaussian pdf:
143  cov.inv_fast(this->cov_inv);
144  }
145 }
146 
148 {
149  if (IS_CLASS(&o, CPosePDFGaussianInf))
150  { // cov is already inverted, but it's a 2D pose:
151  const CPosePDFGaussianInf *ptr = static_cast<const CPosePDFGaussianInf*>(&o);
152 
153  mean = CPose3D(ptr->mean);
154 
155  // 3x3 inv_cov -> 6x6 inv_cov
156  cov_inv.zeros();
157  cov_inv(0,0) = ptr->cov_inv(0,0);
158  cov_inv(1,1) = ptr->cov_inv(1,1);
159  cov_inv(3,3) = ptr->cov_inv(2,2);
160 
161  cov_inv(0,1) = cov_inv(1,0) = ptr->cov_inv(0,1);
162  cov_inv(0,3) = cov_inv(3,0) = ptr->cov_inv(0,2);
163  cov_inv(1,3) = cov_inv(3,1) = ptr->cov_inv(1,2);
164  }
165  else
166  {
168  p.copyFrom(o);
169  this->copyFrom(p);
170  }
171 }
172 
173 /*---------------------------------------------------------------
174 
175  ---------------------------------------------------------------*/
176 void CPose3DPDFGaussianInf::saveToTextFile(const string &file) const
177 {
178  FILE *f=os::fopen(file.c_str(),"wt");
179  if (!f) return;
180 
181  os::fprintf(f,"%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(), mean.pitch(), mean.roll() );
182 
183  for (unsigned int i=0;i<6;i++)
184  os::fprintf(f,"%e %e %e %e %e %e\n", cov_inv(i,0),cov_inv(i,1),cov_inv(i,2),cov_inv(i,3),cov_inv(i,4),cov_inv(i,5));
185 
186  os::fclose(f);
187 }
188 
189 /*---------------------------------------------------------------
190  changeCoordinatesReference
191  ---------------------------------------------------------------*/
193 {
194  MRPT_START
195 
197  a.copyFrom(*this);
198  a.changeCoordinatesReference(newReferenceBase);
199  this->copyFrom(a);
200 
201  MRPT_END
202 }
203 
204 /*---------------------------------------------------------------
205  drawSingleSample
206  ---------------------------------------------------------------*/
208 {
209  MRPT_UNUSED_PARAM(outPart);
210  MRPT_START
211 
213  this->cov_inv.inv(cov);
214 
217 
218  outPart.setFromValues(
219  mean.x() + v[0],
220  mean.y() + v[1],
221  mean.z() + v[2],
222  mean.yaw() + v[3],
223  mean.pitch() + v[4],
224  mean.roll() + v[5] );
225 
227  cov_inv.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"); \
228  );
229 }
230 
231 /*---------------------------------------------------------------
232  drawManySamples
233  ---------------------------------------------------------------*/
235  size_t N,
236  vector<CVectorDouble> &outSamples ) const
237 {
238  MRPT_START
239 
241  this->cov_inv.inv(cov);
242 
244 
245  for (vector<CVectorDouble>::iterator it=outSamples.begin();it!=outSamples.end();++it)
246  {
247  (*it)[0] += mean.x();
248  (*it)[1] += mean.y();
249  (*it)[2] += mean.z();
250  (*it)[3] = math::wrapToPi( (*it)[3] + mean.yaw() );
251  (*it)[4] = math::wrapToPi( (*it)[4] + mean.pitch() );
252  (*it)[5] = math::wrapToPi( (*it)[5] + mean.roll() );
253  }
254 
255  MRPT_END
256 }
257 
258 
259 /*---------------------------------------------------------------
260  bayesianFusion
261  ---------------------------------------------------------------*/
263 {
265 
266  THROW_EXCEPTION("TO DO!!!");
267 }
268 
269 /*---------------------------------------------------------------
270  inverse
271  ---------------------------------------------------------------*/
273 {
275  CPose3DPDFGaussianInf &out = static_cast<CPose3DPDFGaussianInf&>(o);
276 
277  // This is like: b=(0,0,0)
278  // OUT = b - THIS
279  CPose3DPDFGaussianInf b; // Init: all zeros.
280  out = b - *this;
281 }
282 
283 
284 /*---------------------------------------------------------------
285  +=
286  ---------------------------------------------------------------*/
288 {
289  // COV:
290  const CMatrixDouble66 OLD_COV_INV = this->cov_inv;
292 
294  this->mean, // x
295  Ap, // u
296  df_dx,
297  df_du );
298 
299  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
300  // cov_inv = ... => The same than above!
301  df_dx.multiply_HCHt( OLD_COV_INV, cov_inv );
302  // df_du: Nothing to do, since COV(Ap) = zeros
303 
304  // MEAN:
305  this->mean = this->mean + Ap;
306 }
307 
308 /*---------------------------------------------------------------
309  +=
310  ---------------------------------------------------------------*/
312 {
315  a.copyFrom(*this);
316  b.copyFrom(Ap);
317 
318  a+=b;
319 
320  this->mean = a.mean;
321  a.cov.inv( this->cov_inv );
322 }
323 
324 /*---------------------------------------------------------------
325  -=
326  ---------------------------------------------------------------*/
328 {
331  a.copyFrom(*this);
332  b.copyFrom(Ap);
333 
334  a-=b;
335 
336  this->mean = a.mean;
337  a.cov.inv( this->cov_inv );
338 }
339 
340 /*---------------------------------------------------------------
341  evaluatePDF
342  ---------------------------------------------------------------*/
344 {
346  THROW_EXCEPTION("TO DO!!!");
347 }
348 
349 /*---------------------------------------------------------------
350  evaluateNormalizedPDF
351  ---------------------------------------------------------------*/
353 {
355  THROW_EXCEPTION("TO DO!!!");
356 }
357 
358 /*---------------------------------------------------------------
359  assureSymmetry
360  ---------------------------------------------------------------*/
362 {
363  // Differences, when they exist, appear in the ~15'th significant
364  // digit, so... just take one of them arbitrarily!
365  for (unsigned int i=0;i<size(cov_inv,1)-1;i++)
366  for (unsigned int j=i+1;j<size(cov_inv,1);j++)
367  cov_inv.get_unsafe(i,j) = cov_inv.get_unsafe(j,i);
368 }
369 
370 /*---------------------------------------------------------------
371  mahalanobisDistanceTo
372  ---------------------------------------------------------------*/
374 {
375  MRPT_START
376 
377  const CMatrixDouble66 cov = this->cov_inv.inv();
378  const CMatrixDouble66 cov2 = theOther.cov_inv.inv();
379 
380  CMatrixDouble66 COV_ = cov+cov2;
382 
383  for (int i=0;i<6;i++)
384  {
385  if (COV_.get_unsafe(i,i)==0)
386  {
387  if (MU.get_unsafe(i,0)!=0)
388  return std::numeric_limits<double>::infinity();
389  else COV_.get_unsafe(i,i) = 1; // Any arbitrary value since MU(i)=0, and this value doesn't affect the result.
390  }
391  }
392 
393  return std::sqrt( MU.multiply_HtCH_scalar(COV_.inv()) );
394 
395  MRPT_END
396 }
397 
398 /*---------------------------------------------------------------
399  operator <<
400  ---------------------------------------------------------------*/
402  ostream &out,
403  const CPose3DPDFGaussianInf &obj )
404 {
405  out << "Mean: " << obj.mean << "\n";
406  out << "Inverse cov:\n" << obj.cov_inv << "\n";
407 
408  return out;
409 }
410 
411 /*---------------------------------------------------------------
412  getInvCovSubmatrix2D
413  ---------------------------------------------------------------*/
415 {
416  out_cov.setSize(3,3);
417 
418  for (int i=0;i<3;i++)
419  {
420  int a = i==2 ? 3:i;
421  for (int j=i;j<3;j++)
422  {
423  int b = j==2 ? 3:j;
424  double f = cov_inv.get_unsafe(a,b);
425  out_cov.set_unsafe(i,j, f);
426  out_cov.set_unsafe(j,i, f);
427  }
428  }
429 }
430 
432 {
433  return p1.mean==p2.mean && p1.cov_inv==p2.cov_inv;
434 }
435 
437 {
439  res+=u;
440  return res;
441 }
442 
444 {
446  res-=u;
447  return res;
448 }
A namespace of pseudo-random numbers genrators of diferent distributions.
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3D mean
The mean value.
#define MRPT_END_WITH_CLEAN_UP(stuff)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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.
void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the inverse covariance for the variables (x,y,yaw) only.
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
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 1x6 vectors, where each row contains a (x,y,phi) datum.
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:133
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
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
STL namespace.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
CPose3DPDFGaussianInf()
Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful! ...
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double mahalanobisDistanceTo(const CPose3DPDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#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
const GLubyte * c
Definition: glext.h:5590
#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
GLubyte GLubyte b
Definition: glext.h:5575
int version
Definition: mrpt_jpeglib.h:898
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
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...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void operator-=(const CPose3DPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated)
#define MRPT_START
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in ...
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
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:254
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void drawSingleSample(CPose3D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
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...
GLsizeiptr size
Definition: glext.h:3779
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
GLuint res
Definition: glext.h:6298
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.
GLenum GLint x
Definition: glext.h:3516
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:56
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...
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
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:106
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019