Main MRPT website > C++ reference for MRPT 1.5.7
CPosePDFParticles.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 
14 #include <mrpt/system/os.h>
15 #include <mrpt/random.h>
16 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/utils/CStream.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::bayes;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace mrpt::utils;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
32 /*---------------------------------------------------------------
33  Constructor
34  ---------------------------------------------------------------*/
36 {
37  m_particles.resize(M);
38 
39  for (auto & p : m_particles)
40  {
41  p.log_w = .0;
42  p.d.reset(new CPose2D());
43  }
44 
45  CPose2D nullPose(0,0,0);
46  resetDeterministic( nullPose );
47 }
48 
49 void CPosePDFParticles::copyFrom(const CPosePDF &o)
50 {
52 
55 
56  if (this == &o) return; // It may be used sometimes
57 
59  {
60  const CPosePDFParticles *pdf = dynamic_cast<const CPosePDFParticles*>( &o );
61  ASSERT_(pdf);
62 
63  // Both are m_particles:
64  m_particles = pdf->m_particles;
65  }
66  else
68  {
69  const CPosePDFGaussian *pdf = static_cast<const CPosePDFGaussian*>( &o );
70  size_t M = m_particles.size();
71  std::vector<CVectorDouble> parts;
73 
75 
76  clearParticles();
77  m_particles.resize(M);
78 
79  for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();++itDest,++partsIt )
80  {
81  itDest->log_w = 0;
82  itDest->d.reset(new CPose2D(
83  (pdf->mean.x() + (*partsIt)[0]),
84  (pdf->mean.y() + (*partsIt)[1]),
85  (pdf->mean.phi() + (*partsIt)[2]))
86  );
87  itDest->d->normalizePhi();
88  }
89 
90  }
91 
92  MRPT_END
93 }
94 
96 {
97  clearParticles();
98 }
99 
100 /*---------------------------------------------------------------
101  getMean
102  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
103  as a weighted average over all m_particles.
104  ---------------------------------------------------------------*/
105 void CPosePDFParticles::getMean(CPose2D &est_) const
106 {
107  // Calc average on SE(2)
108  const size_t n = m_particles.size();
109  if (n)
110  {
111  mrpt::poses::SE_average<2> se_averager;
112  for (size_t i=0;i<n;i++)
113  {
114  const CPose2D &p = *m_particles[i].d;
115  double w = exp(m_particles[i].log_w);
116  se_averager.append(p,w);
117  }
118  se_averager.get_average(est_);
119  }
120  else
121  {
122  est_ = CPose2D();
123  }
124 }
125 
126 /*---------------------------------------------------------------
127  getEstimatedCovariance
128  ---------------------------------------------------------------*/
129 void CPosePDFParticles::getCovarianceAndMean(CMatrixDouble33 &cov, CPose2D &mean) const
130 {
131  cov.zeros();
132  getMean(mean);
133 
134  size_t i,n = m_particles.size();
135  double var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0;
136  double mean_phi = mean.phi();
137 
138  if (mean_phi<0) mean_phi = M_2PI + mean_phi;
139 
140  double lin_w_sum = 0;
141 
142  for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w );
143  if (lin_w_sum==0) lin_w_sum=1;
144 
145  for (i=0;i<n;i++)
146  {
147  double w = exp( m_particles[i].log_w ) / lin_w_sum;
148 
149  // Manage 1 PI range:
150  double err_x = m_particles[i].d->x() - mean.x();
151  double err_y = m_particles[i].d->y() - mean.y();
152  double err_phi = math::wrapToPi( fabs(m_particles[i].d->phi() - mean_phi) );
153 
154  var_x+= square(err_x)*w;
155  var_y+= square(err_y)*w;
156  var_p+= square(err_phi)*w;
157  var_xy+= err_x*err_y*w;
158  var_xp+= err_x*err_phi*w;
159  var_yp+= err_y*err_phi*w;
160  }
161 
162  if (n<2)
163  {
164  // Not enought information to estimate the variance:
165  }
166  else
167  {
168  // Unbiased estimation of variance:
169  cov(0,0) = var_x;
170  cov(1,1) = var_y;
171  cov(2,2) = var_p;
172 
173  cov(1,0) = cov(0,1) = var_xy;
174  cov(2,0) = cov(0,2) = var_xp;
175  cov(1,2) = cov(2,1) = var_yp;
176 
177  }
178 }
179 
180 
181 /*---------------------------------------------------------------
182  writeToStream
183  ---------------------------------------------------------------*/
184 void CPosePDFParticles::writeToStream(mrpt::utils::CStream &out,int *version) const
185 {
186  if (version)
187  *version = 0;
188  else
189  {
190  writeParticlesToStream( out );
191  }
192 }
193 
194 /*---------------------------------------------------------------
195  readFromStream
196  ---------------------------------------------------------------*/
197 void CPosePDFParticles::readFromStream(mrpt::utils::CStream &in, int version)
198 {
199  switch(version)
200  {
201  case 0:
202  {
203  readParticlesFromStream( in );
204  } break;
205  default:
207 
208  };
209 }
210 
211 
212 /*---------------------------------------------------------------
213  resetDeterministic
214  Reset PDF to a single point and set the number of m_particles
215  ---------------------------------------------------------------*/
216 void CPosePDFParticles::resetDeterministic( const CPose2D &location,
217  size_t particlesCount)
218 {
219  if (particlesCount>0)
220  {
221  clear();
222  m_particles.resize(particlesCount);
223  for (auto &p: m_particles)
224  p.d.resetDefaultCtor();
225  }
226 
227  for (auto &p: m_particles)
228  {
229  *p.d = location;
230  p.log_w = .0;
231  }
232 }
233 
234 /*---------------------------------------------------------------
235  resetUniform
236  ---------------------------------------------------------------*/
237 void CPosePDFParticles::resetUniform(
238  const double & x_min,
239  const double & x_max,
240  const double & y_min,
241  const double & y_max,
242  const double & phi_min,
243  const double & phi_max,
244  const int &particlesCount)
245 {
246  MRPT_START
247 
248  if (particlesCount>0)
249  {
250  clear();
251  m_particles.resize(particlesCount);
252  for (int i = 0; i < particlesCount; i++)
253  m_particles[i].d.reset(new CPose2D());
254  }
255 
256  size_t i,M = m_particles.size();
257  for (i=0;i<M;i++)
258  {
259  m_particles[i].d->x( randomGenerator.drawUniform( x_min, x_max ) );
260  m_particles[i].d->y( randomGenerator.drawUniform( y_min, y_max ) );
261  m_particles[i].d->phi( randomGenerator.drawUniform( phi_min, phi_max ) );
262  m_particles[i].log_w=0;
263  }
264 
265  MRPT_END
266 }
267 
268 void CPosePDFParticles::resetAroundSetOfPoses(
269  const std::vector<mrpt::math::TPose2D> & list_poses,
270  const size_t num_particles_per_pose,
271  const double spread_x,
272  const double spread_y,
273  const double spread_phi_rad)
274 {
275  MRPT_START
276  ASSERT_(!list_poses.empty());
277  ASSERT_(num_particles_per_pose>=1);
278 
279  const size_t N = list_poses.size() * num_particles_per_pose;
280 
281  clear();
282  m_particles.resize(N);
283  size_t i,nSpot;
284  for (i=0,nSpot=0;nSpot<list_poses.size();nSpot++)
285  {
286  const mrpt::math::TPose2D & p = list_poses[nSpot];
287  for (size_t k=0;k<num_particles_per_pose;k++,i++)
288  {
289  m_particles[i].d.reset(new CPose2D());
290  m_particles[i].d->x( randomGenerator.drawUniform( p.x - spread_x*0.5, p.x + spread_x*0.5 ) );
291  m_particles[i].d->y( randomGenerator.drawUniform( p.y - spread_y*0.5, p.y + spread_y*0.5 ) );
292  m_particles[i].d->phi( randomGenerator.drawUniform( p.phi - spread_phi_rad*0.5, p.phi + spread_phi_rad*0.5 ) );
293  m_particles[i].log_w=0;
294  }
295  }
296 
297  ASSERT_EQUAL_(i,N);
298 
299  MRPT_END
300 }
301 
302 /*---------------------------------------------------------------
303  saveToTextFile
304  Save PDF's m_particles to a text file. In each line it
305  will go: "x y phi weight"
306  ---------------------------------------------------------------*/
308 {
309  FILE *f=os::fopen(file.c_str(),"wt");
310  if (!f) return;
311  os::fprintf(f,"%% x y yaw[rad] log_weight\n");
312 
313  for (unsigned int i=0;i<m_particles.size();i++)
314  os::fprintf(f,"%f %f %f %e\n",
315  m_particles[i].d->x(),
316  m_particles[i].d->y(),
317  m_particles[i].d->phi(),
318  m_particles[i].log_w );
319 
320  os::fclose(f);
321 }
322 
323 /*---------------------------------------------------------------
324  getParticlePose
325  ---------------------------------------------------------------*/
326 CPose2D CPosePDFParticles::getParticlePose(size_t i) const
327 {
328  return *m_particles[i].d;
329 }
330 
331 /*---------------------------------------------------------------
332  changeCoordinatesReference
333  ---------------------------------------------------------------*/
334 void CPosePDFParticles::changeCoordinatesReference(const CPose3D &newReferenceBase_ )
335 {
336  const CPose2D newReferenceBase = CPose2D(newReferenceBase_);
337 
338  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
339  it->d->composeFrom(newReferenceBase, *it->d);
340 }
341 
342 /*---------------------------------------------------------------
343  drawSingleSample
344  ---------------------------------------------------------------*/
345 void CPosePDFParticles::drawSingleSample( CPose2D &outPart ) const
346 {
347  const double uni = randomGenerator.drawUniform(0.0,0.9999);
348  double cum = 0;
349 
350  for (CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
351  {
352  cum+= exp(it->log_w);
353  if ( uni<= cum )
354  {
355  outPart= (*it->d);
356  return;
357  }
358  }
359 
360  // Might not come here normally:
361  outPart = *(m_particles.rbegin())->d;
362 }
363 
364 /*---------------------------------------------------------------
365  +=
366  ---------------------------------------------------------------*/
368 {
369  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
370  it->d->composeFrom(*it->d, Ap);
371 }
372 
373 /*---------------------------------------------------------------
374  append
375  ---------------------------------------------------------------*/
376 void CPosePDFParticles::append( CPosePDFParticles &o )
377 {
378  for (unsigned int i=0;i<o.m_particles.size();i++)
379  {
380  CParticleData part;
381  part.d.reset(new CPose2D(*o.m_particles[i].d));
382  part.log_w = o.m_particles[i].log_w;
383  m_particles.push_back( part );
384  }
385 
386  normalizeWeights();
387 }
388 
389 /*---------------------------------------------------------------
390  inverse
391  ---------------------------------------------------------------*/
393 {
394  MRPT_START
396  CPosePDFParticles *out = static_cast<CPosePDFParticles*>( &o );
397 
398 
399  out->copyFrom( *this );
400  static CPose2D nullPose(0,0,0);
401 
402  for (unsigned int i=0;i<out->m_particles.size();i++)
403  (*out->m_particles[i].d) = nullPose - (*out->m_particles[i].d);
404 
405  MRPT_END
406 }
407 
408 /*---------------------------------------------------------------
409  getMostLikelyParticle
410  ---------------------------------------------------------------*/
411 CPose2D CPosePDFParticles::getMostLikelyParticle() const
412 {
413  CParticleList::const_iterator it, itMax=m_particles.begin();
414  double max_w = -1e300;
415 
416 
417  for (it=m_particles.begin();it!=m_particles.end();++it)
418  {
419  if (it->log_w > max_w)
420  {
421  itMax = it;
422  max_w = it->log_w;
423  }
424  }
425 
426  return *itMax->d;
427 }
428 
429 /*---------------------------------------------------------------
430  bayesianFusion
431  ---------------------------------------------------------------*/
432 void CPosePDFParticles::bayesianFusion(const CPosePDF &p1,const CPosePDF &p2, const double &minMahalanobisDistToDrop )
433 {
434  MRPT_UNUSED_PARAM(p1);MRPT_UNUSED_PARAM(p2);MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
435 
436  THROW_EXCEPTION("Not implemented yet!");
437 }
438 
439 /*---------------------------------------------------------------
440  evaluatePDF_parzen
441  ---------------------------------------------------------------*/
442 double CPosePDFParticles::evaluatePDF_parzen(
443  const double & x,
444  const double & y,
445  const double & phi,
446  const double & stdXY,
447  const double & stdPhi ) const
448 {
449  double ret = 0;
450 
451  for (CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
452  {
453  double difPhi = math::wrapToPi( phi - it->d->phi() );
454 
455  ret += exp(it->log_w) *
456  math::normalPDF( it->d->distance2DTo(x,y), 0, stdXY ) *
457  math::normalPDF( fabs( difPhi ), 0, stdPhi );
458  }
459 
460  return ret;
461 }
462 
463 /*---------------------------------------------------------------
464  saveParzenPDFToTextFile
465  ---------------------------------------------------------------*/
466 void CPosePDFParticles::saveParzenPDFToTextFile(
467  const char *fileName,
468  const double & x_min,
469  const double & x_max,
470  const double & y_min,
471  const double & y_max,
472  const double & phi,
473  const double & stepSizeXY,
474  const double & stdXY,
475  const double & stdPhi ) const
476 {
477  FILE *f=os::fopen(fileName,"wt");
478  if (!f) return;
479 
480  for (double y=y_min; y<y_max; y+=stepSizeXY)
481  {
482  for (double x=x_min; x<x_max; x+=stepSizeXY)
483  {
484  os::fprintf(f,"%f ",
485  evaluatePDF_parzen(x,y,phi,stdXY,stdPhi) );
486  } // y
487  os::fprintf(f,"\n");
488  } // x
489 
490  os::fclose(f);
491 }
#define ASSERT_EQUAL_(__A, __B)
Computes weighted and un-weighted averages of SE(2) poses.
Definition: SO_SE_average.h:88
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
GLint location
Definition: glext.h:3910
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
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 saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=NULL)
Generate a given number of multidimensional random samples according to a given covariance matrix...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
#define M_2PI
Definition: mrpt_macros.h:380
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CParticleList m_particles
The array of particles.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#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.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
int version
Definition: mrpt_jpeglib.h:898
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A template class for holding a the data and the weight of a particle.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
double log_w
The (logarithmic) weight value for this particle.
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
mrpt::utils::copy_ptr< T > d
The data associated with this particle. The use of copy_ptr<> allows relying on compiler-generated co...
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516
double BASE_IMPEXP normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:908
GLfloat GLfloat p
Definition: glext.h:5587
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019