Main MRPT website > C++ reference for MRPT 1.5.7
CPointPDFParticles.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 #include <mrpt/utils/CStream.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/math/ops_containers.h> // maximum()
17 #include <mrpt/system/os.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::utils;
22 using namespace mrpt::math;
23 using namespace mrpt::system;
24 
27 
29 {
30  setSize(numParticles);
31 }
32 
33 /** Clear all the particles (free memory) */
35  setSize(0);
36 }
37 
38 /*---------------------------------------------------------------
39  setSize
40  ---------------------------------------------------------------*/
42  size_t numberParticles,
43  const CPoint3D &defaultValue)
44 {
45  // Free old particles: automatic via smart ptr
46  m_particles.resize(numberParticles);
47  for (auto &it : m_particles)
48  {
49  it.log_w = 0;
50  it.d.reset(new TSimple3DPoint(defaultValue));
51  }
52 }
53 
54 /*---------------------------------------------------------------
55  getMean
56  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
57  ---------------------------------------------------------------*/
59 {
61  if (m_particles.empty())
62  THROW_EXCEPTION("Cannot compute mean since there are zero particles.")
63 
65  double sumW=0;
66  double x=0,y=0,z=0;
67  for (it=m_particles.begin();it!=m_particles.end();it++)
68  {
69  const double w = exp(it->log_w);
70  x+=it->d->x*w;
71  y+=it->d->y*w;
72  z+=it->d->z*w;
73  sumW+=w;
74  }
75 
76  ASSERT_(sumW!=0)
77 
78  sumW = 1.0/sumW;
79 
80  p.x( x*sumW );
81  p.y( y*sumW );
82  p.z( z*sumW );
83 
84  MRPT_END
85 }
86 
87 /*---------------------------------------------------------------
88  getEstimatedCovariance
89  ---------------------------------------------------------------*/
91 {
93 
94  getMean(mean);
95  cov.zeros();
96 
97  size_t i,n = m_particles.size();
98  double var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0;
99 
100  double lin_w_sum = 0;
101 
102  for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w );
103  if (lin_w_sum==0) lin_w_sum=1;
104 
105  for (i=0;i<n;i++)
106  {
107  double w = exp( m_particles[i].log_w ) / lin_w_sum;
108 
109  double err_x = m_particles[i].d->x - mean.x();
110  double err_y = m_particles[i].d->y - mean.y();
111  double err_phi = m_particles[i].d->z - mean.z();
112 
113  var_x+= square(err_x)*w;
114  var_y+= square(err_y)*w;
115  var_p+= square(err_phi)*w;
116  var_xy+= err_x*err_y*w;
117  var_xp+= err_x*err_phi*w;
118  var_yp+= err_y*err_phi*w;
119  }
120 
121  if (n>=2)
122  {
123  // Unbiased estimation of variance:
124  cov(0,0) = var_x;
125  cov(1,1) = var_y;
126  cov(2,2) = var_p;
127 
128  cov(1,0) = cov(0,1) = var_xy;
129  cov(2,0) = cov(0,2) = var_xp;
130  cov(1,2) = cov(2,1) = var_yp;
131  }
132 
133  MRPT_END
134 }
135 
136 /*---------------------------------------------------------------
137  writeToStream
138  ---------------------------------------------------------------*/
140 {
141  if (version)
142  *version = 0;
143  else
144  {
145  uint32_t N = size();
146  out << N;
147 
148  for (CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
149  out << it->log_w << it->d->x << it->d->y << it->d->z;
150  }
151 }
152 
153 /*---------------------------------------------------------------
154  readFromStream
155  ---------------------------------------------------------------*/
157 {
158  switch(version)
159  {
160  case 0:
161  {
162  uint32_t N;
163  in >> N;
164  setSize(N);
165 
166  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
167  in >> it->log_w >> it->d->x >> it->d->y >> it->d->z;
168  } break;
169  default:
171 
172  };
173 }
174 
176 {
177  if (this == &o) return; // It may be used sometimes
178 
179  // Convert to samples:
180  THROW_EXCEPTION("NO");
181 }
182 
183 /*---------------------------------------------------------------
184 
185  ---------------------------------------------------------------*/
187 {
188  MRPT_START
189 
190  FILE *f=os::fopen(file.c_str(),"wt");
191  if (!f) return;
192 
193  size_t i,N = m_particles.size();
194  for (i=0;i<N;i++)
195  os::fprintf(f,"%f %f %f %e\n", m_particles[i].d->x,m_particles[i].d->y,m_particles[i].d->z,m_particles[i].log_w);
196 
197  os::fclose(f);
198 
199  MRPT_END
200 }
201 
202 /*---------------------------------------------------------------
203  changeCoordinatesReference
204  ---------------------------------------------------------------*/
206 {
207  TPoint3D pt;
208  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
209  {
210  newReferenceBase.composePoint(
211  it->d->x, it->d->y, it->d->z, // In
212  pt.x, pt.y, pt.z // Out
213  );
214  it->d->x = pt.x;
215  it->d->y = pt.y;
216  it->d->z = pt.z;
217  }
218 }
219 
220 /*---------------------------------------------------------------
221  computeKurtosis
222  ---------------------------------------------------------------*/
224 {
225  MRPT_START
226 
227  // kurtosis = \mu^4 / (\sigma^2) -3
228  CVectorDouble kurts, mu4,m,var;
229  kurts.assign(3,.0);
230  mu4.assign(3,.0);
231  m.assign(3,.0);
232  var.assign(3,.0);
233 
234  // Means:
235  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
236  {
237  m[0]+=it->d->x;
238  m[1]+=it->d->y;
239  m[2]+=it->d->z;
240  }
241  m*=1.0/m_particles.size();
242 
243  // variances:
244  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
245  {
246  var[0]+=square(it->d->x-m[0]);
247  var[1]+=square(it->d->y-m[1]);
248  var[2]+=square(it->d->z-m[2]);
249  }
250  var*=1.0/m_particles.size();
251  var[0]=square(var[0]);
252  var[1]=square(var[1]);
253  var[2]=square(var[2]);
254 
255  // Moment:
256  for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
257  {
258  mu4[0]+=pow(it->d->x-m[0],4.0);
259  mu4[1]+=pow(it->d->y-m[1],4.0);
260  mu4[2]+=pow(it->d->z-m[2],4.0);
261  }
262  mu4*=1.0/m_particles.size();
263 
264  // Kurtosis's
265  kurts.array() = mu4.array() / var.array();
266 
267  return math::maximum(kurts);
268 
269  MRPT_END
270 }
271 
272 
273 /*---------------------------------------------------------------
274  drawSingleSample
275  ---------------------------------------------------------------*/
277 {
278  MRPT_UNUSED_PARAM(outSample);
279  THROW_EXCEPTION("TO DO!")
280 }
281 
282 /*---------------------------------------------------------------
283  bayesianFusion
284  ---------------------------------------------------------------*/
285 void CPointPDFParticles::bayesianFusion( const CPointPDF &p1_, const CPointPDF &p2_,const double &minMahalanobisDistToDrop )
286 {
287  MRPT_UNUSED_PARAM(p1_); MRPT_UNUSED_PARAM(p2_); MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
288  MRPT_START
289 
290  THROW_EXCEPTION("TODO!!!");
291 
292  MRPT_END
293 }
294 
295 
296 /*---------------------------------------------------------------
297  writeToStream
298  ---------------------------------------------------------------*/
300 {
302  THROW_EXCEPTION("Shouldn't arrive here!");
303 }
304 
305 /*---------------------------------------------------------------
306  readFromStream
307  ---------------------------------------------------------------*/
309 {
311  THROW_EXCEPTION("Shouldn't arrive here!");
312 }
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void getMean(CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble z
Definition: glext.h:3734
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
void drawSingleSample(CPoint3D &outSample) const MRPT_OVERRIDE
Draw a sample from the pdf.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
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)
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
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
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double z
X,Y,Z coordinates.
void copyFrom(const CPointPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:427
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
#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.
void setSize(size_t numberParticles, const CPoint3D &defaultValue=CPoint3D(0, 0, 0))
Erase all the previous particles and change the number of particles, with a given initial value...
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
Data within each particle.
int version
Definition: mrpt_jpeglib.h:898
CONTAINER::Scalar maximum(const CONTAINER &v)
GLsizei const GLchar ** string
Definition: glext.h:3919
A class used to store a 3D point.
Definition: CPoint3D.h:32
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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...
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void clear()
Clear all the particles (free memory)
GLsizeiptr size
Definition: glext.h:3779
GLenum GLint x
Definition: glext.h:3516
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:49
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:38
double computeKurtosis()
Compute the kurtosis of the distribution.
GLfloat GLfloat p
Definition: glext.h:5587
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF&#39;s particles to a text file, where each line is: X Y Z LOG_W.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.



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