Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DQuatPDFGaussianInf.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 
13 #include <mrpt/utils/types_math.h> // for CMatrixF...
14 #include <stdio.h> // for size_t
15 #include <algorithm> // for move, max
16 #include <exception> // for exception
17 #include <new> // for operator...
18 #include <ostream> // for operator<<
19 #include <string> // for allocator
20 #include <vector> // for vector
21 #include <mrpt/math/CMatrixFixedNumeric.h> // for CMatrixF...
22 #include <mrpt/math/CQuaternion.h> // for CQuatern...
23 #include <mrpt/poses/CPose3D.h> // for CPose3D
24 #include <mrpt/poses/CPose3DQuat.h> // for CPose3DQuat
25 #include <mrpt/poses/CPose3DQuatPDF.h> // for CPose3DQ...
26 #include <mrpt/random/RandomGenerators.h> // for CRandomG...
27 #include <mrpt/utils/CObject.h> // for CPose3DQ...
28 #include <mrpt/utils/CSerializable.h> // for CSeriali...
29 #include <mrpt/utils/bits.h> // for size
30 #include <mrpt/utils/mrpt_macros.h> // for MRPT_END
31 #include <mrpt/system/os.h> // for fopen
33 
34 using namespace mrpt;
35 using namespace mrpt::system;
36 using namespace mrpt::poses;
37 using namespace mrpt::math;
38 using namespace mrpt::random;
39 using namespace mrpt::utils;
40 using namespace std;
41 
43 
44 /** Default constructor - set all values to zero. */
46  mean(), cov_inv()
47 {
48 }
49 
50 // Un-initialized constructor:
51 CPose3DQuatPDFGaussianInf::CPose3DQuatPDFGaussianInf(TConstructorFlags_Quaternions constructor_dummy_param) :
53 {
54  MRPT_UNUSED_PARAM(constructor_dummy_param);
55 }
56 
57 /** Constructor from a default mean value, covariance equals to zero. */
59  mean(init_Mean), cov_inv()
60 {
61 }
62 
63 /** Constructor with mean and covariance. */
65  mean(init_Mean), cov_inv(init_CovInv)
66 {
67 }
68 
69 /*---------------------------------------------------------------
70  writeToStream
71  ---------------------------------------------------------------*/
73 {
74  if (version)
75  *version = 0;
76  else
77  {
78  out << mean;
79 
80  for (size_t r=0;r<size(cov_inv,1);r++)
81  out << cov_inv.get_unsafe(r,r);
82  for (size_t r=0;r<size(cov_inv,1);r++)
83  for (size_t c=r+1;c<size(cov_inv,2);c++)
84  out << cov_inv.get_unsafe(r,c);
85  }
86 }
87 
88 /*---------------------------------------------------------------
89  readFromStream
90  ---------------------------------------------------------------*/
92 {
93  switch(version)
94  {
95  case 0:
96  {
97  in >> mean;
98 
99  for (size_t r=0;r<size(cov_inv,1);r++)
100  in >> cov_inv.get_unsafe(r,r);
101  for (size_t r=0;r<size(cov_inv,1);r++)
102  for (size_t c=r+1;c<size(cov_inv,2);c++)
103  {
104  double x;
105  in >> x;
106  cov_inv.get_unsafe(r,c) = cov_inv.get_unsafe(c,r) = x;
107  }
108  } break;
109  default:
111 
112  };
113 }
114 
116 {
117  if (this == &o) return; // It may be used sometimes
118 
119  // Convert to gaussian pdf:
121  o.getCovarianceAndMean(C,this->mean);
122  C.inv_fast(this->cov_inv);
123 }
124 
125 /*---------------------------------------------------------------
126  saveToTextFile
127  ---------------------------------------------------------------*/
128 void CPose3DQuatPDFGaussianInf::saveToTextFile(const string &file) const
129 {
130  FILE *f=os::fopen(file.c_str(),"wt");
131  if (!f) return;
132 
133  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]);
134 
135  for (unsigned int i=0;i<7;i++)
136  os::fprintf(f,"%e %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),cov_inv(i,6));
137 
138  os::fclose(f);
139 }
140 
141 /*---------------------------------------------------------------
142  changeCoordinatesReference
143  ---------------------------------------------------------------*/
145 {
146  MRPT_START
147  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
148  MRPT_END
149 }
150 
151 /*---------------------------------------------------------------
152  changeCoordinatesReference
153  ---------------------------------------------------------------*/
155 {
156  MRPT_START
157 
158  // COV:
160  this->cov_inv.inv(OLD_COV);
161 
163 
165  newReferenceBaseQuat, // x
166  this->mean, // u
167  df_dx,
168  df_du,
169  &this->mean // Output: newReferenceBaseQuat + this->mean;
170  );
171 
172  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
173  //df_dx: not used, since its COV are all zeros... // df_dx.multiply_HCHt( OLD_COV, cov );
175  df_du.multiply_HCHt( OLD_COV, NEW_COV);
176  NEW_COV.inv_fast(this->cov_inv);
177 
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  drawSingleSample
183  ---------------------------------------------------------------*/
185 {
186  MRPT_START
188  this->cov_inv.inv(COV);
189 
191  MRPT_END
192 }
193 
194 /*---------------------------------------------------------------
195  drawManySamples
196  ---------------------------------------------------------------*/
198  size_t N,
199  vector<CVectorDouble> &outSamples ) const
200 {
201  MRPT_START
203  this->cov_inv.inv(COV);
204 
206 
207  for (vector<CVectorDouble>::iterator it=outSamples.begin();it!=outSamples.end();++it)
208  for (unsigned int k=0;k<7;k++)
209  (*it)[k] += mean[k];
210 
211  MRPT_END
212 }
213 
214 /*---------------------------------------------------------------
215  inverse
216  ---------------------------------------------------------------*/
218 {
221 
222  // COV:
224  double lx,ly,lz;
225  mean.inverseComposePoint(0,0,0,lx,ly,lz, NULL, &df_dpose);
226 
227 
229  jacob.insertMatrix(0,0, df_dpose );
230  jacob.set_unsafe(3,3, 1);
231  jacob.set_unsafe(4,4, -1);
232  jacob.set_unsafe(5,5, -1);
233  jacob.set_unsafe(6,6, -1);
234 
235  // C(0:2,0:2): H C H^t
237  this->cov_inv.inv(COV);
238 
239  jacob.multiply_HCHt( COV, NEW_COV );
240  NEW_COV.inv_fast(out.cov_inv);
241 
242  // Mean:
243  out.mean.x(lx);
244  out.mean.y(ly);
245  out.mean.z(lz);
246  this->mean.quat().conj( out.mean.quat() );
247 }
248 
249 
250 /*---------------------------------------------------------------
251  +=
252  ---------------------------------------------------------------*/
254 {
255  // COV:
257  this->cov_inv.inv(OLD_COV);
258 
260 
262  this->mean, // x
263  Ap, // u
264  df_dx,
265  df_du,
266  &this->mean // Output: this->mean + Ap;
267  );
268 
269  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
271  df_dx.multiply_HCHt( OLD_COV, NEW_COV);
272  NEW_COV.inv_fast(this->cov_inv);
273  // df_du: Nothing to do, since COV(Ap) = zeros
274 }
275 
276 
277 /*---------------------------------------------------------------
278  +=
279  ---------------------------------------------------------------*/
281 {
282  // COV:
284  this->cov_inv.inv(OLD_COV);
285 
287 
289  this->mean, // x
290  Ap.mean, // u
291  df_dx,
292  df_du,
293  &this->mean // Output: this->mean + Ap.mean;
294  );
295 
296  // this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
299  Ap.cov_inv.inv(Ap_cov);
300 
301  df_dx.multiply_HCHt( OLD_COV, NEW_COV);
302  df_du.multiply_HCHt( Ap_cov, NEW_COV, true); // Accumulate result
303 
304  NEW_COV.inv_fast(this->cov_inv);
305 }
306 
307 /*---------------------------------------------------------------
308  -=
309  ---------------------------------------------------------------*/
311 {
312  // THIS = THIS (-) Ap -->
313  // THIS = inverse(Ap) (+) THIS
314  CPose3DQuatPDFGaussianInf inv_Ap = -Ap;
315  *this = inv_Ap + *this;
316 }
317 
318 /*---------------------------------------------------------------
319  evaluatePDF
320  ---------------------------------------------------------------*/
322 {
324 }
325 
326 /*---------------------------------------------------------------
327  evaluateNormalizedPDF
328  ---------------------------------------------------------------*/
330 {
332 }
333 
334 /*---------------------------------------------------------------
335  operator <<
336  ---------------------------------------------------------------*/
338  ostream &out,
340 {
341  out << "Mean: " << obj.mean << "\n";
342  out << "Information:\n" << obj.cov_inv << "\n";
343  return out;
344 }
345 
347 {
348  return p1.mean==p2.mean && p1.cov_inv==p2.cov_inv;
349 }
350 
351 /** Pose composition for two 3D pose Gaussians \sa CPose3DQuatPDFGaussianInf::operator += */
353 {
355  res+=u;
356  return res;
357 }
358 
359 /** Inverse pose composition for two 3D pose Gaussians \sa CPose3DQuatPDFGaussianInf::operator -= */
361 {
363  res-=u;
364  return res;
365 }
A namespace of pseudo-random numbers genrators of diferent distributions.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
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
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
void drawSingleSample(CPose3DQuat &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
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
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Scalar * iterator
Definition: eigen_plugins.h:23
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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...
double evaluateNormalizedPDF(const CPose3DQuat &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
void operator+=(const CPose3DQuat &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
CPose3DQuatPDFGaussianInf()
Default constructor - set all values to zero.
mrpt::math::CMatrixDouble77 cov_inv
The 7x7 information matrix (the inverse of the covariance)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
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.
void conj(CQuaternion &q_out) const
Return the conjugate quaternion.
Definition: CQuaternion.h:297
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
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 .
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
double evaluatePDF(const CPose3DQuat &x) const
Evaluates the PDF at a given point.
MATRIXLIKE::Scalar normalPDFInf(const VECTORLIKE1 &x, const VECTORLIKE2 &mu, const MATRIXLIKE &cov_inv, const bool scaled_pdf=false)
Evaluates the multivariate normal (Gaussian) distribution at a given point "x".
Definition: distributions.h:42
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
void operator-=(const CPose3DQuatPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
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 inverse(CPose3DQuatPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
GLuint in
Definition: glext.h:6301
#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$.
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 copyFrom(const CPose3DQuatPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
GLsizeiptr size
Definition: glext.h:3779
GLuint res
Definition: glext.h:6298
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.
GLenum GLint x
Definition: glext.h:3516
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)...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
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.
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



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