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
15
16#include <deque>
17#include <algorithm>
18
19namespace mrpt
20{
21namespace 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);
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);
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 {
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();
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 {
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();
199 }
200
201 /** Virtual destructor */
203 {
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 {
214 uint32_t n = static_cast<uint32_t>(m_particles.size());
215 out << n;
217 for (it=m_particles.begin();it!=m_particles.end();++it)
218 out << it->log_w << (*it->d);
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 {
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 }
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 {
249 out_logWeights.resize(m_particles.size());
252 for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();++it,++it2)
253 *it = it2->log_w;
255 }
256
257 /** Returns the particle with the highest weight.
258 */
260 {
262 const CParticleData *ret = NULL;
263 ASSERT_(m_particles.size()>0)
264
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;
273 }
274
275
276 }; // End of class def.
277
278
279} // end namespace
280} // end namespace
281#endif
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...
CParticleList m_particles
The array of particles.
CProbabilityParticle< T > CParticleData
Use this to refer to each element in the m_particles array.
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
CParticleFilterData()
Default constructor.
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
virtual ~CParticleFilterData()
Virtual destructor.
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
Scalar * iterator
Definition: eigen_plugins.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
#define MRPT_END
Definition: mrpt_macros.h:353
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.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
double normalizeWeights(double *out_max_log_w=NULL) MRPT_OVERRIDE
Normalize the (logarithmic) weights, such as the maximum weight is zero.
double getW(size_t i) const MRPT_OVERRIDE
Access to i'th particle (logarithm) weight, where first one is index 0.
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
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...
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Derived & derived()
CRTP helper method.
const Derived & derived() const
CRTP helper method.
void setW(size_t i, double w) MRPT_OVERRIDE
Modifies i'th particle (logarithm) weight, where first one is index 0.
A template class for holding a the data and the weight of a particle.



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:36:32 UTC 2021