22 class CParticleFilterCapable;
31 template <
class Derived,
class particle_list_t>
37 return *
dynamic_cast<const Derived*
>(
this);
40 inline Derived&
derived() {
return *
dynamic_cast<Derived*
>(
this); }
41 double getW(
size_t i)
const override 43 if (i >=
derived().m_particles.size())
45 return derived().m_particles[i].log_w;
48 void setW(
size_t i,
double w)
override 50 if (i >=
derived().m_particles.size())
52 derived().m_particles[i].log_w = w;
57 return derived().m_particles.size();
63 if (
derived().m_particles.empty())
return 0;
64 double minW =
derived().m_particles[0].log_w;
68 for (
auto it =
derived().m_particles.begin();
69 it !=
derived().m_particles.end(); ++it)
71 maxW = std::max<double>(maxW, it->log_w);
72 minW = std::min<double>(minW, it->log_w);
75 for (
auto it =
derived().m_particles.begin();
76 it !=
derived().m_particles.end(); ++it)
78 if (out_max_log_w) *out_max_log_w = maxW;
81 return std::exp(maxW - minW);
85 double ESS()
const override 91 double sumLinearWeights = 0;
92 for (
auto it =
derived().m_particles.begin();
93 it !=
derived().m_particles.end(); ++it)
94 sumLinearWeights += std::exp(it->log_w);
96 for (
auto it =
derived().m_particles.begin();
97 it !=
derived().m_particles.end(); ++it)
98 cum +=
mrpt::square(std::exp(it->log_w) / sumLinearWeights);
103 return 1.0 / (
derived().m_particles.size() * cum);
114 std::vector<size_t> sorted_indx(indx);
115 std::sort(sorted_indx.begin(), sorted_indx.end());
118 particle_list_t parts;
119 parts.resize(sorted_indx.size());
125 const size_t M_old =
derived().m_particles.size();
128 std::vector<int> reusedIdx(M_old, -1);
129 typename particle_list_t::iterator itDest;
130 for (
size_t i = 0; i < parts.size(); i++)
132 const size_t sorted_idx = sorted_indx[i];
133 parts[i].log_w =
derived().m_particles[sorted_idx].log_w;
137 const int idx_of_this_in_new_set = reusedIdx[sorted_idx];
138 if (idx_of_this_in_new_set == -1)
141 parts[i].d = std::move(
derived().m_particles[sorted_idx].d);
142 reusedIdx[sorted_idx] = i;
148 parts[i].d = parts[idx_of_this_in_new_set].d;
156 auto it_idx = sorted_indx.begin();
157 auto itDest = parts.begin();
158 for (; itDest != parts.end(); ++it_idx, ++itDest)
159 *itDest =
derived().m_particles[*it_idx];
162 derived().m_particles = std::move(parts);
207 template <
class STREAM>
211 auto n =
static_cast<uint32_t
>(
m_particles.size());
213 typename CParticleList::const_iterator it;
229 template <
class STREAM>
237 typename CParticleList::iterator it;
243 it->d.reset(
new T());
261 std::vector<double>::iterator it;
262 typename CParticleList::const_iterator it2;
263 for (it = out_logWeights.begin(), it2 =
m_particles.begin();
278 if (ret ==
nullptr || p.log_w > ret->log_w) ret = &p;
void clearParticles()
Free the memory of all the particles and reset the array "m_particles" to length zero.
void writeParticlesToStream(STREAM &out) const
Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable)...
void getWeights(std::vector< double > &out_logWeights) const
Returns a vector with the sequence of the logaritmic weights of all the samples.
Derived & derived()
CRTP helper method.
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
static const particle_storage_mode PARTICLE_STORAGE
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
size_t particlesCount() const override
Get the m_particles count.
CParticleFilterData()
Default constructor.
#define ASSERT_(f)
Defines an assertion mechanism.
void readParticlesFromStream(STREAM &in)
Reads the sequence of particles and their weights from a stream (requires T implementing CSerializabl...
This virtual class defines the interface that any particles based PDF class must implement in order t...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
const CParticleData * getMostLikelyParticle() const
Returns the particle with the highest weight.
This template class declares the array of particles and its internal data, managing some memory-relat...
void performSubstitution(const std::vector< size_t > &indx) override
Replaces the old particles by copies determined by the indexes in "indx", performing an efficient cop...
return_t square(const num_t x)
Inline function for the square of a number.
double getW(size_t i) const override
Access to 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.
mrpt::vision::TStereoCalibResults out
const Derived & derived() const
CRTP helper method.
particle_storage_mode
use for CProbabilityParticle
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
void setW(size_t i, double w) override
Modifies i'th particle (logarithm) weight, where first one is index 0.