MRPT  2.0.5
CPointPDFGaussian.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 
13 #include <mrpt/math/ops_matrices.h>
15 #include <mrpt/poses/CPose3D.h>
19 #include <mrpt/system/os.h>
20 #include <Eigen/Dense>
21 
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 using namespace mrpt::random;
25 using namespace mrpt::system;
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33 /*---------------------------------------------------------------
34  Constructor
35  ---------------------------------------------------------------*/
37  const CPoint3D& init_Mean, const CMatrixDouble33& init_Cov)
38  : mean(init_Mean), cov(init_Cov)
39 {
40 }
41 
42 /*---------------------------------------------------------------
43  Constructor
44  ---------------------------------------------------------------*/
46  : mean(init_Mean), cov()
47 {
48  cov.setZero();
49 }
50 
51 /*---------------------------------------------------------------
52  getMean
53  Returns an estimate of the pose, (the mean, or mathematical expectation of the
54  PDF)
55  ---------------------------------------------------------------*/
56 void CPointPDFGaussian::getMean(CPoint3D& p) const { p = mean; }
57 
58 uint8_t CPointPDFGaussian::serializeGetVersion() const { return 1; }
60 {
61  out << CPoint3D(mean) << cov;
62 }
64  mrpt::serialization::CArchive& in, uint8_t version)
65 {
66  switch (version)
67  {
68  case 0:
69  {
70  in >> mean;
71 
72  CMatrixF c;
73  in >> c;
74  cov = c.cast_double();
75  }
76  break;
77  case 1:
78  {
79  in >> mean >> cov;
80  }
81  break;
82  default:
84  };
85 }
88 {
90  out["mean"] = mean;
91  out["cov"] = CMatrixD(cov);
92 }
95 {
96  uint8_t version;
98  switch (version)
99  {
100  case 1:
101  {
102  in["mean"].readTo(mean);
103  CMatrixD m;
104  in["cov"].readTo(m);
105  cov = m;
106  }
107  break;
108  default:
110  }
111 }
112 
114 {
115  if (this == &o) return; // It may be used sometimes
116 
117  // Convert to gaussian pdf:
119 }
120 
121 /*---------------------------------------------------------------
122 
123  ---------------------------------------------------------------*/
124 bool CPointPDFGaussian::saveToTextFile(const std::string& file) const
125 {
126  MRPT_START
127  FILE* f = os::fopen(file.c_str(), "wt");
128  if (!f) return false;
129  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.z());
130  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
131  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
132  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
133  os::fclose(f);
134  return true;
135  MRPT_END
136 }
137 
138 /*---------------------------------------------------------------
139  changeCoordinatesReference
140  ---------------------------------------------------------------*/
142  const CPose3D& newReferenceBase)
143 {
144  const CMatrixDouble33& M = newReferenceBase.getRotationMatrix();
145 
146  // The mean:
147  mean = newReferenceBase + mean;
148 
149  // The covariance:
150  cov = M.asEigen() * cov.asEigen() * M.transpose();
151 }
152 
153 /*---------------------------------------------------------------
154  bayesianFusion
155  ---------------------------------------------------------------*/
157  const CPointPDFGaussian& p1, const CPointPDFGaussian& p2)
158 {
159  MRPT_START
160 
161  CMatrixDouble31 x1, x2;
162  const auto C1 = p1.cov;
163  const auto C2 = p2.cov;
164  const CMatrixDouble33 C1_inv = C1.inverse_LLt();
165  const CMatrixDouble33 C2_inv = C2.inverse_LLt();
166 
167  x1(0, 0) = p1.mean.x();
168  x1(1, 0) = p1.mean.y();
169  x1(2, 0) = p1.mean.z();
170  x2(0, 0) = p2.mean.x();
171  x2(1, 0) = p2.mean.y();
172  x2(2, 0) = p2.mean.z();
173 
174  cov = CMatrixDouble33(C1_inv + C2_inv).inverse_LLt();
175 
176  auto x = cov.asEigen() * (C1_inv.asEigen() * x1.asEigen() +
177  C2_inv.asEigen() * x2.asEigen());
178 
179  mean.x(x(0, 0));
180  mean.y(x(1, 0));
181  mean.z(x(2, 0));
182 
183  MRPT_END
184 }
185 
186 /*---------------------------------------------------------------
187  productIntegralWith
188  ---------------------------------------------------------------*/
190 {
191  MRPT_START
192 
193  // --------------------------------------------------------------
194  // 12/APR/2009 - Jose Luis Blanco:
195  // The integral over all the variable space of the product of two
196  // Gaussians variables amounts to simply the evaluation of
197  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
198  // ---------------------------------------------------------------
199  CMatrixDouble33 C = cov;
200  C += p.cov; // Sum of covs
201  CMatrixDouble33 C_inv = C.inverse_LLt();
202 
203  const Eigen::Vector3d MU(
204  mean.x() - p.mean.x(), mean.y() - p.mean.y(), mean.z() - p.mean.z());
205 
206  return std::pow(M_2PI, -0.5 * state_length) * (1.0 / std::sqrt(C.det())) *
207  exp(-0.5 * (MU.transpose() * C_inv.asEigen() * MU)(0, 0));
208 
209  MRPT_END
210 }
211 
212 /*---------------------------------------------------------------
213  productIntegralWith2D
214  ---------------------------------------------------------------*/
216  const CPointPDFGaussian& p) const
217 {
218  MRPT_START
219 
220  // --------------------------------------------------------------
221  // 12/APR/2009 - Jose Luis Blanco:
222  // The integral over all the variable space of the product of two
223  // Gaussians variables amounts to simply the evaluation of
224  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
225  // ---------------------------------------------------------------
226  // Sum of covs:
227  const auto C = cov.blockCopy<2, 2>(0, 0) + p.cov.blockCopy<2, 2>(0, 0);
228  CMatrixDouble22 C_inv = C.inverse_LLt();
229 
230  const Eigen::Vector2d MU(mean.x() - p.mean.x(), mean.y() - p.mean.y());
231 
232  return std::pow(M_2PI, -0.5 * (state_length - 1)) *
233  (1.0 / std::sqrt(C.det())) *
234  exp(-0.5 * (MU.transpose() * C_inv.asEigen() * MU)(0, 0));
235 
236  MRPT_END
237 }
238 
239 /*---------------------------------------------------------------
240  productIntegralNormalizedWith
241  ---------------------------------------------------------------*/
243  const CPointPDFGaussian& p) const
244 {
245  return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
246 }
247 
248 /*---------------------------------------------------------------
249  productIntegralNormalizedWith
250  ---------------------------------------------------------------*/
252  const CPointPDFGaussian& p) const
253 {
254  return std::exp(-0.5 * square(mahalanobisDistanceTo(p, true)));
255 }
256 
257 /*---------------------------------------------------------------
258  drawSingleSample
259  ---------------------------------------------------------------*/
261 {
262  MRPT_START
263 
264  CVectorDouble vec;
266 
267  ASSERT_(vec.size() == 3);
268  outSample.x(mean.x() + vec[0]);
269  outSample.y(mean.y() + vec[1]);
270  outSample.z(mean.z() + vec[2]);
271 
272  MRPT_END
273 }
274 
275 /*---------------------------------------------------------------
276  bayesianFusion
277  ---------------------------------------------------------------*/
279  const CPointPDF& p1_, const CPointPDF& p2_,
280  [[maybe_unused]] const double minMahalanobisDistToDrop)
281 {
282  MRPT_START
283 
284  // p1: CPointPDFGaussian, p2: CPosePDFGaussian:
287 
288  THROW_EXCEPTION("TODO!!!");
289 
290  MRPT_END
291 }
292 
293 /*---------------------------------------------------------------
294  mahalanobisDistanceTo
295  ---------------------------------------------------------------*/
297  const CPointPDFGaussian& other, bool only_2D) const
298 {
299  // The difference in means:
300  CMatrixDouble13 deltaX;
301  deltaX(0, 0) = other.mean.x() - mean.x();
302  deltaX(0, 1) = other.mean.y() - mean.y();
303  deltaX(0, 2) = other.mean.z() - mean.z();
304 
305  // The inverse of the combined covs:
306  CMatrixDouble33 COV = other.cov;
307  COV += this->cov;
308 
309  if (!only_2D)
310  {
311  const CMatrixDouble33 COV_inv = COV.inverse_LLt();
312  return sqrt(mrpt::math::multiply_HCHt_scalar(deltaX, COV_inv));
313  }
314  else
315  {
316  auto C = CMatrixDouble22(COV.block<2, 2>(0, 0));
317  const CMatrixDouble22 COV_inv = C.inverse_LLt();
318  auto deltaX2 = CMatrixDouble12(deltaX.block<1, 2>(0, 0));
319  return std::sqrt(mrpt::math::multiply_HCHt_scalar(deltaX2, COV_inv));
320  }
321 }
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
A namespace of pseudo-random numbers generators of diferent distributions.
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
void getMean(CPoint3D &p) const override
CMatrixFixed< double, 1, 2 > CMatrixDouble12
Definition: CMatrixFixed.h:375
#define MRPT_START
Definition: exceptions.h:241
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPoint3D mean
The mean value.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H*C*H^t (H: row vector, C: symmetric matrix)
Definition: ops_matrices.h:63
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Virtual base class for "schematic archives" (JSON, XML,...)
#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 bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
This base provides a set of functions for maths stuff.
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
double productIntegralNormalizedWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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
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
CMatrixFixed< double, 2, 2 > CMatrixDouble22
Definition: CMatrixFixed.h:364
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
static constexpr size_t state_length
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll)...
return_t square(const num_t x)
Inline function for the square of a number.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
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
double mahalanobisDistanceTo(const CPointPDFGaussian &other, bool only_2D=false) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
This file implements matrix/vector text and binary serialization.
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
#define MRPT_END
Definition: exceptions.h:245
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
CPointPDFGaussian()
Default constructor.
CMatrixFixed< double, ROWS, COLS > cast_double() const
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A gaussian distribution for 3D points.



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020