MRPT  2.0.4
CPose3DQuatPDFGaussian.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 
15 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPose3DQuat.h>
21 #include <mrpt/system/os.h>
22 #include <Eigen/Dense>
23 
24 using namespace mrpt;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace mrpt::random;
28 using namespace mrpt::system;
29 using namespace std;
30 
32 
34 {
36 }
38 {
40 }
41 
43 
44 /** Default constructor - set all values to zero. */
46 // Un-initialized constructor:
48  [maybe_unused]] TConstructorFlags_Quaternions constructor_dummy_param)
50 {
51 }
52 
53 /** Constructor from a default mean value, covariance equals to zero. */
55  : mean(init_Mean), cov()
56 {
57 }
58 
59 /** Constructor with mean and covariance. */
61  const CPose3DQuat& init_Mean, const CMatrixDouble77& init_Cov)
62  : mean(init_Mean), cov(init_Cov)
63 {
64 }
65 
66 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables).
67  */
70 {
71  this->copyFrom(CPose3DPDFGaussian(o));
72 }
73 
74 /** Constructor from an equivalent Gaussian in Euler angles representation. */
77 {
78  this->copyFrom(o);
79 }
80 
82 
83 uint8_t CPose3DQuatPDFGaussian::serializeGetVersion() const { return 0; }
86 {
87  out << mean;
89 }
91  mrpt::serialization::CArchive& in, uint8_t version)
92 {
93  switch (version)
94  {
95  case 0:
96  {
97  in >> mean;
99  }
100  break;
101  default:
103  };
104 }
105 
107 {
108  if (this == &o) return; // It may be used sometimes
109 
110  // Convert to gaussian pdf:
112 }
113 
115 {
116  CPose3DPDFGaussian aux;
117  aux.copyFrom(o);
118  this->copyFrom(aux);
119 }
120 
122  const CVectorFixedDouble<6>& x, [[maybe_unused]] const double& dummy,
124 {
125  y[0] = x[0];
126  y[1] = x[1];
127  y[2] = x[2];
128 
129  CPose3D p(0, 0, 0, x[3], x[4], x[5]);
131  p.getAsQuaternion(q);
132  y[3] = q[0];
133  y[4] = q[1];
134  y[5] = q[2];
135  y[6] = q[3];
136 }
137 
139 {
141  { // Use Jacobians
143 
144  // Mean:
145  mean.x(o.mean.x());
146  mean.y(o.mean.y());
147  mean.z(o.mean.z());
148 
149  o.mean.getAsQuaternion(mean.quat(), dq_dr_sub);
150 
151  // Cov:
153  dq_dr(0, 0) = dq_dr(1, 1) = dq_dr(2, 2) = 1;
154  dq_dr.insertMatrix(3, 3, dq_dr_sub);
155  // Now for the covariance:
156  this->cov = mrpt::math::multiply_HCHt(dq_dr, o.cov);
157  }
158  else
159  {
160  // Use UT transformation:
161  // f: R^6 => R^7
162  const CVectorFixedDouble<6> x_mean(o.mean);
163 
164  const double dummy = 0;
166  x_mean, o.cov, &aux_poseypr2posequat, dummy, this->mean, this->cov);
167  }
168 }
169 
170 /*---------------------------------------------------------------
171  saveToTextFile
172  ---------------------------------------------------------------*/
173 bool CPose3DQuatPDFGaussian::saveToTextFile(const string& file) const
174 {
175  FILE* f = os::fopen(file.c_str(), "wt");
176  if (!f) return false;
177 
178  os::fprintf(
179  f, "%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(),
180  mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
181 
182  for (unsigned int i = 0; i < 7; i++)
183  os::fprintf(
184  f, "%e %e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
185  cov(i, 3), cov(i, 4), cov(i, 5), cov(i, 6));
186 
187  os::fclose(f);
188  return true;
189 }
190 
191 /*---------------------------------------------------------------
192  changeCoordinatesReference
193  ---------------------------------------------------------------*/
195  const CPose3D& newReferenceBase)
196 {
197  MRPT_START
198  changeCoordinatesReference(CPose3DQuat(newReferenceBase));
199  MRPT_END
200 }
201 
202 /*---------------------------------------------------------------
203  changeCoordinatesReference
204  ---------------------------------------------------------------*/
206  const CPose3DQuat& newReferenceBaseQuat)
207 {
208  MRPT_START
209 
210  // COV:
211  const CMatrixDouble77 OLD_COV = this->cov;
213 
215  newReferenceBaseQuat, // x
216  this->mean, // u
217  df_dx, df_du,
218  &this->mean // Output: newReferenceBaseQuat + this->mean;
219  );
220 
221  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
222  // df_dx: not used, since its COV are all zeros...
223  cov = mrpt::math::multiply_HCHt(df_du, OLD_COV);
224 
225  MRPT_END
226 }
227 
228 /*---------------------------------------------------------------
229  drawSingleSample
230  ---------------------------------------------------------------*/
232 {
233  MRPT_START
235  MRPT_END
236 }
237 
238 /*---------------------------------------------------------------
239  drawManySamples
240  ---------------------------------------------------------------*/
242  size_t N, vector<CVectorDouble>& outSamples) const
243 {
244  MRPT_START
245 
247 
248  for (auto& outSample : outSamples)
249  for (unsigned int k = 0; k < 7; k++) outSample[k] += mean[k];
250 
251  MRPT_END
252 }
253 
254 /*---------------------------------------------------------------
255  inverse
256  ---------------------------------------------------------------*/
258 {
260  auto& out = dynamic_cast<CPose3DQuatPDFGaussian&>(o);
261 
262  // COV:
264  double lx, ly, lz;
265  mean.inverseComposePoint(0, 0, 0, lx, ly, lz, nullptr, &df_dpose);
266 
268  jacob.insertMatrix(0, 0, df_dpose);
269  jacob(3, 3) = 1;
270  jacob(4, 4) = -1;
271  jacob(5, 5) = -1;
272  jacob(6, 6) = -1;
273 
275  this->mean.quat().normalizationJacobian(norm_jacob);
276  jacob.asEigen().block<4, 4>(3, 3) *= norm_jacob.asEigen();
277 
278  // C(0:2,0:2): H C H^t
279  out.cov = mrpt::math::multiply_HCHt(jacob, this->cov);
280 
281  // Mean:
282  out.mean.x(lx);
283  out.mean.y(ly);
284  out.mean.z(lz);
285  this->mean.quat().conj(out.mean.quat());
286 }
287 
288 /*---------------------------------------------------------------
289  +=
290  ---------------------------------------------------------------*/
292 {
293  // COV:
294  const CMatrixDouble77 OLD_COV = this->cov;
296 
298  this->mean, // x
299  Ap, // u
300  df_dx, df_du,
301  &this->mean // Output: this->mean + Ap;
302  );
303 
304  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
305  cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV);
306  // df_du: Nothing to do, since COV(Ap) = zeros
307 }
308 
309 /*---------------------------------------------------------------
310  +=
311  ---------------------------------------------------------------*/
313 {
314  // COV:
315  const CMatrixDouble77 OLD_COV = this->cov;
317 
319  this->mean, // x
320  Ap.mean, // u
321  df_dx, df_du,
322  &this->mean // Output: this->mean + Ap.mean;
323  );
324 
325  // this->cov = H1*this->cov*H1' + H2*Ap.cov*H2';
326  cov = mrpt::math::multiply_HCHt(df_dx, OLD_COV);
327  cov += mrpt::math::multiply_HCHt(df_du, Ap.cov);
328 }
329 
330 /*---------------------------------------------------------------
331  -=
332  ---------------------------------------------------------------*/
334 {
335  // THIS = THIS (-) Ap -->
336  // THIS = inverse(Ap) (+) THIS
337  CPose3DQuatPDFGaussian inv_Ap = -Ap;
338  *this = inv_Ap + *this;
339 }
340 
341 /*---------------------------------------------------------------
342  evaluatePDF
343  ---------------------------------------------------------------*/
345 {
346  return mrpt::math::normalPDF(
347  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov);
348 }
349 
350 /*---------------------------------------------------------------
351  evaluateNormalizedPDF
352  ---------------------------------------------------------------*/
354 {
355  return mrpt::math::normalPDF(
356  CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov, true);
357 }
358 
359 /*---------------------------------------------------------------
360  enforceCovSymmetry
361  ---------------------------------------------------------------*/
363 {
364  // Differences, when they exist, appear in the ~15'th significant
365  // digit, so... just take one of them arbitrarily!
366  for (int i = 0; i < cov.rows() - 1; i++)
367  for (int j = i + 1; j < cov.rows(); j++) cov(i, j) = cov(j, i);
368 }
369 
370 /*---------------------------------------------------------------
371  mahalanobisDistanceTo
372  ---------------------------------------------------------------*/
374  const CPose3DQuatPDFGaussian& theOther)
375 {
376  MRPT_START
377  const CMatrixDouble77 COV2 = cov + theOther.cov;
379  CMatrixDouble71(this->mean) - CMatrixDouble71(theOther.mean), COV2);
380  MRPT_END
381 }
382 
383 /*---------------------------------------------------------------
384  operator <<
385  ---------------------------------------------------------------*/
387  ostream& out, const CPose3DQuatPDFGaussian& obj)
388 {
389  out << "Mean: " << obj.mean << "\n";
390  out << "Covariance:\n" << obj.cov << "\n";
391  return out;
392 }
393 
395  const CPose3DQuatPDFGaussian& p1, const CPose3DQuatPDFGaussian& p2)
396 {
397  return p1.mean == p2.mean && p1.cov == p2.cov;
398 }
399 
402 {
403  CPose3DQuatPDFGaussian res(x);
404  res += u;
405  return res;
406 }
407 
410 {
411  CPose3DQuatPDFGaussian res(x);
412  res -= u;
413  return res;
414 }
mrpt::math::cov
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
os.h
mrpt::poses::CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian
CPose3DQuatPDFGaussian()
Default constructor - set all values to zero.
Definition: CPose3DQuatPDFGaussian.cpp:45
mrpt::poses::CPose3DQuatPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually),...
Definition: CPose3DQuatPDF.h:43
poses-precomp.h
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
matrix_serialization.h
mrpt::poses::CPose3DQuatPDFGaussian::getMean
void getMean(CPose3DQuat &mean_pose) const override
Definition: CPose3DQuatPDFGaussian.cpp:81
mrpt::math::CProbabilityDensityFunction::getCovarianceAndMean
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,...
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::global_settings::USE_SUT_EULER2QUAT_CONVERSION
void USE_SUT_EULER2QUAT_CONVERSION(bool value)
If set to true (default), a Scaled Unscented Transform is used instead of a linear approximation with...
Definition: CPose3DQuatPDFGaussian.cpp:37
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
mrpt::poses::operator+
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
mrpt::math::mahalanobisDistance
VECTORLIKE1::Scalar mahalanobisDistance(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the mahalanobis distance of a vector X given the mean MU and the covariance inverse COV_inv.
Definition: data_utils.h:57
mrpt::poses::CPose3DQuatPDFGaussian::copyFrom
void copyFrom(const CPose3DQuatPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DQuatPDFGaussian.cpp:106
CPose3DPDFGaussian.h
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:205
mrpt::random::CRandomGenerator::drawGaussianMultivariateMany
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.
Definition: RandomGenerators.h:342
mrpt::poses::CPose3DQuatPDFGaussian::drawManySamples
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,...
Definition: CPose3DQuatPDFGaussian.cpp:241
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::math::serializeSymmetricMatrixTo
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
Definition: matrix_serialization.h:114
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses::operator<<
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:45
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses::operator-
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
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPose3DQuatPDFGaussian::saveToTextFile
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),...
Definition: CPose3DQuatPDFGaussian.cpp:173
mrpt::poses::CPose3DQuatPDFGaussian::drawSingleSample
void drawSingleSample(CPose3DQuat &outPart) const override
Draws a single sample from the distribution.
Definition: CPose3DQuatPDFGaussian.cpp:231
mrpt::poses::CPose3DQuatPDFGaussian::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose3DQuatPDFGaussian.cpp:90
mrpt::poses::CPose3DQuatPDFGaussian::evaluatePDF
double evaluatePDF(const CPose3DQuat &x) const
Evaluates the PDF at a given point.
Definition: CPose3DQuatPDFGaussian.cpp:344
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::math::normalPDF
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
CPose3DQuatPDF.h
mrpt::math::CMatrixFixed::rows
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
mrpt::poses::CPose3DQuatPDFGaussian::operator+=
void operator+=(const CPose3DQuat &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
Definition: CPose3DQuatPDFGaussian.cpp:291
mrpt::poses::CPose3DQuatPDFGaussian::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose3DQuatPDFGaussian.cpp:83
mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition
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,...
Definition: CPose3DQuatPDF.cpp:42
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
mrpt::poses::CPose3DQuatPDFGaussian::cov
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
Definition: CPose3DQuatPDFGaussian.h:81
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::CQuaternion::conj
void conj(CQuaternion &q_out) const
Return the conjugate quaternion
Definition: CQuaternion.h:413
mrpt::math::CMatrixDouble71
CMatrixFixed< double, 7, 1 > CMatrixDouble71
Definition: CMatrixFixed.h:379
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose3DQuatPDFGaussian::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPose3DQuatPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
Definition: CPose3DQuatPDFGaussian.cpp:373
mrpt::math::multiply_HCHt
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
USE_SUT_EULER2QUAT_CONVERSION_value
bool USE_SUT_EULER2QUAT_CONVERSION_value
Definition: CPose3DQuatPDFGaussian.cpp:31
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::deserializeSymmetricMatrixFrom
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
Definition: matrix_serialization.h:95
mrpt::poses::CPose3DQuatPDFGaussian::inverse
void inverse(CPose3DQuatPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DQuatPDFGaussian.cpp:257
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
distributions.h
CPose3DQuat.h
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:314
mrpt::math::MatrixBase< T, CMatrixFixed< T, ROWS, COLS > >::insertMatrix
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
mrpt::math::TConstructorFlags_Quaternions
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
mrpt::poses::CPose3DQuat::inverseComposePoint
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 .
Definition: CPose3DQuat.cpp:187
mrpt::poses::CPose3DQuatPDF::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
aux_poseypr2posequat
void aux_poseypr2posequat(const CVectorFixedDouble< 6 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &y)
Definition: CPose3DQuatPDFGaussian.cpp:121
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::random::CRandomGenerator::drawGaussianMultivariate
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.
Definition: RandomGenerators.h:255
CPose3D.h
mrpt::poses::CPosePDF
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition: CPosePDF.h:38
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:57
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:44
CPose3DQuatPDFGaussian.h
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::poses::CPose3DQuatPDFGaussian::mean
CPose3DQuat mean
The mean value.
Definition: CPose3DQuatPDFGaussian.h:79
mrpt::poses::CPose3DQuatPDFGaussian::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3DQuatPDFGaussian.cpp:84
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::poses::operator==
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:43
mrpt::poses::CPose3DQuatPDFGaussian::evaluateNormalizedPDF
double evaluateNormalizedPDF(const CPose3DQuat &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
Definition: CPose3DQuatPDFGaussian.cpp:353
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
mrpt::math::transform_gaussian_unscented
void transform_gaussian_unscented(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const bool *elem_do_wrap2pi=nullptr, const double alpha=1e-3, const double K=0, const double beta=2.0)
Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...
Definition: transform_gaussian.h:46
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::poses::CPose3DQuatPDFGaussian::operator-=
void operator-=(const CPose3DQuatPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean,...
Definition: CPose3DQuatPDFGaussian.cpp:333
transform_gaussian.h
mrpt::poses::CPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
Definition: CPose3D.cpp:501
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::poses::CPose3DPDFGaussian::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGaussian.cpp:245
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
mrpt::math::UNINITIALIZED_QUATERNION
@ UNINITIALIZED_QUATERNION
Definition: CQuaternion.h:22
mrpt::system
Definition: backtrace.h:14
mrpt::poses::CPose3DQuatPDFGaussian::enforceCovSymmetry
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
Definition: CPose3DQuatPDFGaussian.cpp:362



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020