Main MRPT website > C++ reference for MRPT 1.5.6
CPointPDFGaussian.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>
17 #include <mrpt/system/os.h>
18 #include <mrpt/utils/CStream.h>
19 
20 using namespace mrpt::poses;
21 using namespace mrpt::utils;
22 using namespace mrpt::math;
23 using namespace mrpt::random;
24 using namespace mrpt::system;
25 
27 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33 {
34 
35 }
36 
37 /*---------------------------------------------------------------
38  Constructor
39  ---------------------------------------------------------------*/
41  const CPoint3D &init_Mean,
42  const CMatrixDouble33 &init_Cov ) : mean(init_Mean), cov(init_Cov)
43 {
44 
45 }
46 
47 /*---------------------------------------------------------------
48  Constructor
49  ---------------------------------------------------------------*/
51  const CPoint3D &init_Mean ) : mean(init_Mean), cov()
52 {
53  cov.zeros();
54 }
55 
56 
57 /*---------------------------------------------------------------
58  getMean
59  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
60  ---------------------------------------------------------------*/
62 {
63  p=mean;
64 }
65 
66 /*---------------------------------------------------------------
67  getCovarianceAndMean
68  ---------------------------------------------------------------*/
70 {
71  p=mean;
72  C=cov;
73 }
74 
75 /*---------------------------------------------------------------
76  writeToStream
77  ---------------------------------------------------------------*/
79 {
80  if (version)
81  *version = 1;
82  else
83  {
84  out << CPoint3D(mean) << cov;
85  }
86 }
87 
88 /*---------------------------------------------------------------
89  readFromStream
90  ---------------------------------------------------------------*/
92 {
93  switch(version)
94  {
95  case 0:
96  {
97  in >> mean;
98 
99  CMatrix c;
100  in >> c; cov = c.cast<double>();
101  } break;
102  case 1:
103  {
104  in >> mean >> cov;
105  } break;
106  default:
108 
109  };
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  ---------------------------------------------------------------*/
125 {
126  MRPT_START
127 
128  FILE *f=os::fopen(file.c_str(),"wt");
129  if (!f) return;
130 
131  os::fprintf(f,"%f %f %f\n", mean.x(), mean.y(), mean.z() );
132 
133  os::fprintf(f,"%f %f %f\n", cov(0,0),cov(0,1),cov(0,2) );
134  os::fprintf(f,"%f %f %f\n", cov(1,0),cov(1,1),cov(1,2) );
135  os::fprintf(f,"%f %f %f\n", cov(2,0),cov(2,1),cov(2,2) );
136 
137 
138  os::fclose(f);
139 
140  MRPT_END
141 }
142 
143 /*---------------------------------------------------------------
144  changeCoordinatesReference
145  ---------------------------------------------------------------*/
147 {
148  const CMatrixDouble33 &M = newReferenceBase.getRotationMatrix();
149 
150  // The mean:
151  mean = newReferenceBase + mean;
152 
153  // The covariance:
154  M.multiply_HCHt(CMatrixDouble33(cov), cov); // save in cov
155 }
156 
157 /*---------------------------------------------------------------
158  bayesianFusion
159  ---------------------------------------------------------------*/
161 {
162  MRPT_START
163 
164  CMatrixDouble x1(3,1),x2(3,1),x(3,1);
165  CMatrixDouble C1( p1.cov );
166  CMatrixDouble C2( p2.cov );
167  CMatrixDouble C1_inv = C1.inv();
168  CMatrixDouble C2_inv = C2.inv();
169  CMatrixDouble C;
170 
171  x1(0,0) = p1.mean.x(); x1(1,0) = p1.mean.y(); x1(2,0) = p1.mean.z();
172  x2(0,0) = p2.mean.x(); x2(1,0) = p2.mean.y(); x2(2,0) = p2.mean.z();
173 
174 
175  C = (C1_inv + C2_inv).inv();
176  cov = C;
177 
178  x = C * ( C1_inv*x1 + C2_inv*x2 );
179 
180  mean.x( x(0,0) );
181  mean.y( x(1,0) );
182  mean.z( x(2,0) );
183 
184 // std::cout << "IN1: " << x1 << "\n" << C1 << "\n";
185 // std::cout << "IN2: " << x2 << "\n" << C2 << "\n";
186 // std::cout << "OUT: " << x << "\n" << C << "\n";
187 
188  MRPT_END
189 }
190 
191 /*---------------------------------------------------------------
192  productIntegralWith
193  ---------------------------------------------------------------*/
195 {
196  MRPT_START
197 
198  // --------------------------------------------------------------
199  // 12/APR/2009 - Jose Luis Blanco:
200  // The integral over all the variable space of the product of two
201  // Gaussians variables amounts to simply the evaluation of
202  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
203  // ---------------------------------------------------------------
204  CMatrixDouble33 C = cov; C+=p.cov; // Sum of covs:
205  CMatrixDouble33 C_inv;
206  C.inv(C_inv);
207 
208  CMatrixDouble31 MU(UNINITIALIZED_MATRIX); // Diff. of means
209  MU.get_unsafe(0,0) = mean.x() - p.mean.x();
210  MU.get_unsafe(1,0) = mean.y() - p.mean.y();
211  MU.get_unsafe(2,0) = mean.z() - p.mean.z();
212 
213  return std::pow( M_2PI, -0.5*state_length )
214  * (1.0/std::sqrt( C.det() ))
215  * exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
216 
217  MRPT_END
218 }
219 
220 /*---------------------------------------------------------------
221  productIntegralWith2D
222  ---------------------------------------------------------------*/
224 {
225  MRPT_START
226 
227  // --------------------------------------------------------------
228  // 12/APR/2009 - Jose Luis Blanco:
229  // The integral over all the variable space of the product of two
230  // Gaussians variables amounts to simply the evaluation of
231  // a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
232  // ---------------------------------------------------------------
233  CMatrixDouble22 C = cov.block(0,0,2,2);
234  C+=p.cov.block(0,0,2,2); // Sum of covs:
235 
236  CMatrixDouble22 C_inv;
237  C.inv(C_inv);
238 
239  CMatrixDouble21 MU(UNINITIALIZED_MATRIX); // Diff. of means
240  MU.get_unsafe(0,0) = mean.x() - p.mean.x();
241  MU.get_unsafe(1,0) = mean.y() - p.mean.y();
242 
243  return std::pow( M_2PI, -0.5*(state_length-1) )
244  * (1.0/std::sqrt( C.det() ))
245  * exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
246 
247  MRPT_END
248 }
249 
250 /*---------------------------------------------------------------
251  productIntegralNormalizedWith
252  ---------------------------------------------------------------*/
254 {
255  return std::exp( -0.5*square(mahalanobisDistanceTo(p)) );
256 }
257 
258 /*---------------------------------------------------------------
259  productIntegralNormalizedWith
260  ---------------------------------------------------------------*/
262 {
263  return std::exp( -0.5*square(mahalanobisDistanceTo(p,true)) );
264 }
265 
266 /*---------------------------------------------------------------
267  drawSingleSample
268  ---------------------------------------------------------------*/
270 {
271  MRPT_START
272 
273  CVectorDouble vec;
275 
276  ASSERT_(vec.size()==3);
277  outSample.x( mean.x() + vec[0] );
278  outSample.y( mean.y() + vec[1] );
279  outSample.z( mean.z() + vec[2] );
280 
281  MRPT_END
282 }
283 
284 /*---------------------------------------------------------------
285  bayesianFusion
286  ---------------------------------------------------------------*/
287 void CPointPDFGaussian::bayesianFusion( const CPointPDF &p1_, const CPointPDF &p2_,const double &minMahalanobisDistToDrop )
288 {
289  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
290  MRPT_START
291 
292  // p1: CPointPDFGaussian, p2: CPosePDFGaussian:
295 
296  THROW_EXCEPTION("TODO!!!");
297 
298  MRPT_END
299 }
300 
301 /*---------------------------------------------------------------
302  mahalanobisDistanceTo
303  ---------------------------------------------------------------*/
304 double CPointPDFGaussian::mahalanobisDistanceTo( const CPointPDFGaussian & other, bool only_2D ) const
305 {
306  // The difference in means:
307  CMatrixDouble13 deltaX;
308  deltaX.get_unsafe(0,0) = other.mean.x() - mean.x();
309  deltaX.get_unsafe(0,1) = other.mean.y() - mean.y();
310  deltaX.get_unsafe(0,2) = other.mean.z() - mean.z();
311 
312  // The inverse of the combined covs:
313  CMatrixDouble33 COV = other.cov;
314  COV += this->cov;
315 
316  if (!only_2D)
317  {
318  CMatrixDouble33 COV_inv;
319  COV.inv(COV_inv);
320  return sqrt( deltaX.multiply_HCHt_scalar(COV_inv) );
321  }
322  else
323  {
324  CMatrixDouble22 C = COV.block(0,0,2,2);
325  CMatrixDouble22 COV_inv;
326  C.inv(COV_inv);
327  CMatrixDouble12 deltaX2 = deltaX.block(0,0,1,2);
328  return std::sqrt( deltaX2.multiply_HCHt_scalar(COV_inv) );
329  }
330 
331 
332 }
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
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...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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
void copyFrom(const CPointPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
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 bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
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...
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
EIGEN_STRONG_INLINE PlainObject inv() const
A class used to store a 3D point.
Definition: CPoint3D.h:32
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
#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
void getMean(CPoint3D &p) const MRPT_OVERRIDE
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
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)
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...
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...
void drawSingleSample(CPoint3D &outSample) const MRPT_OVERRIDE
Draw a sample from the pdf.
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.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:38
double productIntegralNormalizedWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
CPointPDFGaussian()
Default constructor.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
A gaussian distribution for 3D points.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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