MRPT  1.9.9
CPose3DPDFGaussianInf.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 
13 #include <mrpt/math/ops_matrices.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPose3D.h>
20 #include <mrpt/random.h>
22 #include <mrpt/system/os.h>
23 #include <Eigen/Dense>
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 
35 /*---------------------------------------------------------------
36  Constructor
37  ---------------------------------------------------------------*/
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  TConstructorFlags_Poses constructor_dummy_param)
45 {
46  MRPT_UNUSED_PARAM(constructor_dummy_param);
47 }
48 
49 /*---------------------------------------------------------------
50  Constructor
51  ---------------------------------------------------------------*/
53  const CPose3D& init_Mean, const CMatrixDouble66& init_Cov)
54  : mean(init_Mean), cov_inv(init_Cov)
55 {
56 }
57 
58 /*---------------------------------------------------------------
59  Constructor
60  ---------------------------------------------------------------*/
62  : mean(init_Mean), cov_inv()
63 {
64 }
65 
66 /*---------------------------------------------------------------
67  CPose3DPDFGaussianInf
68  ---------------------------------------------------------------*/
71 {
72  this->copyFrom(o);
73 }
74 
75 /*---------------------------------------------------------------
76  copyFrom
77  ---------------------------------------------------------------*/
79 {
80  const CPose3DPDFGaussian p(o);
81  this->copyFrom(p);
82 }
83 
84 uint8_t CPose3DPDFGaussianInf::serializeGetVersion() const { return 0; }
87 {
88  out << mean;
90 }
92  mrpt::serialization::CArchive& in, uint8_t version)
93 {
94  switch (version)
95  {
96  case 0:
97  {
98  in >> mean;
100  }
101  break;
102  default:
104  };
105 }
106 
108 {
109  if (this == &o) return; // It may be used sometimes
110 
112  { // It's my same class:
113  const auto* ptr = dynamic_cast<const CPose3DPDFGaussianInf*>(&o);
114  ASSERT_(ptr != nullptr);
115  mean = ptr->mean;
116  cov_inv = ptr->cov_inv;
117  }
118  else
119  {
120  // Convert to gaussian pdf:
123  this->cov_inv = cov.inverse_LLt();
124  }
125 }
126 
128 {
130  { // cov is already inverted, but it's a 2D pose:
131  const auto* ptr = dynamic_cast<const CPosePDFGaussianInf*>(&o);
132  ASSERT_(ptr != nullptr);
133 
134  mean = CPose3D(ptr->mean);
135 
136  // 3x3 inv_cov -> 6x6 inv_cov
137  cov_inv.setZero();
138  cov_inv(0, 0) = ptr->cov_inv(0, 0);
139  cov_inv(1, 1) = ptr->cov_inv(1, 1);
140  cov_inv(3, 3) = ptr->cov_inv(2, 2);
141 
142  cov_inv(0, 1) = cov_inv(1, 0) = ptr->cov_inv(0, 1);
143  cov_inv(0, 3) = cov_inv(3, 0) = ptr->cov_inv(0, 2);
144  cov_inv(1, 3) = cov_inv(3, 1) = ptr->cov_inv(1, 2);
145  }
146  else
147  {
149  p.copyFrom(o);
150  this->copyFrom(p);
151  }
152 }
153 
154 /*---------------------------------------------------------------
155 
156  ---------------------------------------------------------------*/
157 bool CPose3DPDFGaussianInf::saveToTextFile(const string& file) const
158 {
159  FILE* f = os::fopen(file.c_str(), "wt");
160  if (!f) return false;
161 
162  os::fprintf(
163  f, "%e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(), mean.yaw(),
164  mean.pitch(), mean.roll());
165 
166  for (unsigned int i = 0; i < 6; i++)
167  os::fprintf(
168  f, "%e %e %e %e %e %e\n", cov_inv(i, 0), cov_inv(i, 1),
169  cov_inv(i, 2), cov_inv(i, 3), cov_inv(i, 4), cov_inv(i, 5));
170 
171  os::fclose(f);
172  return true;
173 }
174 
175 /*---------------------------------------------------------------
176  changeCoordinatesReference
177  ---------------------------------------------------------------*/
179  const CPose3D& newReferenceBase)
180 {
181  MRPT_START
182 
184  a.copyFrom(*this);
185  a.changeCoordinatesReference(newReferenceBase);
186  this->copyFrom(a);
187 
188  MRPT_END
189 }
190 
191 /*---------------------------------------------------------------
192  drawSingleSample
193  ---------------------------------------------------------------*/
195 {
196  MRPT_UNUSED_PARAM(outPart);
197  MRPT_START
198 
200 
203 
204  outPart.setFromValues(
205  mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
206  mean.pitch() + v[4], mean.roll() + v[5]);
207 
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 
223 
224  for (auto& outSample : outSamples)
225  {
226  outSample[0] += mean.x();
227  outSample[1] += mean.y();
228  outSample[2] += mean.z();
229  outSample[3] = math::wrapToPi(outSample[3] + mean.yaw());
230  outSample[4] = math::wrapToPi(outSample[4] + mean.pitch());
231  outSample[5] = math::wrapToPi(outSample[5] + mean.roll());
232  }
233 
234  MRPT_END
235 }
236 
237 /*---------------------------------------------------------------
238  bayesianFusion
239  ---------------------------------------------------------------*/
241  const CPose3DPDF& p1_, const CPose3DPDF& p2_)
242 {
243  MRPT_UNUSED_PARAM(p1_);
244  MRPT_UNUSED_PARAM(p2_);
245 
246  THROW_EXCEPTION("TO DO!!!");
247 }
248 
249 /*---------------------------------------------------------------
250  inverse
251  ---------------------------------------------------------------*/
253 {
255  auto& out = dynamic_cast<CPose3DPDFGaussianInf&>(o);
256 
257  // This is like: b=(0,0,0)
258  // OUT = b - THIS
259  CPose3DPDFGaussianInf b; // Init: all zeros.
260  out = b - *this;
261 }
262 
263 /*---------------------------------------------------------------
264  +=
265  ---------------------------------------------------------------*/
267 {
268  // COV:
269  const CMatrixDouble66 OLD_COV_INV = this->cov_inv;
271 
273  this->mean, // x
274  Ap, // u
275  df_dx, df_du);
276 
277  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
278  // cov_inv = ... => The same than above!
279  cov_inv = mrpt::math::multiply_HCHt(df_dx, OLD_COV_INV);
280  // df_du: Nothing to do, since COV(Ap) = zeros
281 
282  // MEAN:
283  this->mean = this->mean + Ap;
284 }
285 
286 /*---------------------------------------------------------------
287  +=
288  ---------------------------------------------------------------*/
290 {
293  a.copyFrom(*this);
294  b.copyFrom(Ap);
295 
296  a += b;
297 
298  this->mean = a.mean;
299  cov_inv = a.cov.inverse_LLt();
300 }
301 
302 /*---------------------------------------------------------------
303  -=
304  ---------------------------------------------------------------*/
306 {
309  a.copyFrom(*this);
310  b.copyFrom(Ap);
311 
312  a -= b;
313 
314  this->mean = a.mean;
315  cov_inv = a.cov.inverse_LLt();
316 }
317 
318 /*---------------------------------------------------------------
319  evaluatePDF
320  ---------------------------------------------------------------*/
322 {
324  THROW_EXCEPTION("TO DO!!!");
325 }
326 
327 /*---------------------------------------------------------------
328  evaluateNormalizedPDF
329  ---------------------------------------------------------------*/
331 {
333  THROW_EXCEPTION("TO DO!!!");
334 }
335 
336 /*---------------------------------------------------------------
337  enforceCovSymmetry
338  ---------------------------------------------------------------*/
340 {
341  // Differences, when they exist, appear in the ~15'th significant
342  // digit, so... just take one of them arbitrarily!
343  for (int i = 0; i < cov_inv.rows() - 1; i++)
344  for (int j = i + 1; j < cov_inv.rows(); j++)
345  cov_inv(i, j) = cov_inv(j, i);
346 }
347 
348 /*---------------------------------------------------------------
349  mahalanobisDistanceTo
350  ---------------------------------------------------------------*/
352  const CPose3DPDFGaussianInf& theOther)
353 {
354  MRPT_START
355 
356  const CMatrixDouble66 cov = this->cov_inv.inverse_LLt();
357  const CMatrixDouble66 cov2 = theOther.cov_inv.inverse_LLt();
358 
359  CMatrixDouble66 COV_ = cov + cov2;
361 
362  for (int i = 0; i < 6; i++)
363  {
364  if (COV_(i, i) == 0)
365  {
366  if (MU(i, 0) != 0)
367  return std::numeric_limits<double>::infinity();
368  else
369  COV_(i, i) = 1; // Any arbitrary value since
370  // MU(i)=0, and this value doesn't
371  // affect the result.
372  }
373  }
374 
375  return std::sqrt(mrpt::math::multiply_HtCH_scalar(MU, COV_.inverse_LLt()));
376 
377  MRPT_END
378 }
379 
380 /*---------------------------------------------------------------
381  operator <<
382  ---------------------------------------------------------------*/
384 {
385  out << "Mean: " << obj.mean << "\n";
386  out << "Inverse cov:\n" << obj.cov_inv << "\n";
387 
388  return out;
389 }
390 
391 /*---------------------------------------------------------------
392  getInvCovSubmatrix2D
393  ---------------------------------------------------------------*/
395 {
396  out_cov.setSize(3, 3);
397 
398  for (int i = 0; i < 3; i++)
399  {
400  int a = i == 2 ? 3 : i;
401  for (int j = i; j < 3; j++)
402  {
403  int b = j == 2 ? 3 : j;
404  double f = cov_inv(a, b);
405  out_cov(i, j) = f;
406  out_cov(j, i) = f;
407  }
408  }
409 }
410 
412  const CPose3DPDFGaussianInf& p1, const CPose3DPDFGaussianInf& p2)
413 {
414  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
415 }
416 
419 {
421  res += u;
422  return res;
423 }
424 
427 {
429  res -= u;
430  return res;
431 }
A namespace of pseudo-random numbers generators of diferent distributions.
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
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.
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:67
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.
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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:394
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
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 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
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
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:247
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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
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.
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:102
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:356
CMatrixFixed< double, 6, 1 > CMatrixDouble61
Definition: CMatrixFixed.h:362
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:6372
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
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
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:3684
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...
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 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 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
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
GLuint in
Definition: glext.h:7391
double mean(const CONTAINER &v)
Computes the mean value of a vector.
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:257
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:7385
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:3542
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
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
#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: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019