1 #ifndef SLAM_CTOR_PARTICLE_FILTER_H_INCLUDED 2 #define SLAM_CTOR_PARTICLE_FILTER_H_INCLUDED 7 #include <unordered_set> 23 template <
typename ParticleT>
26 virtual std::shared_ptr<ParticleT> create_particle() = 0;
30 template <
typename ParticlePtr>
38 for (
auto &p : particles) {
39 sq_sum += p->weight() * p->weight();
41 double effective_particles_cnt = 1.0 / sq_sum;
42 return effective_particles_cnt * 2 < particles.size();
45 std::vector<unsigned>
resample(std::vector<ParticlePtr> particles) {
49 std::vector<unsigned> new_particle_inds(particles.size());
51 std::random_device rd;
52 std::mt19937 engine(rd());
53 std::uniform_real_distribution<> uniform_distr(0, 1);
54 for (uint i = 0; i < particles.size(); i++) {
55 double sample = uniform_distr(engine);
57 for (uint j = 0; j < particles.size(); j++) {
58 total_w += particles[j]->weight();
59 if (sample < total_w) {
60 new_particle_inds[i] = j;
65 return new_particle_inds;
69 template <
typename ParticleT>
75 unsigned n = 1) : _particle_supplier{p_ftry} {
76 for (
unsigned i = 0; i < n; i++) {
78 particle->set_weight(1.0 / n);
79 _particles.push_back(particle);
85 if (!_resampler.resampling_is_required(_particles)) {
89 std::vector<ParticlePtr> new_particles;
90 std::unordered_set<unsigned> new_part_inds;
91 for (
unsigned i : _resampler.resample(_particles)) {
93 if (new_part_inds.count(i)) {
94 ParticlePtr new_particle = _particle_supplier->create_particle();
95 *new_particle = *sampled;
96 new_particle->sample();
97 sampled = new_particle;
99 new_part_inds.insert(i);
101 new_particles.push_back(sampled);
103 _particles = std::move(new_particles);
109 double total_weight = 0;
110 for (
auto &p : _particles) { total_weight += p->weight(); }
111 for (
auto &p : _particles) { p->set_weight(p->weight() / total_weight); }
115 const ParticleT *heaviest =
nullptr;
116 for (
auto &p : particles()) {
117 if (heaviest && p->weight() < heaviest->weight()) {
continue; }
124 return const_cast<ParticleT&
>(
128 inline std::vector<ParticlePtr>&
particles() {
return _particles; }
129 inline const std::vector<ParticlePtr>&
particles()
const {
140 #endif // header-guard
std::shared_ptr< GmappingWorld > ParticlePtr
std::vector< ParticlePtr > & particles()
const std::vector< ParticlePtr > & particles() const
ParticleFilter(std::shared_ptr< ParticleFactory< ParticleT >> p_ftry, unsigned n=1)
const ParticleT & heaviest_particle() const
UniformResamling< ParticlePtr > _resampler
std::shared_ptr< ParticleFactory< ParticleT > > _particle_supplier
std::vector< ParticlePtr > _particles
void set_weight(double w)
virtual ~Particle()=default
ParticleT & heaviest_particle()