Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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 *static_cast<const Derived*>(this); }
35  /// CRTP helper method
36  inline Derived& derived() { return *static_cast<Derived*>(this); }
37 
38  double getW(size_t i) const
39  {
40  if (i>=derived().m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("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)
45  {
46  if (i>=derived().m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("Index %i is out of range!",(int)i);
47  derived().m_particles[i].log_w = w;
48  }
49 
50  size_t particlesCount() const
51  {
52  return derived().m_particles.size();
53  }
54 
55  double normalizeWeights( double *out_max_log_w = NULL )
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
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)
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  {
122  delete derived().m_particles[j].d;
123  derived().m_particles[j].d = NULL;
124  }
125  }
126 
127  /* For the next iteration:*/
128  lastIndxOld = sorted_idx;
129 
130  /* If this is the first time that the old particle "indx[i]" appears, */
131  /* we can reuse the old "data" instead of creating a new copy: */
132  if (!oldParticlesReused[sorted_idx])
133  {
134  /* Reuse the data from the particle: */
135  parts[i].d = derived().m_particles[ sorted_idx ].d;
136  oldParticlesReused[sorted_idx]=true;
137  }
138  else
139  {
140  /* Make a copy of the particle's data: */
141  ASSERT_( derived().m_particles[ sorted_idx ].d != NULL);
142  parts[i].d = new typename Derived::CParticleDataContent( *derived().m_particles[ sorted_idx ].d );
143  }
144  }
145  /* Free memory of unused particles */
146  for (itSrc=derived().m_particles.begin(),oldPartIt=oldParticlesReused.begin();itSrc!=derived().m_particles.end();itSrc++,oldPartIt++)
147  if (! *oldPartIt )
148  {
149  delete itSrc->d;
150  itSrc->d = NULL;
151  }
152  /* Copy the pointers only to the final destination */
153  derived().m_particles.resize( parts.size() );
154  for (itSrc=parts.begin(),itDest=derived().m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ )
155  {
156  itDest->log_w = itSrc->log_w;
157  itDest->d = itSrc->d;
158  itSrc->d = NULL;
159  }
160  parts.clear();
161  MRPT_END
162  }
163 
164  }; // end CParticleFilterDataImpl<>
165 
166 
167  /** 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.
168  * See also the methods in the base class CParticleFilterDataImpl<>.
169  *
170  * Since CProbabilityParticle implements all the required operators, the member "m_particles" can be safely copied with "=" or copy constructor operators
171  * and new objects will be created internally instead of copying the internal pointers, which would lead to memory corruption.
172  *
173  * \sa CParticleFilter, CParticleFilterCapable, CParticleFilterDataImpl
174  * \ingroup mrpt_base_grp
175  */
176  template <class T>
178  {
179  public:
180  typedef T CParticleDataContent; //!< This is the type inside the corresponding CParticleData class
181  typedef CProbabilityParticle<T> CParticleData; //!< Use this to refer to each element in the m_particles array.
182  typedef std::deque<CParticleData> CParticleList; //!< Use this type to refer to the list of particles m_particles.
183 
184  CParticleList m_particles; //!< The array of particles
185 
186  /** Default constructor */
187  CParticleFilterData() : m_particles(0)
188  { }
189 
190  /** Free the memory of all the particles and reset the array "m_particles" to length zero.
191  */
193  {
194  MRPT_START
195  for (typename CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
196  if (it->d) delete it->d;
197  m_particles.clear();
198  MRPT_END
199  }
200 
201  /** Virtual destructor
202  */
204  {
205  clearParticles();
206  }
207 
208  /** Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).
209  * \sa readParticlesFromStream
210  */
211  template <class STREAM>
212  void writeParticlesToStream( STREAM &out ) const
213  {
214  MRPT_START
215  uint32_t n = static_cast<uint32_t>(m_particles.size());
216  out << n;
217  typename CParticleList::const_iterator it;
218  for (it=m_particles.begin();it!=m_particles.end();++it)
219  out << it->log_w << (*it->d);
220  MRPT_END
221  }
222 
223  /** Reads the sequence of particles and their weights from a stream (requires T implementing CSerializable).
224  * \sa writeParticlesToStream
225  */
226  template <class STREAM>
227  void readParticlesFromStream(STREAM &in)
228  {
229  MRPT_START
230  clearParticles(); // Erase previous content:
231  uint32_t n;
232  in >> n;
233  m_particles.resize(n);
234  typename CParticleList::iterator it;
235  for (it=m_particles.begin();it!=m_particles.end();++it)
236  {
237  in >> it->log_w;
238  it->d = new T();
239  in >> *it->d;
240  }
241  MRPT_END
242  }
243 
244 
245  /** Returns a vector with the sequence of the logaritmic weights of all the samples.
246  */
247  void getWeights( std::vector<double> &out_logWeights ) const
248  {
249  MRPT_START
250  out_logWeights.resize(m_particles.size());
252  typename CParticleList::const_iterator it2;
253  for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();++it,++it2)
254  *it = it2->log_w;
255  MRPT_END
256  }
257 
258  /** Returns the particle with the highest weight.
259  */
260  const CParticleData * getMostLikelyParticle() const
261  {
262  MRPT_START
263  const CParticleData *ret = NULL;
264  ASSERT_(m_particles.size()>0)
265 
266  typename CParticleList::const_iterator it;
267  for (it=m_particles.begin();it!=m_particles.end();++it)
268  {
269  if (ret==NULL || it->log_w > ret->log_w)
270  ret = &(*it);
271  }
272  return ret;
273  MRPT_END
274  }
275 
276 
277  }; // End of class def.
278 
279 
280 } // end namespace
281 } // end namespace
282 #endif
void setW(size_t i, double w)
Modifies i&#39;th particle (logarithm) weight, where first one is index 0.
Derived & derived()
CRTP helper method.
double normalizeWeights(double *out_max_log_w=NULL)
Normalize the (logarithmic) weights, such as the maximum weight is zero.
double ESS() const
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
double getW(size_t i) const
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
Scalar * iterator
Definition: eigen_plugins.h:23
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void performSubstitution(const std::vector< size_t > &indx)
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
CParticleList m_particles
The array of particles.
#define MRPT_END
CProbabilityParticle< T > CParticleData
Use this to refer to each element in the m_particles array.
size_t particlesCount() const
Get the m_particles count.
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
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
const Derived & derived() const
CRTP helper method.
#define ASSERT_(f)
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
CParticleFilterData()
Default constructor.
virtual ~CParticleFilterData()
Virtual destructor.
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:46:27 UTC 2016