MRPT  2.0.4
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 }
mrpt::math::CMatrixDouble22
CMatrixFixed< double, 2, 2 > CMatrixDouble22
Definition: CMatrixFixed.h:364
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::CPointPDFGaussian::copyFrom
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPointPDFGaussian.cpp:113
mrpt::math::CProbabilityDensityFunction< CPoint3D, 3 >::state_length
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).
Definition: CProbabilityDensityFunction.h:32
poses-precomp.h
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
mrpt::math::CMatrixFixed::cast_double
CMatrixFixed< double, ROWS, COLS > cast_double() const
Definition: CMatrixFixed_impl.h:25
matrix_serialization.h
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:44
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::serialization::CSchemeArchiveBase
Virtual base class for "schematic archives" (JSON, XML,...)
Definition: CSchemeArchiveBase.h:75
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:22
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::poses::CPointPDFGaussian::productIntegralNormalizedWith2D
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:251
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
mrpt::poses::CPointPDF::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::poses::CPointPDFGaussian::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPointPDFGaussian.cpp:58
SCHEMA_DESERIALIZE_DATATYPE_VERSION
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
Definition: CSerializable.h:139
mrpt::poses::CPointPDFGaussian::productIntegralWith2D
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:215
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
mrpt::poses::CPointPDFGaussian::saveToTextFile
bool saveToTextFile(const std::string &file) const override
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
Definition: CPointPDFGaussian.cpp:124
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::serialization::CSchemeArchiveBase::readTo
void readTo(CSerializable &obj)
Definition: CSchemeArchiveBase.h:140
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::math::MatrixBase::inverse_LLt
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
Definition: MatrixBase_impl.h:195
mrpt::math::CMatrixF
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
mrpt::poses::CPointPDFGaussian::bayesianFusion
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
Definition: CPointPDFGaussian.cpp:156
mrpt::poses::CPointPDFGaussian::drawSingleSample
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
Definition: CPointPDFGaussian.cpp:260
mrpt::math::CMatrixFixed< double, 3, 3 >
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
CPointPDFGaussian.h
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
ops_matrices.h
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::math::MatrixVectorBase::block
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
Definition: MatrixVectorBase.h:138
mrpt::poses::CPointPDFGaussian::getMean
void getMean(CPoint3D &p) const override
Definition: CPointPDFGaussian.cpp:56
CSchemeArchiveBase.h
mrpt::poses::CPointPDFGaussian::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPointPDFGaussian.cpp:59
mrpt::math::CMatrixDouble12
CMatrixFixed< double, 1, 2 > CMatrixDouble12
Definition: CMatrixFixed.h:375
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::poses::CPointPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPointPDFGaussian.cpp:141
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
mrpt::poses::CPointPDFGaussian::productIntegralWith
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:189
mrpt::math::MatrixVectorBase::transpose
auto transpose()
Definition: MatrixVectorBase.h:159
mrpt::poses::CPointPDFGaussian::productIntegralNormalizedWith
double productIntegralNormalizedWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:242
mrpt::math::CVectorDynamic< double >
CPose3D.h
RandomGenerators.h
mrpt::math::MatrixBase::blockCopy
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
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::poses::CPointPDFGaussian::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPointPDFGaussian.cpp:63
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
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::math::MatrixVectorBase::setZero
void setZero()
Definition: MatrixVectorBase.h:112
mrpt::math::multiply_HCHt_scalar
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
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
M_2PI
#define M_2PI
Definition: common.h:58
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::poses::CPointPDF
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x,...
Definition: CPointPDF.h:36
CArchive.h
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:42
mrpt::poses::CPointPDFGaussian::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPointPDFGaussian &other, bool only_2D=false) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0,...
Definition: CPointPDFGaussian.cpp:296
mrpt::math::CMatrixDouble33
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
mrpt::poses::CPointPDFGaussian::CPointPDFGaussian
CPointPDFGaussian()
Default constructor.
Definition: CPointPDFGaussian.cpp:32
mrpt::system
Definition: backtrace.h:14
SCHEMA_SERIALIZE_DATATYPE_VERSION
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
Definition: CSerializable.h:131



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020