MRPT  1.9.9
CParticleFilterData.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 #pragma once
10 
13 #include <mrpt/core/bits_math.h>
14 #include <mrpt/core/exceptions.h>
15 #include <algorithm>
16 #include <cmath>
17 #include <deque>
18 
19 namespace mrpt::bayes
20 {
21 class CParticleFilterCapable;
22 
23 /** A curiously recurring template pattern (CRTP) approach to providing the
24  * basic functionality of any CParticleFilterData<> class.
25  * Users should inherit from CParticleFilterData<>, which in turn will
26  * automatically inhirit from this base class.
27  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterData
28  * \ingroup mrpt_bayes_grp
29  */
30 template <class Derived, class particle_list_t>
32 {
33  /// CRTP helper method
34  inline const Derived& derived() const
35  {
36  return *dynamic_cast<const Derived*>(this);
37  }
38  /// CRTP helper method
39  inline Derived& derived() { return *dynamic_cast<Derived*>(this); }
40  double getW(size_t i) const override
41  {
42  if (i >= derived().m_particles.size())
43  THROW_EXCEPTION_FMT("Index %i is out of range!", (int)i);
44  return derived().m_particles[i].log_w;
45  }
46 
47  void setW(size_t i, double w) override
48  {
49  if (i >= derived().m_particles.size())
50  THROW_EXCEPTION_FMT("Index %i is out of range!", (int)i);
51  derived().m_particles[i].log_w = w;
52  }
53 
54  size_t particlesCount() const override
55  {
56  return derived().m_particles.size();
57  }
58 
59  double normalizeWeights(double* out_max_log_w = nullptr) override
60  {
62  if (derived().m_particles.empty()) return 0;
63  double minW = derived().m_particles[0].log_w;
64  double maxW = minW;
65 
66  /* Compute the max/min of weights: */
67  for (auto it = derived().m_particles.begin();
68  it != derived().m_particles.end(); ++it)
69  {
70  maxW = std::max<double>(maxW, it->log_w);
71  minW = std::min<double>(minW, it->log_w);
72  }
73  /* Normalize: */
74  for (auto it = derived().m_particles.begin();
75  it != derived().m_particles.end(); ++it)
76  it->log_w -= maxW;
77  if (out_max_log_w) *out_max_log_w = maxW;
78 
79  /* Return the max/min ratio: */
80  return std::exp(maxW - minW);
81  MRPT_END
82  }
83 
84  double ESS() const override
85  {
87  double cum = 0;
88 
89  /* Sum of weights: */
90  double sumLinearWeights = 0;
91  for (auto it = derived().m_particles.begin();
92  it != derived().m_particles.end(); ++it)
93  sumLinearWeights += std::exp(it->log_w);
94  /* Compute ESS: */
95  for (auto it = derived().m_particles.begin();
96  it != derived().m_particles.end(); ++it)
97  cum += mrpt::square(std::exp(it->log_w) / sumLinearWeights);
98 
99  if (cum == 0)
100  return 0;
101  else
102  return 1.0 / (derived().m_particles.size() * cum);
103  MRPT_END
104  }
105 
106  /** Replaces the old particles by copies determined by the indexes in
107  * "indx", performing an efficient copy of the necesary particles only and
108  * allowing the number of particles to change.*/
109  void performSubstitution(const std::vector<size_t>& indx) override
110  {
111  MRPT_START
112  // Ensure input indices are sorted
113  std::vector<size_t> sorted_indx(indx);
114  std::sort(sorted_indx.begin(), sorted_indx.end());
115 
116  /* Temporary buffer: */
117  particle_list_t parts;
118  parts.resize(sorted_indx.size());
119 
120  // Implementation for particles as pointers:
121  if constexpr (
122  Derived::PARTICLE_STORAGE == particle_storage_mode::POINTER)
123  {
124  const size_t M_old = derived().m_particles.size();
125  // Index, in the *new* set, of reused particle data, indexed
126  // by *old* indices, or "-1" if not reused.
127  std::vector<int> reusedIdx(M_old, -1);
128  typename particle_list_t::iterator itDest;
129  for (size_t i = 0; i < parts.size(); i++)
130  {
131  const size_t sorted_idx = sorted_indx[i];
132  parts[i].log_w = derived().m_particles[sorted_idx].log_w;
133 
134  // The first time that the old particle "indx[i]" appears, we
135  // can reuse the old "data" instead of creating a new copy:
136  const int idx_of_this_in_new_set = reusedIdx[sorted_idx];
137  if (idx_of_this_in_new_set == -1)
138  {
139  // First time: Reuse the data from the particle.
140  parts[i].d = std::move(derived().m_particles[sorted_idx].d);
141  reusedIdx[sorted_idx] = i;
142  }
143  else
144  {
145  // Make a copy of the particle's data
146  // (The "= operator" already makes a deep copy)
147  parts[i].d = parts[idx_of_this_in_new_set].d;
148  }
149  }
150  // Free memory of unused particles: Done automatically.
151  }
152  else
153  {
154  // Implementation for particles as values:
155  auto it_idx = sorted_indx.begin();
156  auto itDest = parts.begin();
157  for (; itDest != parts.end(); ++it_idx, ++itDest)
158  *itDest = derived().m_particles[*it_idx];
159  }
160  /* Move particles to the final container: */
161  derived().m_particles = std::move(parts);
162  MRPT_END
163  }
164 
165 }; // end CParticleFilterDataImpl<>
166 
167 /** This template class declares the array of particles and its internal data,
168  * managing some memory-related issues and providing an easy implementation of
169  * virtual methods required for implementing a CParticleFilterCapable.
170  * See also the methods in the base class CParticleFilterDataImpl<>.
171  *
172  * Since CProbabilityParticle implements all the required operators, the
173  * member "m_particles" can be safely copied with "=" or copy constructor
174  * operators
175  * and new objects will be created internally instead of copying the internal
176  * pointers, which would lead to memory corruption.
177  *
178  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterDataImpl
179  * \ingroup mrpt_bayes_grp
180  */
181 template <
184 {
185  public:
186  /** This is the type inside the corresponding CParticleData class */
188  /** Use this to refer to each element in the m_particles array. */
190  /** Use this type to refer to the list of particles m_particles. */
191  using CParticleList = std::deque<CParticleData>;
192  static const particle_storage_mode PARTICLE_STORAGE = STORAGE;
193 
194  /** The array of particles */
196 
197  /** Default constructor */
199  /** Free the memory of all the particles and reset the array "m_particles"
200  * to length zero */
201  void clearParticles() { m_particles.clear(); }
202  /** Dumps the sequence of particles and their weights to a stream (requires
203  * T implementing CSerializable).
204  * \sa readParticlesFromStream
205  */
206  template <class STREAM>
207  void writeParticlesToStream(STREAM& out) const
208  {
209  MRPT_START
210  auto n = static_cast<uint32_t>(m_particles.size());
211  out << n;
212  typename CParticleList::const_iterator it;
213  for (it = m_particles.begin(); it != m_particles.end(); ++it)
214  {
215  out << it->log_w;
216  if constexpr (STORAGE == particle_storage_mode::POINTER)
217  out << (*it->d);
218  else
219  out << it->d;
220  }
221  MRPT_END
222  }
223 
224  /** Reads the sequence of particles and their weights from a stream
225  * (requires T implementing CSerializable).
226  * \sa writeParticlesToStream
227  */
228  template <class STREAM>
230  {
231  MRPT_START
232  clearParticles(); // Erase previous content:
233  uint32_t n;
234  in >> n;
235  m_particles.resize(n);
236  typename CParticleList::iterator it;
237  for (it = m_particles.begin(); it != m_particles.end(); ++it)
238  {
239  in >> it->log_w;
240  if constexpr (STORAGE == particle_storage_mode::POINTER)
241  {
242  it->d.reset(new T());
243  in >> *it->d;
244  }
245  else
246  {
247  in >> it->d;
248  }
249  }
250  MRPT_END
251  }
252 
253  /** Returns a vector with the sequence of the logaritmic weights of all the
254  * samples.
255  */
256  void getWeights(std::vector<double>& out_logWeights) const
257  {
258  MRPT_START
259  out_logWeights.resize(m_particles.size());
260  std::vector<double>::iterator it;
261  typename CParticleList::const_iterator it2;
262  for (it = out_logWeights.begin(), it2 = m_particles.begin();
263  it2 != m_particles.end(); ++it, ++it2)
264  *it = it2->log_w;
265  MRPT_END
266  }
267 
268  /** Returns the particle with the highest weight.
269  */
271  {
272  MRPT_START
273  const CParticleData* ret = nullptr;
274  ASSERT_(m_particles.size() > 0);
275 
276  for (const auto p : m_particles)
277  if (ret == nullptr || p.log_w > ret->log_w) ret = &p;
278  return ret;
279  MRPT_END
280  }
281 
282 }; // End of class def.
283 
284 } // namespace mrpt::bayes
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
#define MRPT_START
Definition: exceptions.h:241
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
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
GLenum GLsizei n
Definition: glext.h:5136
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
size_t particlesCount() const override
Get the m_particles count.
CParticleFilterData()
Default constructor.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
T square(const T x)
Inline function for the square of a number.
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...
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.
mrpt::vision::TStereoCalibResults out
const Derived & derived() const
CRTP helper method.
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
particle_storage_mode
use for CProbabilityParticle
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
GLfloat GLfloat p
Definition: glext.h:6398
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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019