MRPT  1.9.9
CPoint2DPDFGaussian.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/CMatrixD.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/CPose3D.h>
20 #include <mrpt/system/os.h>
21 #include <Eigen/Dense>
22 
23 using namespace mrpt::poses;
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 CPoint2D& init_Mean, const CMatrixDouble22& init_Cov)
39  : mean(init_Mean), cov(init_Cov)
40 {
41 }
42 
43 /*---------------------------------------------------------------
44  Constructor
45  ---------------------------------------------------------------*/
47  : mean(init_Mean), cov()
48 {
49 }
50 
51 uint8_t CPoint2DPDFGaussian::serializeGetVersion() const { return 0; }
53 {
54  out << CPoint2D(mean) << cov;
55 }
57  mrpt::serialization::CArchive& in, uint8_t version)
58 {
59  switch (version)
60  {
61  case 0:
62  {
63  in >> mean >> cov;
64  }
65  break;
66  default:
68  };
69 }
72 {
74  out["mean"] = mean;
75  out["cov"] = CMatrixD(cov);
76 }
79 {
80  uint8_t version;
82  switch (version)
83  {
84  case 1:
85  {
86  in["mean"].readTo(mean);
87  CMatrixD m;
88  in["cov"].readTo(m);
89  cov = m;
90  }
91  break;
92  default:
94  }
95 }
96 
98 {
99  if (this == &o) return; // It may be used sometimes
100 
101  // Convert to gaussian pdf:
103 }
104 
105 /*---------------------------------------------------------------
106 
107  ---------------------------------------------------------------*/
108 bool CPoint2DPDFGaussian::saveToTextFile(const std::string& file) const
109 {
110  MRPT_START
111 
112  FILE* f = os::fopen(file.c_str(), "wt");
113  if (!f) return false;
114 
115  os::fprintf(f, "%f %f\n", mean.x(), mean.y());
116 
117  os::fprintf(f, "%f %f\n", cov(0, 0), cov(0, 1));
118  os::fprintf(f, "%f %f\n", cov(1, 0), cov(1, 1));
119 
120  os::fclose(f);
121  return true;
122  MRPT_END
123 }
124 
125 /*---------------------------------------------------------------
126  changeCoordinatesReference
127  ---------------------------------------------------------------*/
129  const CPose3D& newReferenceBase)
130 {
131  // Clip the 3x3 rotation matrix
132  const auto M = newReferenceBase.getRotationMatrix().blockCopy<2, 2>();
133 
134  // The mean:
135  mean = CPoint2D(newReferenceBase + mean);
136 
137  // The covariance:
138  cov = M.asEigen() * cov.asEigen() * M.transpose();
139 }
140 
141 /*---------------------------------------------------------------
142  bayesianFusion
143  ---------------------------------------------------------------*/
145  const CPoint2DPDFGaussian& p1, const CPoint2DPDFGaussian& p2)
146 {
147  MRPT_START
148 
149  const auto C1_inv = p1.cov.asEigen().inverse();
150  const auto C2_inv = p2.cov.asEigen().inverse();
151 
152  const Eigen::Matrix2d L = C1_inv + C2_inv;
153 
154  cov.asEigen() = L.inverse(); // The new cov.
155 
156  const Eigen::Vector2d x1{p1.mean.x(), p1.mean.y()};
157  const Eigen::Vector2d x2{p2.mean.x(), p2.mean.y()};
158 
159  const Eigen::Vector2d x = cov.asEigen() * (C1_inv * x1 + C2_inv * x2);
160  mean.x(x[0]);
161  mean.y(x[1]);
162 
163  MRPT_END
164 }
165 
166 /*---------------------------------------------------------------
167  productIntegralWith
168  ---------------------------------------------------------------*/
170  const CPoint2DPDFGaussian& p) const
171 {
172  MRPT_START
173  // --------------------------------------------------------------
174  // 12/APR/2009 - Jose Luis Blanco:
175  // The integral over all the variable space of the product of two
176  // Gaussians variables amounts to simply the evaluation of
177  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
178  // ---------------------------------------------------------------
179  // Sum of covs:
180  const auto C = cov.asEigen() + p.cov.asEigen();
181  const auto C_inv = C.inverse();
182 
183  const Eigen::Vector2d MU{mean.x() - p.mean.x(), mean.y() - p.mean.y()};
184 
185  return std::pow(M_2PI, -0.5 * state_length) *
186  (1.0 / std::sqrt(C.determinant())) *
187  exp(-0.5 * MU.transpose() * C_inv * MU);
188 
189  MRPT_END
190 }
191 
192 /*---------------------------------------------------------------
193  productIntegralNormalizedWith
194  ---------------------------------------------------------------*/
196  const CPoint2DPDFGaussian& p) const
197 {
198  return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
199 }
200 
201 /*---------------------------------------------------------------
202  drawSingleSample
203  ---------------------------------------------------------------*/
205 {
206  MRPT_START
207 
208  // Eigen3 emits an out-of-array warning here, but it seems to be a false
209  // warning? (WTF)
210  CVectorDouble vec;
212 
213  ASSERT_(vec.size() == 2);
214  outSample.x(mean.x() + vec[0]);
215  outSample.y(mean.y() + vec[1]);
216 
217  MRPT_END
218 }
219 
220 /*---------------------------------------------------------------
221  bayesianFusion
222  ---------------------------------------------------------------*/
224  const CPoint2DPDF& p1_, const CPoint2DPDF& p2_,
225  [[maybe_unused]] const double minMahalanobisDistToDrop)
226 {
227  MRPT_START
228 
229  // p1: CPoint2DPDFGaussian, p2: CPosePDFGaussian:
232 
233  THROW_EXCEPTION("TODO!!!");
234 
235  MRPT_END
236 }
237 
238 /*---------------------------------------------------------------
239  mahalanobisDistanceTo
240  ---------------------------------------------------------------*/
242  const CPoint2DPDFGaussian& other) const
243 {
244  // The difference in means:
245  const Eigen::Vector2d deltaX{other.mean.x() - mean.x(),
246  other.mean.y() - mean.y()};
247 
248  // The inverse of the combined covs:
249  return std::sqrt(
250  deltaX.transpose() *
251  (other.cov.asEigen() + this->cov.asEigen()).inverse() * deltaX);
252 }
253 
254 /** Returns the Mahalanobis distance from this PDF to some point */
256  const double x, const double y) const
257 {
258  // The difference in means:
259  const Eigen::Vector2d deltaX{x - mean.x(), y - mean.y()};
260 
261  // The inverse of the combined covs:
262  return std::sqrt(deltaX.transpose() * cov.asEigen().inverse() * deltaX);
263 }
A namespace of pseudo-random numbers generators of diferent distributions.
double productIntegralNormalizedWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
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
A gaussian distribution for 2D points.
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.
void copyFrom(const CPoint2DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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 drawSingleSample(CPoint2D &outSample) const override
Draw a sample from the pdf.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
CPoint2DPDFGaussian()
Default constructor.
void bayesianFusion(const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2)
Bayesian fusion of two points gauss.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Declares a class that represents a Probability Distribution function (PDF) of a 2D point (x...
Definition: CPoint2DPDF.h:33
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
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
A class used to store a 2D point.
Definition: CPoint2D.h:32
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:408
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.
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 serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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
This file implements matrix/vector text and binary serialization.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_END
Definition: exceptions.h:245
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.
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.
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020