Main MRPT website > C++ reference
MRPT logo
CParticleFilterData.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CParticleFilterData_H
29 #define CParticleFilterData_H
30 
31 #include <mrpt/utils/utils_defs.h>
32 #include <mrpt/bayes/CProbabilityParticle.h>
33 
34 #include <algorithm>
35 
36 namespace mrpt
37 {
38 namespace bayes
39 {
40  /** 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.
41  * By adding IMPLEMENT_PARTICLE_FILTER_CAPABLE(T) to the body of the declaration of classes inheriting from both CParticleFilterData and CParticleFilterCapable, the following
42  * pure virtual methods are automatically implemented (the param T must be equal to the argument of the template CParticleFilterData).
43  * - CParticleFilterCapable::getW
44  * - CParticleFilterCapable::setW
45  * - CParticleFilterCapable::particlesCount
46  * - CParticleFilterCapable::normalizeWeights
47  * - CParticleFilterCapable::ESS
48  * - CParticleFilterCapable::performSubstitution
49  *
50  * Since CProbabilityParticle implements all the required operators, the member "m_particles" can be safely copied with "=" or copy constructor operators
51  * and new objects will be created internally instead of copying the internal pointers, which would lead to memory corruption.
52  *
53  * \sa CParticleFilter, CParticleFilterCapable, IMPLEMENT_PARTICLE_FILTER_CAPABLE
54  * \ingroup mrpt_base_grp
55  */
56  template <class T>
57  class CParticleFilterData
58  {
59  public:
60  typedef T CParticleDataContent; //!< This is the type inside the corresponding CParticleData class
61  typedef CProbabilityParticle<T> CParticleData; //!< Use this to refer to each element in the m_particles array.
62  typedef std::deque<CParticleData> CParticleList; //!< Use this type to refer to the list of particles m_particles.
63 
64  CParticleList m_particles; //!< The array of particles
65 
66  /** Default constructor */
67  CParticleFilterData() : m_particles(0)
68  { }
69 
70  /** Free the memory of all the particles and reset the array "m_particles" to length zero.
71  */
72  void clearParticles()
73  {
74  MRPT_START
75  for (typename CParticleList::iterator it=m_particles.begin();it!=m_particles.end();it++)
76  if (it->d) delete it->d;
77  m_particles.clear();
78  MRPT_END
79  }
80 
81  /** Virtual destructor
82  */
83  virtual ~CParticleFilterData()
84  {
85  MRPT_START
86  clearParticles();
87  MRPT_END
88  }
89 
90  /** Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).
91  * \sa readParticlesFromStream
92  */
93  void writeParticlesToStream( utils::CStream &out ) const
94  {
95  MRPT_START
96  uint32_t n = static_cast<uint32_t>(m_particles.size());
97  out << n;
98  typename CParticleList::const_iterator it;
99  for (it=m_particles.begin();it!=m_particles.end();it++)
100  out << it->log_w << (*it->d);
101  MRPT_END
102  }
103 
104  /** Reads the sequence of particles and their weights from a stream (requires T implementing CSerializable).
105  * \sa writeParticlesToStream
106  */
107  void readParticlesFromStream(utils::CStream &in)
108  {
109  MRPT_START
110  clearParticles(); // Erase previous content:
111  uint32_t n;
112  in >> n;
113  m_particles.resize(n);
114  typename CParticleList::iterator it;
115  for (it=m_particles.begin();it!=m_particles.end();it++)
116  {
117  in >> it->log_w;
118  it->d = new T();
119  in >> *it->d;
120  }
121  MRPT_END
122  }
123 
124 
125  /** Returns a vector with the sequence of the logaritmic weights of all the samples.
126  */
127  void getWeights( vector_double &out_logWeights ) const
128  {
129  MRPT_START
130  out_logWeights.resize(m_particles.size());
131  vector_double::iterator it;
132  typename CParticleList::const_iterator it2;
133  for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();it++,it2++)
134  *it = it2->log_w;
135  MRPT_END
136  }
137 
138  /** Returns the particle with the highest weight.
139  */
140  const CParticleData * getMostLikelyParticle() const
141  {
142  MRPT_START
143  const CParticleData *ret = NULL;
144  ASSERT_(m_particles.size()>0)
145 
146  typename CParticleList::const_iterator it;
147  for (it=m_particles.begin();it!=m_particles.end();it++)
148  {
149  if (ret==NULL || it->log_w > ret->log_w)
150  ret = &(*it);
151  }
152  return ret;
153  MRPT_END
154  }
155 
156 
157  }; // End of class def.
158 
159  /** This must be placed within the declaration of classes inheriting from both CParticleFilterData and CParticleFilterCapable to implement some pure virtual methods (the param T must be equal to the argument of the template CParticleFilterData).
160  * The following pure virtual methods are implemented:
161  * - CParticleFilterCapable::getW
162  * - CParticleFilterCapable::setW
163  * - CParticleFilterCapable::particlesCount
164  * - CParticleFilterCapable::normalizeWeights
165  * - CParticleFilterCapable::ESS
166  * - CParticleFilterCapable::performSubstitution
167  */
168 #define IMPLEMENT_PARTICLE_FILTER_CAPABLE(T) \
169  public: \
170  virtual double getW(size_t i) const \
171  { \
172  MRPT_START \
173  if (i>=m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("Index %i is out of range!",(int)i); \
174  return m_particles[i].log_w; \
175  MRPT_END \
176  } \
177  virtual void setW(size_t i, double w) \
178  { \
179  MRPT_START \
180  if (i>=m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("Index %i is out of range!",(int)i); \
181  m_particles[i].log_w = w; \
182  MRPT_END \
183  } \
184  virtual size_t particlesCount() const { return m_particles.size(); } \
185  virtual double normalizeWeights( double *out_max_log_w = NULL ) \
186  { \
187  MRPT_START \
188  CParticleList::iterator it;\
189  \
190  if (!m_particles.size()) return 0; \
191  double minW,maxW; \
192  minW = maxW = m_particles[0].log_w; \
193  /* Compute the max/min of weights: */ \
194  for (it=m_particles.begin();it!=m_particles.end();it++) \
195  { \
196  maxW = std::max<double>( maxW, it->log_w ); \
197  minW = std::min<double>( minW, it->log_w ); \
198  } \
199  /* Normalize: */ \
200  for (it=m_particles.begin();it!=m_particles.end();it++) \
201  it->log_w -= maxW; \
202  if (out_max_log_w) \
203  *out_max_log_w = maxW; \
204  /* Return the max/min ratio: */ \
205  return exp(maxW-minW); \
206  MRPT_END \
207  } \
208  virtual double ESS() \
209  { \
210  MRPT_START \
211  CParticleList::iterator it; \
212  double cum = 0; \
213  \
214  /* Sum of weights: */ \
215  double sumLinearWeights = 0; \
216  for (it=m_particles.begin();it!=m_particles.end();it++) \
217  sumLinearWeights += exp( it->log_w ); \
218  /* Compute ESS: */ \
219  for (it=m_particles.begin();it!=m_particles.end();it++) \
220  cum+= utils::square( exp( it->log_w ) / sumLinearWeights ); \
221  \
222  if (cum==0) \
223  return 0; \
224  else return 1.0/(m_particles.size()*cum); \
225  MRPT_END \
226  } \
227  /** 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.*/ \
228  virtual void performSubstitution( const std::vector<size_t> &indx) \
229  { \
230  MRPT_START \
231  CParticleList parts; \
232  CParticleList::iterator itDest,itSrc; \
233  size_t M_old = m_particles.size(); \
234  size_t i,j,lastIndxOld = 0; \
235  std::vector<bool> oldParticlesReused(M_old,false); \
236  std::vector<bool>::const_iterator oldPartIt; \
237  std::vector<size_t> sorted_indx(indx); \
238  std::vector<size_t>::iterator sort_idx_it; \
239  /* Assure the input index is sorted: */ \
240  std::sort( sorted_indx.begin(), sorted_indx.end() ); \
241  /* Set the new size: */ \
242  parts.resize( sorted_indx.size() ); \
243  for (i=0,itDest=parts.begin();itDest!=parts.end();i++,itDest++) \
244  { \
245  const size_t sorted_idx = sorted_indx[i]; \
246  itDest->log_w = m_particles[ sorted_idx ].log_w; \
247  /* We can safely delete old m_particles from [lastIndxOld,indx[i]-1] (inclusive): */ \
248  for (j=lastIndxOld;j<sorted_idx;j++) \
249  { \
250  if (!oldParticlesReused[j]) /* If reused we can not delete that memory! */ \
251  { \
252  delete m_particles[j].d; \
253  m_particles[j].d = NULL; \
254  } \
255  } \
256  /* For the next iteration:*/ \
257  lastIndxOld = sorted_idx; \
258  /* If this is the first time that the old particle "indx[i]" appears, */ \
259  /* we can reuse the old "data" instead of creating a new copy: */ \
260  if (!oldParticlesReused[sorted_idx]) \
261  { \
262  /* Reuse the data from the particle: */ \
263  parts[i].d = m_particles[ sorted_idx ].d; \
264  oldParticlesReused[sorted_idx]=true; \
265  } \
266  else \
267  { \
268  /* Make a copy of the particle's data: */ \
269  ASSERT_( m_particles[ sorted_idx ].d != NULL); \
270  parts[i].d = new T( *m_particles[ sorted_idx ].d ); \
271  } \
272  } \
273  /* Free memory of unused particles */ \
274  for (itSrc=m_particles.begin(),oldPartIt=oldParticlesReused.begin();itSrc!=m_particles.end();itSrc++,oldPartIt++) \
275  if (! *oldPartIt ) \
276  { \
277  delete itSrc->d; \
278  itSrc->d = NULL; \
279  } \
280  /* Copy the pointers only to the final destination */ \
281  m_particles.resize( parts.size() ); \
282  for (itSrc=parts.begin(),itDest=m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ ) \
283  { \
284  itDest->log_w = itSrc->log_w; \
285  itDest->d = itSrc->d; \
286  itSrc->d = NULL; \
287  } \
288  parts.clear(); \
289  MRPT_END \
290  } \
291 
292  } // end namespace
293 } // end namespace
294 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013