Main MRPT website > C++ reference for MRPT 1.9.9
CParticleFilterData.h
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 #pragma once
10 
13 #include <mrpt/core/exceptions.h>
14 #include <mrpt/core/bits_math.h>
15 #include <cmath>
16 #include <deque>
17 #include <algorithm>
18 
19 namespace mrpt
20 {
21 namespace bayes
22 {
23 class CParticleFilterCapable;
24 
25 /** A curiously recurring template pattern (CRTP) approach to providing the
26  * basic functionality of any CParticleFilterData<> class.
27  * Users should inherit from CParticleFilterData<>, which in turn will
28  * automatically inhirit from this base class.
29  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterData
30  * \ingroup mrpt_bayes_grp
31  */
32 template <class Derived, class particle_list_t>
34 {
35  /// CRTP helper method
36  inline const Derived& derived() const
37  {
38  return *dynamic_cast<const Derived*>(this);
39  }
40  /// CRTP helper method
41  inline Derived& derived() { return *dynamic_cast<Derived*>(this); }
42  double getW(size_t i) const override
43  {
44  if (i >= derived().m_particles.size())
45  THROW_EXCEPTION_FMT("Index %i is out of range!", (int)i);
46  return derived().m_particles[i].log_w;
47  }
48 
49  void setW(size_t i, double w) override
50  {
51  if (i >= derived().m_particles.size())
52  THROW_EXCEPTION_FMT("Index %i is out of range!", (int)i);
53  derived().m_particles[i].log_w = w;
54  }
55 
56  size_t particlesCount() const override
57  {
58  return derived().m_particles.size();
59  }
60 
61  double normalizeWeights(double* out_max_log_w = nullptr) override
62  {
64  if (derived().m_particles.empty()) return 0;
65  double minW = derived().m_particles[0].log_w;
66  double maxW = minW;
67 
68  /* Compute the max/min of weights: */
69  for (typename particle_list_t::iterator it =
70  derived().m_particles.begin();
71  it != derived().m_particles.end(); ++it)
72  {
73  maxW = std::max<double>(maxW, it->log_w);
74  minW = std::min<double>(minW, it->log_w);
75  }
76  /* Normalize: */
77  for (typename particle_list_t::iterator it =
78  derived().m_particles.begin();
79  it != derived().m_particles.end(); ++it)
80  it->log_w -= maxW;
81  if (out_max_log_w) *out_max_log_w = maxW;
82 
83  /* Return the max/min ratio: */
84  return std::exp(maxW - minW);
85  MRPT_END
86  }
87 
88  double ESS() const override
89  {
91  double cum = 0;
92 
93  /* Sum of weights: */
94  double sumLinearWeights = 0;
95  for (typename particle_list_t::const_iterator it =
96  derived().m_particles.begin();
97  it != derived().m_particles.end(); ++it)
98  sumLinearWeights += std::exp(it->log_w);
99  /* Compute ESS: */
100  for (typename particle_list_t::const_iterator it =
101  derived().m_particles.begin();
102  it != derived().m_particles.end(); ++it)
103  cum += mrpt::square(std::exp(it->log_w) / sumLinearWeights);
104 
105  if (cum == 0)
106  return 0;
107  else
108  return 1.0 / (derived().m_particles.size() * cum);
109  MRPT_END
110  }
111 
112  /** Replaces the old particles by copies determined by the indexes in
113  * "indx", performing an efficient copy of the necesary particles only and
114  * allowing the number of particles to change.*/
115  void performSubstitution(const std::vector<size_t>& indx) override
116  {
117  MRPT_START
118  // Ensure input indices are sorted
119  std::vector<size_t> sorted_indx(indx);
120  std::sort(sorted_indx.begin(), sorted_indx.end());
121 
122  /* Temporary buffer: */
123  particle_list_t parts;
124  parts.resize(sorted_indx.size());
125 
126  // Implementation for particles as pointers:
127  if constexpr(Derived::PARTICLE_STORAGE == particle_storage_mode::POINTER)
128  {
129  const size_t M_old = derived().m_particles.size();
130  std::vector<bool> oldParticlesReused(M_old, false);
132  typename particle_list_t::iterator itDest, itSrc;
133  size_t i, lastIndxOld = 0;
134 
135  for (i = 0, itDest = parts.begin(); itDest != parts.end();
136  i++, itDest++)
137  {
138  const size_t sorted_idx = sorted_indx[i];
139  itDest->log_w = derived().m_particles[sorted_idx].log_w;
140  /* We can safely delete old m_particles from [lastIndxOld,indx[i]-1]
141  * (inclusive): */
142  for (size_t j = lastIndxOld; j < sorted_idx; j++)
143  {
144  if (!oldParticlesReused
145  [j]) /* If reused we can not delete that memory! */
146  derived().m_particles[j].d.reset();
147  }
148 
149  /* For the next iteration:*/
150  lastIndxOld = sorted_idx;
151 
152  /* If this is the first time that the old particle "indx[i]" appears, */
153  /* we can reuse the old "data" instead of creating a new copy: */
154  if (!oldParticlesReused[sorted_idx])
155  {
156  /* Reuse the data from the particle: */
157  parts[i].d.reset(derived().m_particles[sorted_idx].d.get());
158  oldParticlesReused[sorted_idx] = true;
159  }
160  else
161  {
162  /* Make a copy of the particle's data: */
163  ASSERT_(derived().m_particles[sorted_idx].d);
164  parts[i].d.reset(
165  new typename Derived::CParticleDataContent(
166  *derived().m_particles[sorted_idx].d));
167  }
168  }
169  /* Free memory of unused particles */
170  for (itSrc = derived().m_particles.begin(),
171  oldPartIt = oldParticlesReused.begin();
172  itSrc != derived().m_particles.end(); itSrc++, oldPartIt++)
173  if (!*oldPartIt) itSrc->d.reset();
174  }
175  else
176  {
177  // Implementation for particles as values:
178  auto it_idx = sorted_indx.begin();
179  auto itDest = parts.begin();
180  for (; itDest != parts.end(); ++it_idx, ++itDest)
181  *itDest = derived().m_particles[*it_idx];
182  }
183  /* Move particles to the final container: */
184  derived().m_particles = std::move(parts);
185  MRPT_END
186  }
187 
188 }; // end CParticleFilterDataImpl<>
189 
190 /** This template class declares the array of particles and its internal data,
191  * managing some memory-related issues and providing an easy implementation of
192  * virtual methods required for implementing a CParticleFilterCapable.
193  * See also the methods in the base class CParticleFilterDataImpl<>.
194  *
195  * Since CProbabilityParticle implements all the required operators, the
196  * member "m_particles" can be safely copied with "=" or copy constructor
197  * operators
198  * and new objects will be created internally instead of copying the internal
199  * pointers, which would lead to memory corruption.
200  *
201  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterDataImpl
202  * \ingroup mrpt_bayes_grp
203  */
204 template <
207 {
208  public:
209  /** This is the type inside the corresponding CParticleData class */
211  /** Use this to refer to each element in the m_particles array. */
213  /** Use this type to refer to the list of particles m_particles. */
214  using CParticleList = std::deque<CParticleData>;
215  static const particle_storage_mode PARTICLE_STORAGE = STORAGE;
216 
217  /** The array of particles */
219 
220  /** Default constructor */
222  /** Free the memory of all the particles and reset the array "m_particles"
223  * to length zero */
224  void clearParticles() { m_particles.clear(); }
225  /** Dumps the sequence of particles and their weights to a stream (requires
226  * T implementing CSerializable).
227  * \sa readParticlesFromStream
228  */
229  template <class STREAM>
230  void writeParticlesToStream(STREAM& out) const
231  {
232  MRPT_START
233  uint32_t n = static_cast<uint32_t>(m_particles.size());
234  out << n;
235  typename CParticleList::const_iterator it;
236  for (it = m_particles.begin(); it != m_particles.end(); ++it)
237  {
238  out << it->log_w;
239  if constexpr(STORAGE == particle_storage_mode::POINTER)
240  out << (*it->d);
241  else out << it->d;
242  }
243  MRPT_END
244  }
245 
246  /** Reads the sequence of particles and their weights from a stream
247  * (requires T implementing CSerializable).
248  * \sa writeParticlesToStream
249  */
250  template <class STREAM>
252  {
253  MRPT_START
254  clearParticles(); // Erase previous content:
255  uint32_t n;
256  in >> n;
257  m_particles.resize(n);
258  typename CParticleList::iterator it;
259  for (it = m_particles.begin(); it != m_particles.end(); ++it)
260  {
261  in >> it->log_w;
262  if constexpr(STORAGE == particle_storage_mode::POINTER)
263  {
264  it->d.reset(new T());
265  in >> *it->d;
266  }
267  else
268  {
269  in >> it->d;
270  }
271  }
272  MRPT_END
273  }
274 
275  /** Returns a vector with the sequence of the logaritmic weights of all the
276  * samples.
277  */
278  void getWeights(std::vector<double>& out_logWeights) const
279  {
280  MRPT_START
281  out_logWeights.resize(m_particles.size());
283  typename CParticleList::const_iterator it2;
284  for (it = out_logWeights.begin(), it2 = m_particles.begin();
285  it2 != m_particles.end(); ++it, ++it2)
286  *it = it2->log_w;
287  MRPT_END
288  }
289 
290  /** Returns the particle with the highest weight.
291  */
293  {
294  MRPT_START
295  const CParticleData* ret = nullptr;
296  ASSERT_(m_particles.size() > 0);
297 
298  for (const auto p : m_particles)
299  if (ret == nullptr || p.log_w > ret->log_w) ret = &p;
300  return ret;
301  MRPT_END
302  }
303 
304 }; // End of class def.
305 
306 } // namespace bayes
307 } // namespace mrpt
Scalar * iterator
Definition: eigen_plugins.h:26
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
#define MRPT_START
Definition: exceptions.h:262
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
Derived & derived()
CRTP helper method.
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
static const particle_storage_mode PARTICLE_STORAGE
GLenum GLsizei n
Definition: glext.h:5074
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
size_t particlesCount() const override
Get the m_particles count.
T square(const T x)
Inline function for the square of a number.
CParticleFilterData()
Default constructor.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
This virtual class defines the interface that any particles based PDF class must implement in order t...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
This template class declares the array of particles and its internal data, managing some memory-relat...
void performSubstitution(const std::vector< size_t > &indx) override
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getW(size_t i) const override
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
A template class for holding a the data and the weight of a particle.
const Derived & derived() const
CRTP helper method.
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
particle_storage_mode
use for CProbabilityParticle
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
void setW(size_t i, double w) override
Modifies i&#39;th particle (logarithm) weight, where first one is index 0.



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