625f5fe04d
removed KLib stuff added new activity filter is uncommand! at the moment, the app is not able to load new maps and breaks using old maps
73 lines
1.9 KiB
C++
73 lines
1.9 KiB
C++
#ifndef REGIONALRESAMPLING_H
|
|
#define REGIONALRESAMPLING_H
|
|
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
|
|
#include "State.h"
|
|
|
|
namespace GridBased {
|
|
|
|
class RegionalResampling : public SMC::ParticleFilterResampling<MyState> {
|
|
|
|
public:
|
|
|
|
float maxDist = 12.5;
|
|
|
|
RegionalResampling() {;}
|
|
|
|
void resample(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
|
|
Point3 sum;
|
|
for (const SMC::Particle<MyState>& p : particles) {
|
|
sum += p.state.position.inMeter();
|
|
}
|
|
const Point3 avg = sum / particles.size();
|
|
|
|
std::vector<SMC::Particle<MyState>> next;
|
|
for (const SMC::Particle<MyState>& p : particles) {
|
|
const float dist = p.state.position.inMeter().getDistance(avg);
|
|
if (rand() % 6 != 0) {continue;}
|
|
if (dist < maxDist) {next.push_back(p);}
|
|
}
|
|
|
|
// cumulate
|
|
std::vector<SMC::Particle<MyState>> copy = particles;
|
|
double cumWeight = 0;
|
|
for ( SMC::Particle<MyState>& p : copy) {
|
|
cumWeight += p.weight;
|
|
p.weight = cumWeight;
|
|
}
|
|
|
|
// draw missing particles
|
|
const int missing = particles.size() - next.size();
|
|
for (int i = 0; i < missing; ++i) {
|
|
next.push_back(draw(copy, cumWeight));
|
|
}
|
|
|
|
std::swap(next, particles);
|
|
|
|
}
|
|
|
|
std::minstd_rand gen;
|
|
|
|
/** draw one particle according to its weight from the copy vector */
|
|
const SMC::Particle<MyState>& draw(std::vector<SMC::Particle<MyState>>& copy, const double cumWeight) {
|
|
|
|
// generate random values between [0:cumWeight]
|
|
std::uniform_real_distribution<float> dist(0, cumWeight);
|
|
|
|
// draw a random value between [0:cumWeight]
|
|
const float rand = dist(gen);
|
|
|
|
// search comparator (cumWeight is ordered -> use binary search)
|
|
auto comp = [] (const SMC::Particle<MyState>& s, const float d) {return s.weight < d;};
|
|
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
|
|
return *it;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // REGIONALRESAMPLING_H
|