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); }
41 return derived().m_particles[i].log_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;
63 for (
typename particle_list_t::iterator it=
derived().m_particles.begin();it!=
derived().m_particles.end();++it)
65 maxW = std::max<double>( maxW, it->log_w );
66 minW = std::min<double>( minW, it->log_w );
69 for (
typename particle_list_t::iterator it=
derived().m_particles.begin();it!=
derived().m_particles.end();++it)
71 if (out_max_log_w) *out_max_log_w = maxW;
74 return exp(maxW-minW);
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 );
88 for (
typename particle_list_t::const_iterator it=
derived().m_particles.begin();it!=
derived().m_particles.end();++it)
93 else return 1.0/(
derived().m_particles.size()*cum);
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);
106 std::vector<bool>::const_iterator oldPartIt;
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;
210 template <
class STREAM>
216 typename CParticleList::const_iterator it;
218 out << it->log_w << (*it->d);
225 template <
class STREAM>
233 typename CParticleList::iterator it;
250 std::vector<double>::iterator it;
251 typename CParticleList::const_iterator it2;
265 typename CParticleList::const_iterator it;
268 if (ret==NULL || it->log_w > ret->log_w)
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...
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.
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.