21 class CParticleFilterCapable;
30 template <
class Derived,
class particle_list_t>
36 return *
dynamic_cast<const Derived*
>(
this);
39 inline Derived&
derived() {
return *
dynamic_cast<Derived*
>(
this); }
40 double getW(
size_t i)
const override
42 if (i >=
derived().m_particles.size())
44 return derived().m_particles[i].log_w;
47 void setW(
size_t i,
double w)
override
49 if (i >=
derived().m_particles.size())
51 derived().m_particles[i].log_w = w;
56 return derived().m_particles.size();
62 if (
derived().m_particles.empty())
return 0;
63 double minW =
derived().m_particles[0].log_w;
67 for (
auto it =
derived().m_particles.begin();
68 it !=
derived().m_particles.end(); ++it)
70 maxW = std::max<double>(maxW, it->log_w);
71 minW = std::min<double>(minW, it->log_w);
74 for (
auto it =
derived().m_particles.begin();
75 it !=
derived().m_particles.end(); ++it)
77 if (out_max_log_w) *out_max_log_w = maxW;
80 return std::exp(maxW - minW);
84 double ESS()
const override
90 double sumLinearWeights = 0;
91 for (
auto it =
derived().m_particles.begin();
92 it !=
derived().m_particles.end(); ++it)
93 sumLinearWeights += std::exp(it->log_w);
95 for (
auto it =
derived().m_particles.begin();
96 it !=
derived().m_particles.end(); ++it)
97 cum +=
mrpt::square(std::exp(it->log_w) / sumLinearWeights);
102 return 1.0 / (
derived().m_particles.size() * cum);
113 std::vector<size_t> sorted_indx(indx);
114 std::sort(sorted_indx.begin(), sorted_indx.end());
117 particle_list_t parts;
118 parts.resize(sorted_indx.size());
124 const size_t M_old =
derived().m_particles.size();
127 std::vector<int> reusedIdx(M_old, -1);
128 typename particle_list_t::iterator itDest;
129 for (
size_t i = 0; i < parts.size(); i++)
131 const size_t sorted_idx = sorted_indx[i];
132 parts[i].log_w =
derived().m_particles[sorted_idx].log_w;
136 const int idx_of_this_in_new_set = reusedIdx[sorted_idx];
137 if (idx_of_this_in_new_set == -1)
140 parts[i].d = std::move(
derived().m_particles[sorted_idx].d);
141 reusedIdx[sorted_idx] = i;
147 parts[i].d = parts[idx_of_this_in_new_set].d;
155 auto it_idx = sorted_indx.begin();
156 auto itDest = parts.begin();
157 for (; itDest != parts.end(); ++it_idx, ++itDest)
158 *itDest =
derived().m_particles[*it_idx];
161 derived().m_particles = std::move(parts);
206 template <
class STREAM>
210 auto n =
static_cast<uint32_t
>(
m_particles.size());
212 typename CParticleList::const_iterator it;
228 template <
class STREAM>
236 typename CParticleList::iterator it;
242 it->d.reset(
new T());
260 std::vector<double>::iterator it;
261 typename CParticleList::const_iterator it2;
262 for (it = out_logWeights.begin(), it2 =
m_particles.begin();
277 if (ret ==
nullptr || p.log_w > ret->log_w) ret = &p;