MRPT  1.9.9
CPose3DQuatPDFGaussianInf.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 
12 #include <mrpt/math/CMatrixFixed.h> // for CMatrixF...
13 #include <mrpt/math/CQuaternion.h> // for CQuatern...
15 #include <mrpt/poses/CPose3D.h> // for CPose3D
16 #include <mrpt/poses/CPose3DQuat.h> // for CPose3DQuat
17 #include <mrpt/poses/CPose3DQuatPDF.h> // for CPose3DQ...
19 #include <mrpt/random/RandomGenerators.h> // for CRandomG...
20 #include <mrpt/serialization/CSerializable.h> // for CSeriali...
22 #include <mrpt/system/os.h> // for fopen
23 #include <Eigen/Dense>
24 #include <algorithm> // for move, max
25 #include <cstdio> // for size_t
26 #include <exception> // for exception
27 #include <new> // for operator...
28 #include <ostream> // for operator<<
29 #include <string> // for allocator
30 #include <vector> // for vector
31 
32 using namespace mrpt;
33 using namespace mrpt::system;
34 using namespace mrpt::poses;
35 using namespace mrpt::math;
36 using namespace mrpt::random;
37 
38 using namespace std;
39 
41 
42 /** Default constructor - set all values to zero. */
44 // Un-initialized constructor:
45 CPose3DQuatPDFGaussianInf::CPose3DQuatPDFGaussianInf(
46  TConstructorFlags_Quaternions constructor_dummy_param)
48 {
49  MRPT_UNUSED_PARAM(constructor_dummy_param);
50 }
51 
52 /** Constructor from a default mean value, covariance equals to zero. */
54  const CPose3DQuat& init_Mean)
55  : mean(init_Mean), cov_inv()
56 {
57 }
58 
59 /** Constructor with mean and covariance. */
61  const CPose3DQuat& init_Mean, const CMatrixDouble77& init_CovInv)
62  : mean(init_Mean), cov_inv(init_CovInv)
63 {
64 }
65 
69 {
70  out << mean;
71 
72  for (int r = 0; r < cov_inv.rows(); r++) out << cov_inv(r, r);
73  for (int r = 0; r < cov_inv.rows(); r++)
74  for (int c = r + 1; c < cov_inv.cols(); c++) out << cov_inv(r, c);
75 }
77  mrpt::serialization::CArchive& in, uint8_t version)
78 {
79  switch (version)
80  {
81  case 0:
82  {
83  in >> mean;
84 
85  for (int r = 0; r < cov_inv.rows(); r++) in >> cov_inv(r, r);
86  for (int r = 0; r < cov_inv.rows(); r++)
87  for (int c = r + 1; c < cov_inv.cols(); c++)
88  {
89  double x;
90  in >> x;
91  cov_inv(r, c) = cov_inv(c, r) = x;
92  }
93  }
94  break;
95  default:
97  };
98 }
99 
101 {
102  if (this == &o) return; // It may be used sometimes
103 
104  // Convert to gaussian pdf:
106  o.getCovarianceAndMean(C, this->mean);
107  this->cov_inv = C.inverse_LLt();
108 }
109 
110 /*---------------------------------------------------------------
111  saveToTextFile
112  ---------------------------------------------------------------*/
113 bool CPose3DQuatPDFGaussianInf::saveToTextFile(const string& file) const
114 {
115  FILE* f = os::fopen(file.c_str(), "wt");
116  if (!f) return false;
117 
118  os::fprintf(
119  f, "%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(),
120  mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
121 
122  for (unsigned int i = 0; i < 7; i++)
123  os::fprintf(
124  f, "%e %e %e %e %e %e %e\n", cov_inv(i, 0), cov_inv(i, 1),
125  cov_inv(i, 2), cov_inv(i, 3), cov_inv(i, 4), cov_inv(i, 5),
126  cov_inv(i, 6));
127 
128  os::fclose(f);
129  return true;
130 }
131 
132 /*---------------------------------------------------------------
133  changeCoordinatesReference
134  ---------------------------------------------------------------*/
136  const CPose3D& newReferenceBase)
137 {
138  MRPT_START
139  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
140  MRPT_END
141 }
142 
143 /*---------------------------------------------------------------
144  changeCoordinatesReference
145  ---------------------------------------------------------------*/
147  const CPose3DQuat& newReferenceBaseQuat)
148 {
149  MRPT_START
150 
151  // COV:
152  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
153 
155 
157  newReferenceBaseQuat, // x
158  this->mean, // u
159  df_dx, df_du,
160  &this->mean // Output: newReferenceBaseQuat + this->mean;
161  );
162 
163  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
164  // df_dx: not used, since its COV are all zeros...
165 
166  this->cov_inv = mrpt::math::multiply_HCHt(df_du, OLD_COV).inverse_LLt();
167 
168  MRPT_END
169 }
170 
171 /*---------------------------------------------------------------
172  drawSingleSample
173  ---------------------------------------------------------------*/
175 {
176  MRPT_START
177  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
178 
180  MRPT_END
181 }
182 
183 /*---------------------------------------------------------------
184  drawManySamples
185  ---------------------------------------------------------------*/
187  size_t N, vector<CVectorDouble>& outSamples) const
188 {
189  MRPT_START
190  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
191 
192  getRandomGenerator().drawGaussianMultivariateMany(outSamples, N, COV);
193 
194  for (auto& outSample : outSamples)
195  for (unsigned int k = 0; k < 7; k++) outSample[k] += mean[k];
196 
197  MRPT_END
198 }
199 
200 /*---------------------------------------------------------------
201  inverse
202  ---------------------------------------------------------------*/
204 {
206  auto& out = dynamic_cast<CPose3DQuatPDFGaussianInf&>(o);
207 
208  // COV:
210  double lx, ly, lz;
211  mean.inverseComposePoint(0, 0, 0, lx, ly, lz, nullptr, &df_dpose);
212 
214  jacob.insertMatrix(0, 0, df_dpose);
215  jacob(3, 3) = 1;
216  jacob(4, 4) = -1;
217  jacob(5, 5) = -1;
218  jacob(6, 6) = -1;
219 
220  // C(0:2,0:2): H C H^t
221  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
222  out.cov_inv = mrpt::math::multiply_HCHt(jacob, COV).inverse_LLt();
223 
224  // Mean:
225  out.mean.x(lx);
226  out.mean.y(ly);
227  out.mean.z(lz);
228  this->mean.quat().conj(out.mean.quat());
229 }
230 
231 /*---------------------------------------------------------------
232  +=
233  ---------------------------------------------------------------*/
235 {
236  // COV:
237  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
238 
240 
242  this->mean, // x
243  Ap, // u
244  df_dx, df_du,
245  &this->mean // Output: this->mean + Ap;
246  );
247 
248  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
249  this->cov_inv = mrpt::math::multiply_HCHt(df_dx, OLD_COV).inverse_LLt();
250  // df_du: Nothing to do, since COV(Ap) = zeros
251 }
252 
253 /*---------------------------------------------------------------
254  +=
255  ---------------------------------------------------------------*/
257 {
258  // COV:
259  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
260 
262 
264  this->mean, // x
265  Ap.mean, // u
266  df_dx, df_du,
267  &this->mean // Output: this->mean + Ap.mean;
268  );
269 
270  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
271  const CMatrixDouble77 Ap_cov = Ap.cov_inv.inverse_LLt();
272  CMatrixDouble77 NEW_COV = mrpt::math::multiply_HCHt(df_dx, OLD_COV) +
273  mrpt::math::multiply_HCHt(df_du, Ap_cov);
274 
275  this->cov_inv = NEW_COV.inverse_LLt();
276 }
277 
278 /*---------------------------------------------------------------
279  -=
280  ---------------------------------------------------------------*/
282 {
283  // THIS = THIS (-) Ap -->
284  // THIS = inverse(Ap) (+) THIS
285  CPose3DQuatPDFGaussianInf inv_Ap = -Ap;
286  *this = inv_Ap + *this;
287 }
288 
289 /*---------------------------------------------------------------
290  evaluatePDF
291  ---------------------------------------------------------------*/
293 {
295  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov_inv);
296 }
297 
298 /*---------------------------------------------------------------
299  evaluateNormalizedPDF
300  ---------------------------------------------------------------*/
302  const CPose3DQuat& x) const
303 {
305  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov_inv, true);
306 }
307 
308 /*---------------------------------------------------------------
309  operator <<
310  ---------------------------------------------------------------*/
312  ostream& out, const CPose3DQuatPDFGaussianInf& obj)
313 {
314  out << "Mean: " << obj.mean << "\n";
315  out << "Information:\n" << obj.cov_inv << "\n";
316  return out;
317 }
318 
321 {
322  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
323 }
324 
325 /** Pose composition for two 3D pose Gaussians \sa
326  * CPose3DQuatPDFGaussianInf::operator += */
329 {
331  res += u;
332  return res;
333 }
334 
335 /** Inverse pose composition for two 3D pose Gaussians \sa
336  * CPose3DQuatPDFGaussianInf::operator -= */
339 {
341  res -= u;
342  return res;
343 }
A namespace of pseudo-random numbers generators of diferent distributions.
void inverse(CPose3DQuatPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
CMatrixFixed< double, 7, 1 > CMatrixDouble71
Definition: CMatrixFixed.h:364
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
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.
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
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
void copyFrom(const CPose3DQuatPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
STL namespace.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::math::CMatrixDouble77 cov_inv
The 7x7 information matrix (the inverse of the covariance)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
This base provides a set of functions for maths stuff.
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
void conj(CQuaternion &q_out) const
Return the conjugate quaternion.
Definition: CQuaternion.h:377
const GLubyte * c
Definition: glext.h:6406
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
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line (x y z qr qx qy qz)...
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:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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 1x7 vectors, where each row contains a (x,y,z,qr,qx,qy,qz) datum.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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.
void operator-=(const CPose3DQuatPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
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...
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:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
double mean(const CONTAINER &v)
Computes the mean value of a vector.
void drawSingleSample(CPose3DQuat &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
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230
GLuint res
Definition: glext.h:7385
GLenum GLint x
Definition: glext.h:3542
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=nullptr)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
#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