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;