9 #ifndef CParticleFilterData_H 10 #define CParticleFilterData_H 23 class CParticleFilterCapable;
30 template <
class Derived,
class particle_list_t>
34 inline const Derived&
derived()
const {
return *
static_cast<const Derived*
>(
this); }
36 inline Derived&
derived() {
return *
static_cast<Derived*
>(
this); }
38 double getW(
size_t i)
const 41 return derived().m_particles[i].log_w;
44 void setW(
size_t i,
double w)
47 derived().m_particles[i].log_w = w;
52 return derived().m_particles.size();
58 if (
derived().m_particles.empty())
return 0;
59 double minW =
derived().m_particles[0].log_w;
65 maxW = std::max<double>( maxW, it->log_w );
66 minW = std::min<double>( minW, it->log_w );
71 if (out_max_log_w) *out_max_log_w = maxW;
74 return exp(maxW-minW);
84 double sumLinearWeights = 0;
86 sumLinearWeights += exp( it->log_w );
93 else return 1.0/(
derived().m_particles.size()*cum);
101 particle_list_t parts;
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);
110 std::sort( sorted_indx.begin(), sorted_indx.end() );
112 parts.resize( sorted_indx.size() );
113 for (i=0,itDest=parts.begin();itDest!=parts.end();i++,itDest++)
115 const size_t sorted_idx = sorted_indx[i];
116 itDest->log_w =
derived().m_particles[ sorted_idx ].log_w;
118 for (j=lastIndxOld;j<sorted_idx;j++)
120 if (!oldParticlesReused[j])
122 delete derived().m_particles[j].d;
123 derived().m_particles[j].d = NULL;
128 lastIndxOld = sorted_idx;
132 if (!oldParticlesReused[sorted_idx])
135 parts[i].d =
derived().m_particles[ sorted_idx ].d;
136 oldParticlesReused[sorted_idx]=
true;
142 parts[i].d =
new typename Derived::CParticleDataContent( *
derived().m_particles[ sorted_idx ].d );
146 for (itSrc=
derived().m_particles.begin(),oldPartIt=oldParticlesReused.begin();itSrc!=
derived().m_particles.end();itSrc++,oldPartIt++)
153 derived().m_particles.resize( parts.size() );
154 for (itSrc=parts.begin(),itDest=
derived().m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ )
156 itDest->log_w = itSrc->log_w;
157 itDest->d = itSrc->d;
196 if (it->d)
delete it->d;
211 template <
class STREAM>
215 uint32_t n =
static_cast<uint32_t
>(m_particles.size());
218 for (it=m_particles.begin();it!=m_particles.end();++it)
219 out << it->log_w << (*it->d);
226 template <
class STREAM>
233 m_particles.resize(n);
235 for (it=m_particles.begin();it!=m_particles.end();++it)
250 out_logWeights.resize(m_particles.size());
253 for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();++it,++it2)
263 const CParticleData *ret = NULL;
267 for (it=m_particles.begin();it!=m_particles.end();++it)
269 if (ret==NULL || it->log_w > ret->log_w)
void setW(size_t i, double w)
Modifies i'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.
const Derived & derived() const
CRTP helper method.
const Scalar * const_iterator
void performSubstitution(const std::vector< size_t > &indx)
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
CParticleList m_particles
The array of particles.
double getW(size_t i) const
Access to i'th particle (logarithm) weight, where first one is index 0.
CProbabilityParticle< T > CParticleData
Use this to refer to each element in the m_particles array.
double ESS() const
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
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...
T square(const T x)
Inline function for the square of a number.
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...
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
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)
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
size_t particlesCount() const
Get the m_particles count.