Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 "poses-precomp.h" // Precompiled headers
11 
14 #include <mrpt/system/os.h>
15 #include <mrpt/random.h>
16 #include <mrpt/math/wrap2pi.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::system;
27 using namespace std;
28 
30 
32 {
33  m_particles.resize(M);
34  for (auto& p : m_particles)
35  {
36  p.log_w = .0;
37  p.d = TPose2D();
38  }
39  TPose2D nullPose(0, 0, 0);
40  resetDeterministic(nullPose);
41 }
42 
43 void CPosePDFParticles::copyFrom(const CPosePDF& o)
44 {
46 
49 
50  if (this == &o) return; // It may be used sometimes
51 
53  {
54  const CPosePDFParticles* pdf =
55  dynamic_cast<const CPosePDFParticles*>(&o);
56  ASSERT_(pdf);
57 
58  // Both are m_particles:
59  m_particles = pdf->m_particles;
60  }
61  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFGaussian))
62  {
63  const CPosePDFGaussian* pdf = static_cast<const CPosePDFGaussian*>(&o);
64  size_t M = m_particles.size();
65  std::vector<CVectorDouble> parts;
67 
69 
70  clearParticles();
71  m_particles.resize(M);
72 
73  for (itDest = m_particles.begin(), partsIt = parts.begin();
74  itDest != m_particles.end(); ++itDest, ++partsIt)
75  {
76  itDest->log_w = 0;
77  itDest->d = TPose2D(
78  pdf->mean.x() + (*partsIt)[0], (pdf->mean.y() + (*partsIt)[1]),
79  (pdf->mean.phi() + (*partsIt)[2]));
80  itDest->d.normalizePhi();
81  }
82  }
83 
84  MRPT_END
85 }
86 
87 void CPosePDFParticles::clear() { clearParticles(); }
88 
89 void CPosePDFParticles::getMean(CPose2D& est_) const
90 {
91  // Calc average on SE(2)
92  const size_t n = m_particles.size();
93  if (n)
94  {
95  mrpt::poses::SE_average<2> se_averager;
96  for (size_t i = 0; i < n; i++)
97  {
98  double w = exp(m_particles[i].log_w);
99  se_averager.append(m_particles[i].d, w);
100  }
101  se_averager.get_average(est_);
102  }
103  else
104  {
105  est_ = CPose2D();
106  }
107 }
108 
109 void CPosePDFParticles::getCovarianceAndMean(
110  CMatrixDouble33& cov, CPose2D& mean) const
111 {
112  cov.zeros();
113  getMean(mean);
114 
115  size_t i, n = m_particles.size();
116  double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
117  double mean_phi = mean.phi();
118 
119  if (mean_phi < 0) mean_phi = M_2PI + mean_phi;
120 
121  double lin_w_sum = 0;
122 
123  for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
124  if (lin_w_sum == 0) lin_w_sum = 1;
125 
126  for (i = 0; i < n; i++)
127  {
128  double w = exp(m_particles[i].log_w) / lin_w_sum;
129 
130  // Manage 1 PI range:
131  double err_x = m_particles[i].d.x - mean.x();
132  double err_y = m_particles[i].d.y - mean.y();
133  double err_phi =
134  math::wrapToPi(fabs(m_particles[i].d.phi - mean_phi));
135 
136  var_x += square(err_x) * w;
137  var_y += square(err_y) * w;
138  var_p += square(err_phi) * w;
139  var_xy += err_x * err_y * w;
140  var_xp += err_x * err_phi * w;
141  var_yp += err_y * err_phi * w;
142  }
143 
144  if (n < 2)
145  {
146  // Not enought information to estimate the variance:
147  }
148  else
149  {
150  // Unbiased estimation of variance:
151  cov(0, 0) = var_x;
152  cov(1, 1) = var_y;
153  cov(2, 2) = var_p;
154 
155  cov(1, 0) = cov(0, 1) = var_xy;
156  cov(2, 0) = cov(0, 2) = var_xp;
157  cov(1, 2) = cov(2, 1) = var_yp;
158  }
159 }
160 
161 uint8_t CPosePDFParticles::serializeGetVersion() const { return 0; }
162 void CPosePDFParticles::serializeTo(mrpt::serialization::CArchive& out) const
163 {
164  writeParticlesToStream(out);
165 }
166 void CPosePDFParticles::serializeFrom(
168 {
169  switch (version)
170  {
171  case 0:
172  {
173  readParticlesFromStream(in);
174  }
175  break;
176  default:
178  };
179 }
180 
181 void CPosePDFParticles::resetDeterministic(
182  const TPose2D& location, size_t particlesCount)
183 {
184  if (particlesCount > 0)
185  m_particles.resize(particlesCount);
186 
187  for (auto& p : m_particles)
188  {
189  p.d = location;
190  p.log_w = .0;
191  }
192 }
193 
194 void CPosePDFParticles::resetUniform(
195  const double x_min, const double x_max, const double y_min,
196  const double y_max, const double phi_min, const double phi_max,
197  const int particlesCount)
198 {
199  MRPT_START
200  if (particlesCount > 0)
201  m_particles.resize(particlesCount);
202 
203  for (auto& p : m_particles)
204  {
205  p.d.x= getRandomGenerator().drawUniform(x_min, x_max);
206  p.d.y = getRandomGenerator().drawUniform(y_min, y_max);
207  p.d.phi = getRandomGenerator().drawUniform(phi_min, phi_max);
208  p.log_w = 0;
209  }
210  MRPT_END
211 }
212 
213 void CPosePDFParticles::resetAroundSetOfPoses(
214  const std::vector<mrpt::math::TPose2D>& list_poses,
215  const size_t num_particles_per_pose, const double spread_x,
216  const double spread_y, const double spread_phi_rad)
217 {
218  MRPT_START
219  ASSERT_(!list_poses.empty());
220  ASSERT_(num_particles_per_pose >= 1);
221 
222  const size_t N = list_poses.size() * num_particles_per_pose;
223 
224  clear();
225  m_particles.resize(N);
226  size_t i, nSpot;
227  for (i = 0, nSpot = 0; nSpot < list_poses.size(); nSpot++)
228  {
229  const mrpt::math::TPose2D& p = list_poses[nSpot];
230  for (size_t k = 0; k < num_particles_per_pose; k++, i++)
231  {
232  m_particles[i].d.x=
234  p.x - spread_x * 0.5, p.x + spread_x * 0.5);
235  m_particles[i].d.y=
237  p.y - spread_y * 0.5, p.y + spread_y * 0.5);
238  m_particles[i].d.phi = getRandomGenerator().drawUniform(
239  p.phi - spread_phi_rad * 0.5, p.phi + spread_phi_rad * 0.5);
240  m_particles[i].log_w = 0;
241  }
242  }
243  ASSERT_EQUAL_(i, N);
244  MRPT_END
245 }
246 
248 {
249  std::string buf;
250  buf += mrpt::format("%% x y yaw[rad] log_weight\n");
251 
252  for (const auto & p : m_particles)
253  buf += mrpt::format("%f %f %f %e\n", p.d.x, p.d.y, p.d.phi, p.log_w);
254 
255  std::ofstream f(file);
256  if (!f.is_open()) return false;
257  f << buf;
258  return true;
259 }
260 
261 TPose2D CPosePDFParticles::getParticlePose(size_t i) const
262 {
263  return m_particles[i].d;
264 }
265 
266 void CPosePDFParticles::changeCoordinatesReference(
267  const CPose3D& newReferenceBase_)
268 {
269  const TPose2D newReferenceBase = CPose2D(newReferenceBase_).asTPose();
270  for (auto &p : m_particles)
271  p.d = newReferenceBase + p.d;
272 }
273 
274 void CPosePDFParticles::drawSingleSample(CPose2D& outPart) const
275 {
276  const double uni = getRandomGenerator().drawUniform(0.0, 0.9999);
277  double cum = 0;
278 
279  for (auto &p : m_particles)
280  {
281  cum += exp(p.log_w);
282  if (uni <= cum)
283  {
284  outPart = CPose2D(p.d);
285  return;
286  }
287  }
288 
289  // Might not come here normally:
290  outPart = CPose2D(m_particles.rbegin()->d);
291 }
292 
294 {
295  for (auto &p : m_particles)
296  p.d = p.d + Ap;
297 }
298 
299 void CPosePDFParticles::append(CPosePDFParticles& o)
300 {
301  for (auto &p : o.m_particles)
302  m_particles.emplace_back(p);
303  normalizeWeights();
304 }
305 
307 {
308  MRPT_START
310  CPosePDFParticles* out = static_cast<CPosePDFParticles*>(&o);
311 
312  out->copyFrom(*this);
313  TPose2D nullPose(0, 0, 0);
314 
315  for (auto &p : out->m_particles)
316  p.d = nullPose - p.d;
317 
318  MRPT_END
319 }
320 
321 mrpt::math::TPose2D CPosePDFParticles::getMostLikelyParticle() const
322 {
323  mrpt::math::TPose2D ret{ 0,0,0 };
324  double max_w = -std::numeric_limits<double>::max();
325  for (const auto & p: m_particles)
326  {
327  if (p.log_w > max_w)
328  {
329  ret = p.d;
330  max_w = p.log_w;
331  }
332  }
333  return ret;
334 }
335 
336 void CPosePDFParticles::bayesianFusion(
337  const CPosePDF& p1, const CPosePDF& p2,
338  const double minMahalanobisDistToDrop)
339 {
340  MRPT_UNUSED_PARAM(p1);
341  MRPT_UNUSED_PARAM(p2);
342  MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
343 
344  THROW_EXCEPTION("Not implemented yet!");
345 }
346 
347 double CPosePDFParticles::evaluatePDF_parzen(
348  const double x, const double y, const double phi, const double stdXY,
349  const double stdPhi) const
350 {
351  double ret = 0;
352  for (const auto & p : m_particles)
353  {
354  double difPhi = math::wrapToPi(phi - p.d.phi);
355  ret += exp(p.log_w) *
356  math::normalPDF(std::sqrt(square(p.d.x-x)+square(p.d.y - y)), 0, stdXY) *
357  math::normalPDF(std::abs(difPhi), 0, stdPhi);
358  }
359  return ret;
360 }
361 
362 void CPosePDFParticles::saveParzenPDFToTextFile(
363  const char* fileName, const double x_min, const double x_max,
364  const double y_min, const double y_max, const double phi,
365  const double stepSizeXY, const double stdXY, const double stdPhi) const
366 {
367  std::string buf;
368 
369  for (double y = y_min; y < y_max; y += stepSizeXY)
370  for (double x = x_min; x < x_max; x += stepSizeXY)
371  buf+=mrpt::format("%f ", evaluatePDF_parzen(x, y, phi, stdXY, stdPhi));
372  buf += "\n";
373 
374  std::ofstream f(fileName);
375  if (!f.is_open()) return;
376  f << buf;
377 }
Computes weighted and un-weighted averages of SE(2) poses.
A namespace of pseudo-random numbers generators 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:140
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
CPose2D mean
The mean value.
#define M_2PI
Definition: common.h:58
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
CParticleList m_particles
The array of particles.
GLint location
Definition: glext.h:4086
#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...
GLenum GLsizei n
Definition: glext.h:5074
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.
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:441
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix...
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:67
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
GLenum GLint GLint y
Definition: glext.h:3538
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:148
GLenum GLint x
Definition: glext.h:3538
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:33
GLfloat GLfloat p
Definition: glext.h:6305
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
const Scalar * const_iterator
Definition: eigen_plugins.h:27
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019