MRPT  1.9.9
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-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 
12 #include <mrpt/random.h>
13 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/poses/CPose3D.h>
21 #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 
28 using namespace mrpt::system;
29 using namespace std;
30 
32 
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41  TConstructorFlags_Poses constructor_dummy_param)
43 {
44  MRPT_UNUSED_PARAM(constructor_dummy_param);
45 }
46 
47 /*---------------------------------------------------------------
48  Constructor
49  ---------------------------------------------------------------*/
51  const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
52  : mean(init_Mean), cov_inv(init_Cov)
53 {
54 }
55 
56 /*---------------------------------------------------------------
57  Constructor
58  ---------------------------------------------------------------*/
60  : mean(init_Mean), cov_inv()
61 {
62 }
63 
64 /*---------------------------------------------------------------
65  CPose3DPDFGaussianInf
66  ---------------------------------------------------------------*/
69 {
70  this->copyFrom(o);
71 }
72 
73 /*---------------------------------------------------------------
74  copyFrom
75  ---------------------------------------------------------------*/
77 {
78  const CPose3DPDFGaussian p(o);
79  this->copyFrom(p);
80 }
81 
85 {
86  out << mean;
88 }
91 {
92  switch (version)
93  {
94  case 0:
95  {
96  in >> mean;
98  }
99  break;
100  default:
102  };
103 }
104 
106 {
107  if (this == &o) return; // It may be used sometimes
108 
110  { // It's my same class:
111  const CPose3DPDFGaussianInf* ptr =
112  static_cast<const CPose3DPDFGaussianInf*>(&o);
113  mean = ptr->mean;
114  cov_inv = ptr->cov_inv;
115  }
116  else
117  {
118  // Convert to gaussian pdf:
121  cov.inv_fast(this->cov_inv);
122  }
123 }
124 
126 {
127  if (IS_CLASS(&o, CPosePDFGaussianInf))
128  { // cov is already inverted, but it's a 2D pose:
129  const CPosePDFGaussianInf* ptr =
130  static_cast<const CPosePDFGaussianInf*>(&o);
131 
132  mean = CPose3D(ptr->mean);
133 
134  // 3x3 inv_cov -> 6x6 inv_cov
135  cov_inv.zeros();
136  cov_inv(0, 0) = ptr->cov_inv(0, 0);
137  cov_inv(1, 1) = ptr->cov_inv(1, 1);
138  cov_inv(3, 3) = ptr->cov_inv(2, 2);
139 
140  cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
141  cov_inv(0, 3) = cov_inv(3, 0) = ptr->cov_inv(0, 2);
142  cov_inv(1, 3) = cov_inv(3, 1) = ptr->cov_inv(1, 2);
143  }
144  else
145  {
147  p.copyFrom(o);
148  this->copyFrom(p);
149  }
150 }
151 
152 /*---------------------------------------------------------------
153 
154  ---------------------------------------------------------------*/
155 bool CPose3DPDFGaussianInf::saveToTextFile(const string& file) const
156 {
157  FILE* f = os::fopen(file.c_str(), "wt");
158  if (!f) return false;
159 
160  os::fprintf(
161  f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
162  mean.pitch(), mean.roll());
163 
164  for (unsigned int i = 0; i < 6; i++)
165  os::fprintf(
166  f, "%e %e %e %e %e %e\n", cov_inv(i, 0), cov_inv(i, 1),
167  cov_inv(i, 2), cov_inv(i, 3), cov_inv(i, 4), cov_inv(i, 5));
168 
169  os::fclose(f);
170  return true;
171 }
172 
173 /*---------------------------------------------------------------
174  changeCoordinatesReference
175  ---------------------------------------------------------------*/
177  const CPose3D& newReferenceBase)
178 {
179  MRPT_START
180 
182  a.copyFrom(*this);
183  a.changeCoordinatesReference(newReferenceBase);
184  this->copyFrom(a);
185 
186  MRPT_END
187 }
188 
189 /*---------------------------------------------------------------
190  drawSingleSample
191  ---------------------------------------------------------------*/
193 {
194  MRPT_UNUSED_PARAM(outPart);
195  MRPT_START
196 
198  this->cov_inv.inv(cov);
199 
202 
203  outPart.setFromValues(
204  mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
205  mean.pitch() + v[4], mean.roll() + v[5]);
206 
208  cov_inv.saveToTextFile(
209  "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
210 }
211 
212 /*---------------------------------------------------------------
213  drawManySamples
214  ---------------------------------------------------------------*/
216  size_t N, vector<CVectorDouble>& outSamples) const
217 {
218  MRPT_START
219 
221  this->cov_inv.inv(cov);
222 
224 
225  for (vector<CVectorDouble>::iterator it = outSamples.begin();
226  it != outSamples.end(); ++it)
227  {
228  (*it)[0] += mean.x();
229  (*it)[1] += mean.y();
230  (*it)[2] += mean.z();
231  (*it)[3] = math::wrapToPi((*it)[3] + mean.yaw());
232  (*it)[4] = math::wrapToPi((*it)[4] + mean.pitch());
233  (*it)[5] = math::wrapToPi((*it)[5] + mean.roll());
234  }
235 
236  MRPT_END
237 }
238 
239 /*---------------------------------------------------------------
240  bayesianFusion
241  ---------------------------------------------------------------*/
243  const CPose3DPDF& p1_, const CPose3DPDF& p2_)
244 {
245  MRPT_UNUSED_PARAM(p1_);
246  MRPT_UNUSED_PARAM(p2_);
247 
248  THROW_EXCEPTION("TO DO!!!");
249 }
250 
251 /*---------------------------------------------------------------
252  inverse
253  ---------------------------------------------------------------*/
255 {
257  CPose3DPDFGaussianInf& out = static_cast<CPose3DPDFGaussianInf&>(o);
258 
259  // This is like: b=(0,0,0)
260  // OUT = b - THIS
261  CPose3DPDFGaussianInf b; // Init: all zeros.
262  out = b - *this;
263 }
264 
265 /*---------------------------------------------------------------
266  +=
267  ---------------------------------------------------------------*/
269 {
270  // COV:
271  const CMatrixDouble66 OLD_COV_INV = this->cov_inv;
273 
275  this->mean, // x
276  Ap, // u
277  df_dx, df_du);
278 
279  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
280  // cov_inv = ... => The same than above!
281  df_dx.multiply_HCHt(OLD_COV_INV, cov_inv);
282  // df_du: Nothing to do, since COV(Ap) = zeros
283 
284  // MEAN:
285  this->mean = this->mean + Ap;
286 }
287 
288 /*---------------------------------------------------------------
289  +=
290  ---------------------------------------------------------------*/
292 {
295  a.copyFrom(*this);
296  b.copyFrom(Ap);
297 
298  a += b;
299 
300  this->mean = a.mean;
301  a.cov.inv(this->cov_inv);
302 }
303 
304 /*---------------------------------------------------------------
305  -=
306  ---------------------------------------------------------------*/
308 {
311  a.copyFrom(*this);
312  b.copyFrom(Ap);
313 
314  a -= b;
315 
316  this->mean = a.mean;
317  a.cov.inv(this->cov_inv);
318 }
319 
320 /*---------------------------------------------------------------
321  evaluatePDF
322  ---------------------------------------------------------------*/
324 {
326  THROW_EXCEPTION("TO DO!!!");
327 }
328 
329 /*---------------------------------------------------------------
330  evaluateNormalizedPDF
331  ---------------------------------------------------------------*/
333 {
335  THROW_EXCEPTION("TO DO!!!");
336 }
337 
338 /*---------------------------------------------------------------
339  assureSymmetry
340  ---------------------------------------------------------------*/
342 {
343  // Differences, when they exist, appear in the ~15'th significant
344  // digit, so... just take one of them arbitrarily!
345  for (int i = 0; i < cov_inv.rows() - 1; i++)
346  for (int j = i + 1; j < cov_inv.rows(); j++)
347  cov_inv.get_unsafe(i, j) = cov_inv.get_unsafe(j, i);
348 }
349 
350 /*---------------------------------------------------------------
351  mahalanobisDistanceTo
352  ---------------------------------------------------------------*/
354  const CPose3DPDFGaussianInf& theOther)
355 {
356  MRPT_START
357 
358  const CMatrixDouble66 cov = this->cov_inv.inv();
359  const CMatrixDouble66 cov2 = theOther.cov_inv.inv();
360 
361  CMatrixDouble66 COV_ = cov + cov2;
363 
364  for (int i = 0; i < 6; i++)
365  {
366  if (COV_.get_unsafe(i, i) == 0)
367  {
368  if (MU.get_unsafe(i, 0) != 0)
369  return std::numeric_limits<double>::infinity();
370  else
371  COV_.get_unsafe(i, i) = 1; // Any arbitrary value since
372  // MU(i)=0, and this value doesn't
373  // affect the result.
374  }
375  }
376 
377  return std::sqrt(MU.multiply_HtCH_scalar(COV_.inv()));
378 
379  MRPT_END
380 }
381 
382 /*---------------------------------------------------------------
383  operator <<
384  ---------------------------------------------------------------*/
385 ostream& mrpt::poses::operator<<(ostream& out, const CPose3DPDFGaussianInf& obj)
386 {
387  out << "Mean: " << obj.mean << "\n";
388  out << "Inverse cov:\n" << obj.cov_inv << "\n";
389 
390  return out;
391 }
392 
393 /*---------------------------------------------------------------
394  getInvCovSubmatrix2D
395  ---------------------------------------------------------------*/
397 {
398  out_cov.setSize(3, 3);
399 
400  for (int i = 0; i < 3; i++)
401  {
402  int a = i == 2 ? 3 : i;
403  for (int j = i; j < 3; j++)
404  {
405  int b = j == 2 ? 3 : j;
406  double f = cov_inv.get_unsafe(a, b);
407  out_cov.set_unsafe(i, j, f);
408  out_cov.set_unsafe(j, i, f);
409  }
410  }
411 }
412 
414  const CPose3DPDFGaussianInf& p1, const CPose3DPDFGaussianInf& p2)
415 {
416  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
417 }
418 
421 {
423  res += u;
424  return res;
425 }
426 
429 {
431  res -= u;
432  return res;
433 }
A namespace of pseudo-random numbers generators 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:140
Scalar * iterator
Definition: eigen_plugins.h:26
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.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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.
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 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
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:141
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:44
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:532
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
STL namespace.
CPose3DPDFGaussianInf()
Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful! ...
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:268
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
Definition: eigen_frwds.h:65
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
#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
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double mahalanobisDistanceTo(const CPose3DPDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
A numeric matrix of compile-time fixed size.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
This base provides a set of functions for maths stuff.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
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
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
GLubyte GLubyte b
Definition: glext.h:6279
void copyFrom(const CPose3DPDF &o) 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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:538
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
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)
const GLdouble * v
Definition: glext.h:3678
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...
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
This file implements matrix/vector text and binary serialization.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
#define MRPT_END
Definition: exceptions.h:266
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:239
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:7274
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
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 serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLuint res
Definition: glext.h:7268
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 ...
GLenum GLint x
Definition: glext.h:3538
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
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: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