Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 MRPT_OVERRIDE
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) MRPT_OVERRIDE
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 
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  {
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 */
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 */
203  {
204  clearParticles();
205  }
206 
207  /** Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).
208  * \sa readParticlesFromStream
209  */
210  template <class STREAM>
211  void writeParticlesToStream( STREAM &out ) const
212  {
213  MRPT_START
214  uint32_t n = static_cast<uint32_t>(m_particles.size());
215  out << n;
216  typename CParticleList::const_iterator it;
217  for (it=m_particles.begin();it!=m_particles.end();++it)
218  out << it->log_w << (*it->d);
219  MRPT_END
220  }
221 
222  /** Reads the sequence of particles and their weights from a stream (requires T implementing CSerializable).
223  * \sa writeParticlesToStream
224  */
225  template <class STREAM>
226  void readParticlesFromStream(STREAM &in)
227  {
228  MRPT_START
229  clearParticles(); // Erase previous content:
230  uint32_t n;
231  in >> n;
232  m_particles.resize(n);
233  typename CParticleList::iterator it;
234  for (it=m_particles.begin();it!=m_particles.end();++it)
235  {
236  in >> it->log_w;
237  it->d = new T();
238  in >> *it->d;
239  }
240  MRPT_END
241  }
242 
243 
244  /** Returns a vector with the sequence of the logaritmic weights of all the samples.
245  */
246  void getWeights( std::vector<double> &out_logWeights ) const
247  {
248  MRPT_START
249  out_logWeights.resize(m_particles.size());
251  typename CParticleList::const_iterator it2;
252  for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();++it,++it2)
253  *it = it2->log_w;
254  MRPT_END
255  }
256 
257  /** Returns the particle with the highest weight.
258  */
260  {
261  MRPT_START
262  const CParticleData *ret = NULL;
263  ASSERT_(m_particles.size()>0)
264 
265  typename CParticleList::const_iterator it;
266  for (it=m_particles.begin();it!=m_particles.end();++it)
267  {
268  if (ret==NULL || it->log_w > ret->log_w)
269  ret = &(*it);
270  }
271  return ret;
272  MRPT_END
273  }
274 
275 
276  }; // End of class def.
277 
278 
279 } // end namespace
280 } // end namespace
281 #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:
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 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.
void setW(size_t i, double w) MRPT_OVERRIDE
Modifies i&#39;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
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.
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...
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.
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.
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...
double getW(size_t i) const MRPT_OVERRIDE
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
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.14 for MRPT 1.4.0 SVN: at Mon Mar 5 22:51:34 UTC 2018