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 *
dynamic_cast<const Derived*
>(
this); }
36 inline Derived&
derived() {
return *
dynamic_cast<Derived*
>(
this); }
41 return derived().m_particles[i].log_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])
121 derived().m_particles[j].d.reset();
125 lastIndxOld = sorted_idx;
129 if (!oldParticlesReused[sorted_idx])
132 parts[i].d.reset(
derived().m_particles[ sorted_idx ].d.get() );
133 oldParticlesReused[sorted_idx]=
true;
139 parts[i].d.reset(
new typename Derived::CParticleDataContent(*
derived().m_particles[sorted_idx].d));
143 for (itSrc =
derived().m_particles.begin(), oldPartIt = oldParticlesReused.begin(); itSrc !=
derived().m_particles.end(); itSrc++, oldPartIt++)
147 derived().m_particles.resize( parts.size() );
148 for (itSrc=parts.begin(),itDest=
derived().m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ )
150 itDest->log_w = itSrc->log_w;
151 itDest->d.move_from(itSrc->d);
191 template <
class STREAM>
199 out << it->log_w << (*it->d);
206 template <
class STREAM>
218 it->d.reset(
new T());
249 if (ret==NULL || it->log_w > ret->log_w)
size_t particlesCount() const MRPT_OVERRIDE
Get the m_particles count.
Derived & derived()
CRTP helper method.
double ESS() const MRPT_OVERRIDE
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
const Scalar * const_iterator
GLubyte GLubyte GLubyte GLubyte w
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
T square(const T x)
Inline function for the square of a number.
T CParticleDataContent
This is the type inside the corresponding CParticleData class.
CParticleList m_particles
The array of particles.
void setW(size_t i, double w) MRPT_OVERRIDE
Modifies 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.
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...
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.
double normalizeWeights(double *out_max_log_w=NULL) MRPT_OVERRIDE
Normalize the (logarithmic) weights, such as the maximum weight is zero.
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
const Derived & derived() const
CRTP helper method.
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
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...
CParticleFilterData()
Default constructor.
unsigned __int32 uint32_t
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
double getW(size_t i) const MRPT_OVERRIDE
Access to i'th particle (logarithm) weight, where first one is index 0.
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...