Main MRPT website > C++ reference for MRPT 1.5.6
CPoint2DPDFGaussian.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/math/CMatrixD.h>
18 #include <mrpt/utils/CStream.h>
20 #include <mrpt/system/os.h>
21 
22 using namespace mrpt::poses;
23 using namespace mrpt::utils;
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace mrpt::system;
27 
29 
30 
31 /*---------------------------------------------------------------
32  Constructor
33  ---------------------------------------------------------------*/
35 {
36 }
37 
38 /*---------------------------------------------------------------
39  Constructor
40  ---------------------------------------------------------------*/
42  const CPoint2D &init_Mean,
43  const CMatrixDouble22 &init_Cov ) : mean(init_Mean), cov(init_Cov)
44 {
45 }
46 
47 /*---------------------------------------------------------------
48  Constructor
49  ---------------------------------------------------------------*/
51  const CPoint2D &init_Mean ) : mean(init_Mean), cov()
52 {
53 }
54 
55 
56 /*---------------------------------------------------------------
57  writeToStream
58  ---------------------------------------------------------------*/
60 {
61  if (version)
62  *version = 0;
63  else
64  {
65  out << CPoint2D(mean) << cov;
66  }
67 }
68 
69 /*---------------------------------------------------------------
70  readFromStream
71  ---------------------------------------------------------------*/
73 {
74  switch(version)
75  {
76  case 0:
77  {
78  in >> mean >> cov;
79  } break;
80  default:
82 
83  };
84 }
85 
86 
88 {
89  if (this == &o) return; // It may be used sometimes
90 
91  // Convert to gaussian pdf:
93 }
94 
95 /*---------------------------------------------------------------
96 
97  ---------------------------------------------------------------*/
99 {
100  MRPT_START
101 
102  FILE *f=os::fopen(file.c_str(),"wt");
103  if (!f) return;
104 
105  os::fprintf(f,"%f %f\n", mean.x(), mean.y() );
106 
107  os::fprintf(f,"%f %f\n", cov(0,0),cov(0,1) );
108  os::fprintf(f,"%f %f\n", cov(1,0),cov(1,1) );
109 
110  os::fclose(f);
111 
112  MRPT_END
113 }
114 
115 /*---------------------------------------------------------------
116  changeCoordinatesReference
117  ---------------------------------------------------------------*/
119 {
120  // Clip the 3x3 rotation matrix
121  const CMatrixDouble22 M = newReferenceBase.getRotationMatrix().block(0,0,2,2);
122 
123  // The mean:
124  mean = CPoint2D( newReferenceBase + mean );
125 
126  // The covariance:
127  cov = M*cov*M.transpose();
128 }
129 
130 /*---------------------------------------------------------------
131  bayesianFusion
132  ---------------------------------------------------------------*/
134 {
135  MRPT_START
136 
137  CMatrixDouble22 C1_inv;
138  p1.cov.inv(C1_inv);
139 
140  CMatrixDouble22 C2_inv;
141  p2.cov.inv(C2_inv);
142 
143  CMatrixDouble22 L = C1_inv;
144  L+= C2_inv;
145 
146  L.inv(cov); // The new cov.
147 
150  CMatrixDouble21 x = cov * ( C1_inv*x1 + C2_inv*x2 );
151 
152  mean.x( x.get_unsafe(0,0) );
153  mean.y( x.get_unsafe(1,0) );
154 
155  std::cout << "IN1: " << p1.mean << "\n" << p1.cov << "\n";
156  std::cout << "IN2: " << p2.mean << "\n" << p2.cov << "\n";
157  std::cout << "OUT: " << mean << "\n" << cov << "\n";
158 
159  MRPT_END
160 }
161 
162 /*---------------------------------------------------------------
163  productIntegralWith
164  ---------------------------------------------------------------*/
166 {
167  MRPT_START
168  // --------------------------------------------------------------
169  // 12/APR/2009 - Jose Luis Blanco:
170  // The integral over all the variable space of the product of two
171  // Gaussians variables amounts to simply the evaluation of
172  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
173  // ---------------------------------------------------------------
174  CMatrixDouble22 C = cov + p.cov; // Sum of covs:
175 
176  CMatrixDouble22 C_inv;
177  C.inv(C_inv);
178 
179  CMatrixDouble21 MU(UNINITIALIZED_MATRIX); // Diff. of means
180  MU.get_unsafe(0,0) = mean.x() - p.mean.x();
181  MU.get_unsafe(1,0) = mean.y() - p.mean.y();
182 
183  return std::pow( M_2PI, -0.5*state_length )
184  * (1.0/std::sqrt( C.det() ))
185  * exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
186 
187  MRPT_END
188 }
189 
190 
191 /*---------------------------------------------------------------
192  productIntegralNormalizedWith
193  ---------------------------------------------------------------*/
195 {
196  return std::exp( -0.5*square(mahalanobisDistanceTo(p)) );
197 }
198 
199 /*---------------------------------------------------------------
200  drawSingleSample
201  ---------------------------------------------------------------*/
203 {
204  MRPT_START
205 
206  // Eigen3 emits an out-of-array warning here, but it seems to be a false warning? (WTF)
207  CVectorDouble vec;
209 
210  ASSERT_(vec.size()==2);
211  outSample.x( mean.x() + vec[0] );
212  outSample.y( mean.y() + vec[1] );
213 
214  MRPT_END
215 }
216 
217 /*---------------------------------------------------------------
218  bayesianFusion
219  ---------------------------------------------------------------*/
220 void CPoint2DPDFGaussian::bayesianFusion( const CPoint2DPDF &p1_, const CPoint2DPDF &p2_,const double &minMahalanobisDistToDrop )
221 {
222  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
223  MRPT_START
224 
225  // p1: CPoint2DPDFGaussian, p2: CPosePDFGaussian:
228 
229  THROW_EXCEPTION("TODO!!!");
230 
231  MRPT_END
232 }
233 
234 /*---------------------------------------------------------------
235  mahalanobisDistanceTo
236  ---------------------------------------------------------------*/
238 {
239  // The difference in means:
240  Eigen::Matrix<double,2,1> deltaX;
241  deltaX[0] = other.mean.x() - mean.x();
242  deltaX[1] = other.mean.y() - mean.y();
243 
244  // The inverse of the combined covs:
245  return std::sqrt( deltaX.multiply_HtCH_scalar( (other.cov + this->cov).inverse() ) );
246 }
247 
248 /** Returns the Mahalanobis distance from this PDF to some point */
249 double CPoint2DPDFGaussian::mahalanobisDistanceToPoint( const double x, const double y ) const
250 {
251  // The difference in means:
252  Eigen::Matrix<double,2,1> deltaX;
253  deltaX[0] = x - mean.x();
254  deltaX[1] = y - mean.y();
255 
256  // The inverse of the combined covs:
257  return std::sqrt( deltaX.multiply_HtCH_scalar( this->cov.inverse() ) );
258 }
259 
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
A gaussian distribution for 2D points.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
double productIntegralNormalizedWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
CMatrixFixedNumeric< double, 2, 1 > CMatrixDouble21
Definition: eigen_frwds.h:55
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLuint in
Definition: glew.h:7146
#define M_2PI
Definition: mrpt_macros.h:380
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:135
CPoint2DPDFGaussian()
Default constructor.
void bayesianFusion(const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2)
Bayesian fusion of two points gauss.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class that represents a Probability Distribution function (PDF) of a 2D point (x...
Definition: CPoint2DPDF.h:35
void drawSingleSample(CPoint2D &outSample) const MRPT_OVERRIDE
Draw a sample from the pdf.
void copyFrom(const CPoint2DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
A class used to store a 2D point.
Definition: CPoint2D.h:36
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
#define MRPT_START
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This file implements matrix/vector text and binary serialization.
#define ASSERT_(f)
static const 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)...
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=NULL)
Generate multidimensional random samples according to a given covariance matrix.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018