Main MRPT website > C++ reference for MRPT 1.5.6
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-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 #ifndef CParticleFilterData_H
10 #define CParticleFilterData_H
11 
12 #include <mrpt/utils/core_defs.h>
15 
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 basic functionality of any CParticleFilterData<> class.
26  * Users should inherit from CParticleFilterData<>, which in turn will automatically inhirit from this base class.
27  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterData
28  * \ingroup mrpt_base_grp
29  */
30  template <class Derived,class particle_list_t>
32  {
33  /// CRTP helper method
34  inline const Derived& derived() const { return *dynamic_cast<const Derived*>(this); }
35  /// CRTP helper method
36  inline Derived& derived() { return *dynamic_cast<Derived*>(this); }
37 
38  double getW(size_t i) const MRPT_OVERRIDE
39  {
40  if (i>=derived().m_particles.size()) THROW_EXCEPTION_FMT("Index %i is out of range!",(int)i);
41  return derived().m_particles[i].log_w;
42  }
43 
44  void setW(size_t i, double w) MRPT_OVERRIDE
45  {
46  if (i>=derived().m_particles.size()) THROW_EXCEPTION_FMT("Index %i is out of range!",(int)i);
47  derived().m_particles[i].log_w = w;
48  }
49 
51  {
52  return derived().m_particles.size();
53  }
54 
55  double normalizeWeights( double *out_max_log_w = NULL ) MRPT_OVERRIDE
56  {
58  if (derived().m_particles.empty()) return 0;
59  double minW = derived().m_particles[0].log_w;
60  double maxW = minW;
61 
62  /* Compute the max/min of weights: */
63  for (typename particle_list_t::iterator it=derived().m_particles.begin();it!=derived().m_particles.end();++it)
64  {
65  maxW = std::max<double>( maxW, it->log_w );
66  minW = std::min<double>( minW, it->log_w );
67  }
68  /* Normalize: */
69  for (typename particle_list_t::iterator it=derived().m_particles.begin();it!=derived().m_particles.end();++it)
70  it->log_w -= maxW;
71  if (out_max_log_w) *out_max_log_w = maxW;
72 
73  /* Return the max/min ratio: */
74  return exp(maxW-minW);
75  MRPT_END
76  }
77 
78  double ESS() const MRPT_OVERRIDE
79  {
81  double cum = 0;
82 
83  /* Sum of weights: */
84  double sumLinearWeights = 0;
85  for (typename particle_list_t::const_iterator it=derived().m_particles.begin();it!=derived().m_particles.end();++it)
86  sumLinearWeights += exp( it->log_w );
87  /* Compute ESS: */
88  for (typename particle_list_t::const_iterator it=derived().m_particles.begin();it!=derived().m_particles.end();++it)
89  cum+= utils::square( exp( it->log_w ) / sumLinearWeights );
90 
91  if (cum==0)
92  return 0;
93  else return 1.0/(derived().m_particles.size()*cum);
94  MRPT_END
95  }
96 
97  /** Replaces the old particles by copies determined by the indexes in "indx", performing an efficient copy of the necesary particles only and allowing the number of particles to change.*/
98  void performSubstitution( const std::vector<size_t> &indx) MRPT_OVERRIDE
99  {
100  MRPT_START
101  particle_list_t parts;
102  typename particle_list_t::iterator itDest,itSrc;
103  const size_t M_old = derived().m_particles.size();
104  size_t i,j,lastIndxOld = 0;
105  std::vector<bool> oldParticlesReused(M_old,false);
107  std::vector<size_t> sorted_indx(indx);
108 
109  /* Assure the input index is sorted: */
110  std::sort( sorted_indx.begin(), sorted_indx.end() );
111  /* Set the new size: */
112  parts.resize( sorted_indx.size() );
113  for (i=0,itDest=parts.begin();itDest!=parts.end();i++,itDest++)
114  {
115  const size_t sorted_idx = sorted_indx[i];
116  itDest->log_w = derived().m_particles[ sorted_idx ].log_w;
117  /* We can safely delete old m_particles from [lastIndxOld,indx[i]-1] (inclusive): */
118  for (j=lastIndxOld;j<sorted_idx;j++)
119  {
120  if (!oldParticlesReused[j]) /* If reused we can not delete that memory! */
121  derived().m_particles[j].d.reset();
122  }
123 
124  /* For the next iteration:*/
125  lastIndxOld = sorted_idx;
126 
127  /* If this is the first time that the old particle "indx[i]" appears, */
128  /* we can reuse the old "data" instead of creating a new copy: */
129  if (!oldParticlesReused[sorted_idx])
130  {
131  /* Reuse the data from the particle: */
132  parts[i].d.reset( derived().m_particles[ sorted_idx ].d.get() );
133  oldParticlesReused[sorted_idx]=true;
134  }
135  else
136  {
137  /* Make a copy of the particle's data: */
138  ASSERT_( derived().m_particles[ sorted_idx ].d );
139  parts[i].d.reset(new typename Derived::CParticleDataContent(*derived().m_particles[sorted_idx].d));
140  }
141  }
142  /* Free memory of unused particles */
143  for (itSrc = derived().m_particles.begin(), oldPartIt = oldParticlesReused.begin(); itSrc != derived().m_particles.end(); itSrc++, oldPartIt++)
144  if (!*oldPartIt)
145  itSrc->d.reset();
146  /* Copy the pointers only to the final destination */
147  derived().m_particles.resize( parts.size() );
148  for (itSrc=parts.begin(),itDest=derived().m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ )
149  {
150  itDest->log_w = itSrc->log_w;
151  itDest->d.move_from(itSrc->d);
152  }
153  parts.clear();
154  MRPT_END
155  }
156 
157  }; // end CParticleFilterDataImpl<>
158 
159 
160  /** This template class declares the array of particles and its internal data, managing some memory-related issues and providing an easy implementation of virtual methods required for implementing a CParticleFilterCapable.
161  * See also the methods in the base class CParticleFilterDataImpl<>.
162  *
163  * Since CProbabilityParticle implements all the required operators, the member "m_particles" can be safely copied with "=" or copy constructor operators
164  * and new objects will be created internally instead of copying the internal pointers, which would lead to memory corruption.
165  *
166  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterDataImpl
167  * \ingroup mrpt_base_grp
168  */
169  template <class T>
171  {
172  public:
173  typedef T CParticleDataContent; //!< This is the type inside the corresponding CParticleData class
174  typedef CProbabilityParticle<T> CParticleData; //!< Use this to refer to each element in the m_particles array.
175  typedef std::deque<CParticleData> CParticleList; //!< Use this type to refer to the list of particles m_particles.
176 
177  CParticleList m_particles; //!< The array of particles
178 
179  /** Default constructor */
181 
182  /** Free the memory of all the particles and reset the array "m_particles" to length zero */
184  {
185  m_particles.clear();
186  }
187 
188  /** Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).
189  * \sa readParticlesFromStream
190  */
191  template <class STREAM>
192  void writeParticlesToStream( STREAM &out ) const
193  {
194  MRPT_START
195  uint32_t n = static_cast<uint32_t>(m_particles.size());
196  out << n;
197  typename CParticleList::const_iterator it;
198  for (it=m_particles.begin();it!=m_particles.end();++it)
199  out << it->log_w << (*it->d);
200  MRPT_END
201  }
202 
203  /** Reads the sequence of particles and their weights from a stream (requires T implementing CSerializable).
204  * \sa writeParticlesToStream
205  */
206  template <class STREAM>
208  {
209  MRPT_START
210  clearParticles(); // Erase previous content:
211  uint32_t n;
212  in >> n;
213  m_particles.resize(n);
214  typename CParticleList::iterator it;
215  for (it=m_particles.begin();it!=m_particles.end();++it)
216  {
217  in >> it->log_w;
218  it->d.reset(new T());
219  in >> *it->d;
220  }
221  MRPT_END
222  }
223 
224 
225  /** Returns a vector with the sequence of the logaritmic weights of all the samples.
226  */
227  void getWeights( std::vector<double> &out_logWeights ) const
228  {
229  MRPT_START
230  out_logWeights.resize(m_particles.size());
232  typename CParticleList::const_iterator it2;
233  for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();++it,++it2)
234  *it = it2->log_w;
235  MRPT_END
236  }
237 
238  /** Returns the particle with the highest weight.
239  */
241  {
242  MRPT_START
243  const CParticleData *ret = NULL;
244  ASSERT_(m_particles.size()>0)
245 
246  typename CParticleList::const_iterator it;
247  for (it=m_particles.begin();it!=m_particles.end();++it)
248  {
249  if (ret==NULL || it->log_w > ret->log_w)
250  ret = &(*it);
251  }
252  return ret;
253  MRPT_END
254  }
255 
256 
257  }; // End of class def.
258 
259 
260 } // end namespace
261 } // end namespace
262 #endif
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
Derived & derived()
CRTP helper method.
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
const Derived & derived() const
CRTP helper method.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLuint in
Definition: glew.h:7146
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
CParticleList m_particles
The array of particles.
GLsizei n
Definition: glew.h:5051
void setW(size_t i, double w) MRPT_OVERRIDE
Modifies i'th particle (logarithm) weight, where first one is index 0.
#define MRPT_END
CProbabilityParticle< T > CParticleData
Use this to refer to each element in the m_particles array.
This virtual class defines the interface that any particles based PDF class must implement in order t...
This template class declares the array of particles and its internal data, managing some memory-relat...
#define MRPT_START
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A template class for holding a the data and the weight of a particle.
double normalizeWeights(double *out_max_log_w=NULL) MRPT_OVERRIDE
Normalize the (logarithmic) weights, such as the maximum weight is zero.
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
#define ASSERT_(f)
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
void performSubstitution(const std::vector< size_t > &indx) MRPT_OVERRIDE
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
CParticleFilterData()
Default constructor.
unsigned __int32 uint32_t
Definition: rptypes.h:49
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
double getW(size_t i) const MRPT_OVERRIDE
Access to i'th particle (logarithm) weight, where first one is index 0.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018