MRPT  1.9.9
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-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 
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 
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace mrpt::system;
27 
29 
30 /*---------------------------------------------------------------
31  Constructor
32  ---------------------------------------------------------------*/
34 /*---------------------------------------------------------------
35  Constructor
36  ---------------------------------------------------------------*/
38  const CPoint3D& init_Mean, const CMatrixDouble33& init_Cov)
39  : mean(init_Mean), cov(init_Cov)
40 {
41 }
42 
43 /*---------------------------------------------------------------
44  Constructor
45  ---------------------------------------------------------------*/
47  : mean(init_Mean), cov()
48 {
49  cov.setZero();
50 }
51 
52 /*---------------------------------------------------------------
53  getMean
54  Returns an estimate of the pose, (the mean, or mathematical expectation of the
55  PDF)
56  ---------------------------------------------------------------*/
58 
59 uint8_t CPointPDFGaussian::serializeGetVersion() const { return 1; }
61 {
62  out << CPoint3D(mean) << cov;
63 }
65  mrpt::serialization::CArchive& in, uint8_t version)
66 {
67  switch (version)
68  {
69  case 0:
70  {
71  in >> mean;
72 
73  CMatrixF c;
74  in >> c;
75  cov = c.cast_double();
76  }
77  break;
78  case 1:
79  {
80  in >> mean >> cov;
81  }
82  break;
83  default:
85  };
86 }
89 {
91  out["mean"] = mean;
92  out["cov"] = CMatrixD(cov);
93 }
96 {
97  uint8_t version;
99  switch (version)
100  {
101  case 1:
102  {
103  in["mean"].readTo(mean);
104  CMatrixD m;
105  in["cov"].readTo(m);
106  cov = m;
107  }
108  break;
109  default:
111  }
112 }
113 
115 {
116  if (this == &o) return; // It may be used sometimes
117 
118  // Convert to gaussian pdf:
120 }
121 
122 /*---------------------------------------------------------------
123 
124  ---------------------------------------------------------------*/
126 {
127  MRPT_START
128  FILE* f = os::fopen(file.c_str(), "wt");
129  if (!f) return false;
130  os::fprintf(f, "%f %f %f\n", mean.x(), mean.y(), mean.z());
131  os::fprintf(f, "%f %f %f\n", cov(0, 0), cov(0, 1), cov(0, 2));
132  os::fprintf(f, "%f %f %f\n", cov(1, 0), cov(1, 1), cov(1, 2));
133  os::fprintf(f, "%f %f %f\n", cov(2, 0), cov(2, 1), cov(2, 2));
134  os::fclose(f);
135  return true;
136  MRPT_END
137 }
138 
139 /*---------------------------------------------------------------
140  changeCoordinatesReference
141  ---------------------------------------------------------------*/
143  const CPose3D& newReferenceBase)
144 {
145  const CMatrixDouble33& M = newReferenceBase.getRotationMatrix();
146 
147  // The mean:
148  mean = newReferenceBase + mean;
149 
150  // The covariance:
151  cov = M.asEigen() * cov.asEigen() * M.transpose();
152 }
153 
154 /*---------------------------------------------------------------
155  bayesianFusion
156  ---------------------------------------------------------------*/
158  const CPointPDFGaussian& p1, const CPointPDFGaussian& p2)
159 {
160  MRPT_START
161 
162  CMatrixDouble31 x1, x2;
163  const auto C1 = p1.cov;
164  const auto C2 = p2.cov;
165  const CMatrixDouble33 C1_inv = C1.inverse_LLt();
166  const CMatrixDouble33 C2_inv = C2.inverse_LLt();
167 
168  x1(0, 0) = p1.mean.x();
169  x1(1, 0) = p1.mean.y();
170  x1(2, 0) = p1.mean.z();
171  x2(0, 0) = p2.mean.x();
172  x2(1, 0) = p2.mean.y();
173  x2(2, 0) = p2.mean.z();
174 
175  cov = CMatrixDouble33(C1_inv + C2_inv).inverse_LLt();
176 
177  auto x = cov.asEigen() * (C1_inv.asEigen() * x1.asEigen() +
178  C2_inv.asEigen() * x2.asEigen());
179 
180  mean.x(x(0, 0));
181  mean.y(x(1, 0));
182  mean.z(x(2, 0));
183 
184  MRPT_END
185 }
186 
187 /*---------------------------------------------------------------
188  productIntegralWith
189  ---------------------------------------------------------------*/
191 {
192  MRPT_START
193 
194  // --------------------------------------------------------------
195  // 12/APR/2009 - Jose Luis Blanco:
196  // The integral over all the variable space of the product of two
197  // Gaussians variables amounts to simply the evaluation of
198  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
199  // ---------------------------------------------------------------
200  CMatrixDouble33 C = cov;
201  C += p.cov; // Sum of covs
202  CMatrixDouble33 C_inv = C.inverse_LLt();
203 
204  const Eigen::Vector3d MU(
205  mean.x() - p.mean.x(), mean.y() - p.mean.y(), mean.z() - p.mean.z());
206 
207  return std::pow(M_2PI, -0.5 * state_length) * (1.0 / std::sqrt(C.det())) *
208  exp(-0.5 * (MU.transpose() * C_inv.asEigen() * MU)(0, 0));
209 
210  MRPT_END
211 }
212 
213 /*---------------------------------------------------------------
214  productIntegralWith2D
215  ---------------------------------------------------------------*/
217  const CPointPDFGaussian& p) const
218 {
219  MRPT_START
220 
221  // --------------------------------------------------------------
222  // 12/APR/2009 - Jose Luis Blanco:
223  // The integral over all the variable space of the product of two
224  // Gaussians variables amounts to simply the evaluation of
225  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
226  // ---------------------------------------------------------------
227  // Sum of covs:
228  const auto C = cov.blockCopy<2, 2>(0, 0) + p.cov.blockCopy<2, 2>(0, 0);
229  CMatrixDouble22 C_inv = C.inverse_LLt();
230 
231  const Eigen::Vector2d MU(mean.x() - p.mean.x(), mean.y() - p.mean.y());
232 
233  return std::pow(M_2PI, -0.5 * (state_length - 1)) *
234  (1.0 / std::sqrt(C.det())) *
235  exp(-0.5 * (MU.transpose() * C_inv.asEigen() * MU)(0, 0));
236 
237  MRPT_END
238 }
239 
240 /*---------------------------------------------------------------
241  productIntegralNormalizedWith
242  ---------------------------------------------------------------*/
244  const CPointPDFGaussian& p) const
245 {
246  return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
247 }
248 
249 /*---------------------------------------------------------------
250  productIntegralNormalizedWith
251  ---------------------------------------------------------------*/
253  const CPointPDFGaussian& p) const
254 {
255  return std::exp(-0.5 * square(mahalanobisDistanceTo(p, true)));
256 }
257 
258 /*---------------------------------------------------------------
259  drawSingleSample
260  ---------------------------------------------------------------*/
262 {
263  MRPT_START
264 
265  CVectorDouble vec;
267 
268  ASSERT_(vec.size() == 3);
269  outSample.x(mean.x() + vec[0]);
270  outSample.y(mean.y() + vec[1]);
271  outSample.z(mean.z() + vec[2]);
272 
273  MRPT_END
274 }
275 
276 /*---------------------------------------------------------------
277  bayesianFusion
278  ---------------------------------------------------------------*/
280  const CPointPDF& p1_, const CPointPDF& p2_,
281  const double minMahalanobisDistToDrop)
282 {
283  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
284  MRPT_START
285 
286  // p1: CPointPDFGaussian, p2: CPosePDFGaussian:
289 
290  THROW_EXCEPTION("TODO!!!");
291 
292  MRPT_END
293 }
294 
295 /*---------------------------------------------------------------
296  mahalanobisDistanceTo
297  ---------------------------------------------------------------*/
299  const CPointPDFGaussian& other, bool only_2D) const
300 {
301  // The difference in means:
302  CMatrixDouble13 deltaX;
303  deltaX(0, 0) = other.mean.x() - mean.x();
304  deltaX(0, 1) = other.mean.y() - mean.y();
305  deltaX(0, 2) = other.mean.z() - mean.z();
306 
307  // The inverse of the combined covs:
308  CMatrixDouble33 COV = other.cov;
309  COV += this->cov;
310 
311  if (!only_2D)
312  {
313  const CMatrixDouble33 COV_inv = COV.inverse_LLt();
314  return sqrt(mrpt::math::multiply_HCHt_scalar(deltaX, COV_inv));
315  }
316  else
317  {
318  auto C = CMatrixDouble22(COV.block<2, 2>(0, 0));
319  const CMatrixDouble22 COV_inv = C.inverse_LLt();
320  auto deltaX2 = CMatrixDouble12(deltaX.block<1, 2>(0, 0));
321  return std::sqrt(mrpt::math::multiply_HCHt_scalar(deltaX2, COV_inv));
322  }
323 }
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:358
#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:275
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
T square(const T x)
Inline function for the square of a number.
#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:352
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:89
const GLubyte * c
Definition: glext.h:6406
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:349
GLsizei const GLchar ** string
Definition: glext.h:4116
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:410
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)...
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:84
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
GLuint in
Definition: glext.h:7391
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:257
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLenum GLint x
Definition: glext.h:3542
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:224
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
GLfloat GLfloat p
Definition: glext.h:6398
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019