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
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::bayes::CParticleFilterData::CParticleList
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Definition: CParticleFilterData.h:182
mrpt::bayes::CParticleFilterDataImpl::derived
const Derived & derived() const
CRTP helper method.
Definition: CParticleFilterData.h:34
CProbabilityParticle.h
mrpt::bayes::CParticleFilterDataImpl::normalizeWeights
double normalizeWeights(double *out_max_log_w=NULL) MRPT_OVERRIDE
Normalize the (logarithmic) weights, such as the maximum weight is zero.
Definition: CParticleFilterData.h:55
mrpt::bayes::CParticleFilterData::readParticlesFromStream
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
Definition: CParticleFilterData.h:226
mrpt::bayes::CParticleFilterData::clearParticles
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
Definition: CParticleFilterData.h:192
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
const_iterator
const typedef Scalar * const_iterator
Definition: eigen_plugins.h:24
MRPT_END
#define MRPT_END
Definition: mrpt_macros.h:353
mrpt::bayes::CParticleFilterDataImpl::derived
Derived & derived()
CRTP helper method.
Definition: CParticleFilterData.h:36
mrpt::bayes::CParticleFilterData::CParticleFilterData
CParticleFilterData()
Default constructor.
Definition: CParticleFilterData.h:187
mrpt::bayes::CParticleFilterData::~CParticleFilterData
virtual ~CParticleFilterData()
Virtual destructor.
Definition: CParticleFilterData.h:202
mrpt::bayes::CProbabilityParticle
A template class for holding a the data and the weight of a particle.
Definition: CProbabilityParticle.h:25
core_defs.h
mrpt::bayes::CParticleFilterDataImpl::getW
double getW(size_t i) const MRPT_OVERRIDE
Access to i'th particle (logarithm) weight, where first one is index 0.
Definition: CParticleFilterData.h:38
MRPT_START
#define MRPT_START
Definition: mrpt_macros.h:349
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:27
mrpt::bayes::CParticleFilterData::writeParticlesToStream
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
Definition: CParticleFilterData.h:211
mrpt::bayes::CParticleFilterDataImpl::setW
void setW(size_t i, double w) MRPT_OVERRIDE
Modifies i'th particle (logarithm) weight, where first one is index 0.
Definition: CParticleFilterData.h:44
mrpt::bayes::CParticleFilterData
This template class declares the array of particles and its internal data, managing some memory-relat...
Definition: CParticleFilterData.h:177
CParticleFilterCapable.h
mrpt::bayes::CParticleFilterData::getMostLikelyParticle
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
Definition: CParticleFilterData.h:259
mrpt::bayes::CParticleFilterData::getWeights
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
Definition: CParticleFilterData.h:246
mrpt::bayes::CParticleFilterDataImpl::ESS
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Definition: CParticleFilterData.h:78
mrpt::utils::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
ASSERT_
#define ASSERT_(f)
Definition: mrpt_macros.h:261
mrpt::bayes::CParticleFilterDataImpl
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
Definition: CParticleFilterData.h:31
mrpt::bayes::CParticleFilterData::CParticleData
CProbabilityParticle< T > CParticleData
Use this to refer to each element in the m_particles array.
Definition: CParticleFilterData.h:181
mrpt::bayes::CParticleFilterDataImpl::particlesCount
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
Definition: CParticleFilterData.h:50
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:184
mrpt::bayes::CParticleFilterData::CParticleDataContent
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
Definition: CParticleFilterData.h:180
MRPT_OVERRIDE
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
THROW_EXCEPTION_CUSTOM_MSG1
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
mrpt::bayes::CParticleFilterDataImpl::performSubstitution
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...
Definition: CParticleFilterData.h:98



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:08:25 UTC 2019