MRPT  1.9.9
CPosePDFParticles.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
13 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/random.h>
19 #include <mrpt/system/os.h>
20 #include <fstream>
21 
22 using namespace mrpt;
23 using namespace mrpt::bayes;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace mrpt::random;
27 using namespace mrpt::system;
28 using namespace std;
29 
31 
33 {
34  m_particles.resize(M);
35  for (auto& p : m_particles)
36  {
37  p.log_w = .0;
38  p.d = TPose2D();
39  }
40  TPose2D nullPose(0, 0, 0);
41  resetDeterministic(nullPose);
42 }
43 
44 void CPosePDFParticles::copyFrom(const CPosePDF& o)
45 {
47 
48  CParticleList::iterator itDest;
49  CParticleList::const_iterator itSrc;
50 
51  if (this == &o) return; // It may be used sometimes
52 
54  {
55  const auto* pdf = 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 auto* pdf = dynamic_cast<const CPosePDFGaussian*>(&o);
64  size_t M = m_particles.size();
65  std::vector<CVectorDouble> parts;
66  std::vector<CVectorDouble>::iterator partsIt;
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 void CPosePDFParticles::getMean(CPose2D& est_) const
89 {
90  // Calc average on SE(2)
91  const size_t n = m_particles.size();
92  if (n)
93  {
94  mrpt::poses::SE_average<2> se_averager;
95  for (size_t i = 0; i < n; i++)
96  {
97  double w = exp(m_particles[i].log_w);
98  se_averager.append(m_particles[i].d, w);
99  }
100  se_averager.get_average(est_);
101  }
102  else
103  {
104  est_ = CPose2D();
105  }
106 }
107 
108 std::tuple<CMatrixDouble33, CPose2D> CPosePDFParticles::getCovarianceAndMean()
109  const
110 {
112  CPose2D mean;
113  cov.setZero();
114  getMean(mean);
115 
116  size_t i, n = m_particles.size();
117  double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
118  double mean_phi = mean.phi();
119 
120  if (mean_phi < 0) mean_phi = M_2PI + mean_phi;
121 
122  double lin_w_sum = 0;
123 
124  for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
125  if (lin_w_sum == 0) lin_w_sum = 1;
126 
127  for (i = 0; i < n; i++)
128  {
129  double w = exp(m_particles[i].log_w) / lin_w_sum;
130 
131  // Manage 1 PI range:
132  double err_x = m_particles[i].d.x - mean.x();
133  double err_y = m_particles[i].d.y - mean.y();
134  double err_phi = 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 enough 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  return {cov, mean};
160 }
161 
162 uint8_t CPosePDFParticles::serializeGetVersion() const { return 1; }
163 void CPosePDFParticles::serializeTo(mrpt::serialization::CArchive& out) const
164 {
165  writeParticlesToStream(out); // v1: changed CPose2D -> TPose2D
166 }
167 void CPosePDFParticles::serializeFrom(
168  mrpt::serialization::CArchive& in, uint8_t version)
169 {
170  switch (version)
171  {
172  case 0:
173  {
175  mrpt::poses::CPose2D, PARTICLE_STORAGE>
176  old;
177  old.readParticlesFromStream(in);
178  m_particles.clear();
179  std::transform(
180  old.m_particles.begin(), old.m_particles.end(),
181  std::back_inserter(m_particles),
182  [](const auto& p) -> CParticleData {
183  return CParticleData(p.d.asTPose(), p.log_w);
184  });
185  }
186  break;
187  case 1:
188  {
189  readParticlesFromStream(in);
190  }
191  break;
192  default:
194  };
195 }
196 
197 void CPosePDFParticles::resetDeterministic(
198  const TPose2D& location, size_t particlesCount)
199 {
200  if (particlesCount > 0) m_particles.resize(particlesCount);
201 
202  for (auto& p : m_particles)
203  {
204  p.d = location;
205  p.log_w = .0;
206  }
207 }
208 
209 void CPosePDFParticles::resetUniform(
210  const double x_min, const double x_max, const double y_min,
211  const double y_max, const double phi_min, const double phi_max,
212  const int particlesCount)
213 {
214  MRPT_START
215  if (particlesCount > 0) m_particles.resize(particlesCount);
216 
217  for (auto& p : m_particles)
218  {
219  p.d.x = getRandomGenerator().drawUniform(x_min, x_max);
220  p.d.y = getRandomGenerator().drawUniform(y_min, y_max);
221  p.d.phi = getRandomGenerator().drawUniform(phi_min, phi_max);
222  p.log_w = 0;
223  }
224  MRPT_END
225 }
226 
227 void CPosePDFParticles::resetAroundSetOfPoses(
228  const std::vector<mrpt::math::TPose2D>& list_poses,
229  const size_t num_particles_per_pose, const double spread_x,
230  const double spread_y, const double spread_phi_rad)
231 {
232  MRPT_START
233  ASSERT_(!list_poses.empty());
234  ASSERT_(num_particles_per_pose >= 1);
235 
236  const size_t N = list_poses.size() * num_particles_per_pose;
237 
238  clear();
239  m_particles.resize(N);
240  size_t i, nSpot;
241  for (i = 0, nSpot = 0; nSpot < list_poses.size(); nSpot++)
242  {
243  const mrpt::math::TPose2D& p = list_poses[nSpot];
244  for (size_t k = 0; k < num_particles_per_pose; k++, i++)
245  {
246  m_particles[i].d.x = getRandomGenerator().drawUniform(
247  p.x - spread_x * 0.5, p.x + spread_x * 0.5);
248  m_particles[i].d.y = getRandomGenerator().drawUniform(
249  p.y - spread_y * 0.5, p.y + spread_y * 0.5);
250  m_particles[i].d.phi = getRandomGenerator().drawUniform(
251  p.phi - spread_phi_rad * 0.5, p.phi + spread_phi_rad * 0.5);
252  m_particles[i].log_w = 0;
253  }
254  }
255  ASSERT_EQUAL_(i, N);
256  MRPT_END
257 }
258 
259 bool CPosePDFParticles::saveToTextFile(const std::string& file) const
260 {
261  std::string buf;
262  buf += "%% x y yaw[rad] log_weight\n";
263 
264  for (const auto& p : m_particles)
265  buf += mrpt::format("%f %f %f %e\n", p.d.x, p.d.y, p.d.phi, p.log_w);
266 
267  std::ofstream f(file);
268  if (!f.is_open()) return false;
269  f << buf;
270  return true;
271 }
272 
273 TPose2D CPosePDFParticles::getParticlePose(size_t i) const
274 {
275  return m_particles[i].d;
276 }
277 
278 void CPosePDFParticles::changeCoordinatesReference(
279  const CPose3D& newReferenceBase_)
280 {
281  const TPose2D newReferenceBase = CPose2D(newReferenceBase_).asTPose();
282  for (auto& p : m_particles) p.d = newReferenceBase + p.d;
283 }
284 
285 void CPosePDFParticles::drawSingleSample(CPose2D& outPart) const
286 {
287  const double uni = getRandomGenerator().drawUniform(0.0, 0.9999);
288  double cum = 0;
289 
290  for (auto& p : m_particles)
291  {
292  cum += exp(p.log_w);
293  if (uni <= cum)
294  {
295  outPart = CPose2D(p.d);
296  return;
297  }
298  }
299 
300  // Might not come here normally:
301  outPart = CPose2D(m_particles.rbegin()->d);
302 }
303 
305 {
306  for (auto& p : m_particles) p.d = p.d + Ap;
307 }
308 
309 void CPosePDFParticles::append(CPosePDFParticles& o)
310 {
311  for (auto& p : o.m_particles) m_particles.emplace_back(p);
312  normalizeWeights();
313 }
314 
315 void CPosePDFParticles::inverse(CPosePDF& o) const
316 {
317  MRPT_START
319  auto* out = dynamic_cast<CPosePDFParticles*>(&o);
320 
321  out->copyFrom(*this);
322  TPose2D nullPose(0, 0, 0);
323 
324  for (auto& p : out->m_particles) p.d = nullPose - p.d;
325 
326  MRPT_END
327 }
328 
329 mrpt::math::TPose2D CPosePDFParticles::getMostLikelyParticle() const
330 {
331  mrpt::math::TPose2D ret{0, 0, 0};
332  double max_w = -std::numeric_limits<double>::max();
333  for (const auto& p : m_particles)
334  {
335  if (p.log_w > max_w)
336  {
337  ret = p.d;
338  max_w = p.log_w;
339  }
340  }
341  return ret;
342 }
343 
344 void CPosePDFParticles::bayesianFusion(
345  [[maybe_unused]] const CPosePDF& p1, [[maybe_unused]] const CPosePDF& p2,
346  [[maybe_unused]] const double minMahalanobisDistToDrop)
347 {
348  THROW_EXCEPTION("Not implemented yet!");
349 }
350 
351 double CPosePDFParticles::evaluatePDF_parzen(
352  const double x, const double y, const double phi, const double stdXY,
353  const double stdPhi) const
354 {
355  double ret = 0;
356  for (const auto& p : m_particles)
357  {
358  double difPhi = math::wrapToPi(phi - p.d.phi);
359  ret += exp(p.log_w) *
361  std::sqrt(square(p.d.x - x) + square(p.d.y - y)), 0, stdXY) *
362  math::normalPDF(std::abs(difPhi), 0, stdPhi);
363  }
364  return ret;
365 }
366 
367 void CPosePDFParticles::saveParzenPDFToTextFile(
368  const char* fileName, const double x_min, const double x_max,
369  const double y_min, const double y_max, const double phi,
370  const double stepSizeXY, const double stdXY, const double stdPhi) const
371 {
372  std::string buf;
373 
374  for (double y = y_min; y < y_max; y += stepSizeXY)
375  for (double x = x_min; x < x_max; x += stepSizeXY)
376  buf += mrpt::format(
377  "%f ", evaluatePDF_parzen(x, y, phi, stdXY, stdPhi));
378  buf += "\n";
379 
380  std::ofstream f(fileName);
381  if (!f.is_open()) return;
382  f << buf;
383 }
Computes weighted and un-weighted averages of SE(2) poses.
A namespace of pseudo-random numbers generators of diferent distributions.
#define MRPT_START
Definition: exceptions.h:241
#define M_2PI
Definition: common.h:58
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CParticleList m_particles
The array of particles.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
STL namespace.
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
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:102
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:137
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:62
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
CMatrixDouble 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:149
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
This template class declares the array of particles and its internal data, managing some memory-relat...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
return_t square(const num_t x)
Inline function for the square of a number.
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:54
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
Lightweight 2D pose.
Definition: TPose2D.h:22
double mean(const CONTAINER &v)
Computes the mean value of a vector.
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
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
double phi
Orientation (rads)
Definition: TPose2D.h:32
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020