MRPT  2.0.1
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-2020, 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 using namespace std;
38 
40 
41 /** Default constructor - set all values to zero. */
43 // Un-initialized constructor:
44 CPose3DQuatPDFGaussianInf::CPose3DQuatPDFGaussianInf([
45  [maybe_unused]] TConstructorFlags_Quaternions constructor_dummy_param)
47 {
48 }
49 
50 /** Constructor from a default mean value, covariance equals to zero. */
52  const CPose3DQuat& init_Mean)
53  : mean(init_Mean), cov_inv()
54 {
55 }
56 
57 /** Constructor with mean and covariance. */
59  const CPose3DQuat& init_Mean, const CMatrixDouble77& init_CovInv)
60  : mean(init_Mean), cov_inv(init_CovInv)
61 {
62 }
63 
67 {
68  out << mean;
69 
70  for (int r = 0; r < cov_inv.rows(); r++) out << cov_inv(r, r);
71  for (int r = 0; r < cov_inv.rows(); r++)
72  for (int c = r + 1; c < cov_inv.cols(); c++) out << cov_inv(r, c);
73 }
75  mrpt::serialization::CArchive& in, uint8_t version)
76 {
77  switch (version)
78  {
79  case 0:
80  {
81  in >> mean;
82 
83  for (int r = 0; r < cov_inv.rows(); r++) in >> cov_inv(r, r);
84  for (int r = 0; r < cov_inv.rows(); r++)
85  for (int c = r + 1; c < cov_inv.cols(); c++)
86  {
87  double x;
88  in >> x;
89  cov_inv(r, c) = cov_inv(c, r) = x;
90  }
91  }
92  break;
93  default:
95  };
96 }
97 
99 {
100  if (this == &o) return; // It may be used sometimes
101 
102  // Convert to gaussian pdf:
104  o.getCovarianceAndMean(C, this->mean);
105  this->cov_inv = C.inverse_LLt();
106 }
107 
108 /*---------------------------------------------------------------
109  saveToTextFile
110  ---------------------------------------------------------------*/
111 bool CPose3DQuatPDFGaussianInf::saveToTextFile(const string& file) const
112 {
113  FILE* f = os::fopen(file.c_str(), "wt");
114  if (!f) return false;
115 
116  os::fprintf(
117  f, "%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(),
118  mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
119 
120  for (unsigned int i = 0; i < 7; i++)
121  os::fprintf(
122  f, "%e %e %e %e %e %e %e\n", cov_inv(i, 0), cov_inv(i, 1),
123  cov_inv(i, 2), cov_inv(i, 3), cov_inv(i, 4), cov_inv(i, 5),
124  cov_inv(i, 6));
125 
126  os::fclose(f);
127  return true;
128 }
129 
130 /*---------------------------------------------------------------
131  changeCoordinatesReference
132  ---------------------------------------------------------------*/
134  const CPose3D& newReferenceBase)
135 {
136  MRPT_START
137  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
138  MRPT_END
139 }
140 
141 /*---------------------------------------------------------------
142  changeCoordinatesReference
143  ---------------------------------------------------------------*/
145  const CPose3DQuat& newReferenceBaseQuat)
146 {
147  MRPT_START
148 
149  // COV:
150  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
151 
153 
155  newReferenceBaseQuat, // x
156  this->mean, // u
157  df_dx, df_du,
158  &this->mean // Output: newReferenceBaseQuat + this->mean;
159  );
160 
161  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
162  // df_dx: not used, since its COV are all zeros...
163 
164  this->cov_inv = mrpt::math::multiply_HCHt(df_du, OLD_COV).inverse_LLt();
165 
166  MRPT_END
167 }
168 
169 /*---------------------------------------------------------------
170  drawSingleSample
171  ---------------------------------------------------------------*/
173 {
174  MRPT_START
175  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
176 
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  drawManySamples
183  ---------------------------------------------------------------*/
185  size_t N, vector<CVectorDouble>& outSamples) const
186 {
187  MRPT_START
188  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
189 
190  getRandomGenerator().drawGaussianMultivariateMany(outSamples, N, COV);
191 
192  for (auto& outSample : outSamples)
193  for (unsigned int k = 0; k < 7; k++) outSample[k] += mean[k];
194 
195  MRPT_END
196 }
197 
198 /*---------------------------------------------------------------
199  inverse
200  ---------------------------------------------------------------*/
202 {
204  auto& out = dynamic_cast<CPose3DQuatPDFGaussianInf&>(o);
205 
206  // COV:
208  double lx, ly, lz;
209  mean.inverseComposePoint(0, 0, 0, lx, ly, lz, nullptr, &df_dpose);
210 
212  jacob.insertMatrix(0, 0, df_dpose);
213  jacob(3, 3) = 1;
214  jacob(4, 4) = -1;
215  jacob(5, 5) = -1;
216  jacob(6, 6) = -1;
217 
218  // C(0:2,0:2): H C H^t
219  const CMatrixDouble77 COV = this->cov_inv.inverse_LLt();
220  out.cov_inv = mrpt::math::multiply_HCHt(jacob, COV).inverse_LLt();
221 
222  // Mean:
223  out.mean.x(lx);
224  out.mean.y(ly);
225  out.mean.z(lz);
226  this->mean.quat().conj(out.mean.quat());
227 }
228 
229 /*---------------------------------------------------------------
230  +=
231  ---------------------------------------------------------------*/
233 {
234  // COV:
235  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
236 
238 
240  this->mean, // x
241  Ap, // u
242  df_dx, df_du,
243  &this->mean // Output: this->mean + Ap;
244  );
245 
246  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
247  this->cov_inv = mrpt::math::multiply_HCHt(df_dx, OLD_COV).inverse_LLt();
248  // df_du: Nothing to do, since COV(Ap) = zeros
249 }
250 
251 /*---------------------------------------------------------------
252  +=
253  ---------------------------------------------------------------*/
255 {
256  // COV:
257  const CMatrixDouble77 OLD_COV = this->cov_inv.inverse_LLt();
258 
260 
262  this->mean, // x
263  Ap.mean, // u
264  df_dx, df_du,
265  &this->mean // Output: this->mean + Ap.mean;
266  );
267 
268  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
269  const CMatrixDouble77 Ap_cov = Ap.cov_inv.inverse_LLt();
270  CMatrixDouble77 NEW_COV = mrpt::math::multiply_HCHt(df_dx, OLD_COV) +
271  mrpt::math::multiply_HCHt(df_du, Ap_cov);
272 
273  this->cov_inv = NEW_COV.inverse_LLt();
274 }
275 
276 /*---------------------------------------------------------------
277  -=
278  ---------------------------------------------------------------*/
280 {
281  // THIS = THIS (-) Ap -->
282  // THIS = inverse(Ap) (+) THIS
283  CPose3DQuatPDFGaussianInf inv_Ap = -Ap;
284  *this = inv_Ap + *this;
285 }
286 
287 /*---------------------------------------------------------------
288  evaluatePDF
289  ---------------------------------------------------------------*/
291 {
293  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov_inv);
294 }
295 
296 /*---------------------------------------------------------------
297  evaluateNormalizedPDF
298  ---------------------------------------------------------------*/
300  const CPose3DQuat& x) const
301 {
303  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov_inv, true);
304 }
305 
306 /*---------------------------------------------------------------
307  operator <<
308  ---------------------------------------------------------------*/
310  ostream& out, const CPose3DQuatPDFGaussianInf& obj)
311 {
312  out << "Mean: " << obj.mean << "\n";
313  out << "Information:\n" << obj.cov_inv << "\n";
314  return out;
315 }
316 
319 {
320  return p1.mean == p2.mean && p1.cov_inv == p2.cov_inv;
321 }
322 
323 /** Pose composition for two 3D pose Gaussians \sa
324  * CPose3DQuatPDFGaussianInf::operator += */
327 {
329  res += u;
330  return res;
331 }
332 
333 /** Inverse pose composition for two 3D pose Gaussians \sa
334  * CPose3DQuatPDFGaussianInf::operator -= */
337 {
339  res -= u;
340  return res;
341 }
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:379
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.
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:412
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:408
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
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
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
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.
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020