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
124 lines
3.3 KiB
C++
124 lines
3.3 KiB
C++
#ifndef NODERESAMPLING_H
|
|
#define NODERESAMPLING_H
|
|
|
|
|
|
#include <algorithm>
|
|
#include <random>
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
|
|
|
|
|
|
/**
|
|
* uses simple probability resampling by drawing particles according
|
|
* to their current weight.
|
|
* HOWEVER: after drawing them, do NOT use them directly, but replace them with a neighbor
|
|
* O(log(n)) per particle
|
|
*/
|
|
template <typename State, typename Node>
|
|
class NodeResampling : public SMC::ParticleFilterResampling<State> {
|
|
|
|
private:
|
|
|
|
/** this is a copy of the particle-set to draw from it */
|
|
std::vector<SMC::Particle<State>> particlesCopy;
|
|
|
|
/** random number generator */
|
|
std::minstd_rand gen;
|
|
|
|
Grid<Node>& grid;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
NodeResampling(Grid<Node>& grid) : grid(grid) {
|
|
gen.seed(1234);
|
|
}
|
|
|
|
void resample(std::vector<SMC::Particle<State>>& particles) override {
|
|
|
|
// compile-time sanity checks
|
|
// TODO: this solution requires EXPLICIT overloading which is bad...
|
|
//static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
|
|
|
|
const uint32_t cnt = (uint32_t) particles.size();
|
|
|
|
// equal weight for all particles. sums up to 1.0
|
|
const double equalWeight = 1.0 / (double) cnt;
|
|
|
|
// ensure the copy vector has the same size as the real particle vector
|
|
particlesCopy.resize(cnt);
|
|
|
|
// swap both vectors
|
|
particlesCopy.swap(particles);
|
|
|
|
// calculate cumulative weight
|
|
double cumWeight = 0;
|
|
for (uint32_t i = 0; i < cnt; ++i) {
|
|
cumWeight += particlesCopy[i].weight;
|
|
particlesCopy[i].weight = cumWeight;
|
|
}
|
|
|
|
std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
|
|
std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
|
|
std::normal_distribution<float> distTurn(0.0, +0.03);
|
|
|
|
// now draw from the copy vector and fill the original one
|
|
// with the resampled particle-set
|
|
for (uint32_t i = 0; i < cnt; ++i) {
|
|
|
|
// slight chance to get a truely random node as particle
|
|
// mainly for testing
|
|
if (distNewOne(gen) < 0.005) {
|
|
particles[i].state.position = grid[distRndNode(gen)];
|
|
particles[i].weight = equalWeight;
|
|
continue;
|
|
}
|
|
|
|
// normal redraw procedure
|
|
particles[i] = draw(cumWeight);
|
|
particles[i].weight = equalWeight;
|
|
|
|
const Node* n = grid.getNodePtrFor(particles[i].state.position);
|
|
if (n == nullptr) {continue;} // should not happen!
|
|
|
|
for (int j = 0; j < 2; ++j) {
|
|
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
|
|
const int idx = distIdx(gen);
|
|
n = &grid.getNeighbor(*n, idx);
|
|
}
|
|
|
|
|
|
particles[i].state.position = *n;
|
|
particles[i].state.heading.direction += distTurn(gen);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
/** draw one particle according to its weight from the copy vector */
|
|
const SMC::Particle<State>& draw(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<State>& s, const float d) {return s.weight < d;};
|
|
auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
|
|
return *it;
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // NODERESAMPLING_H
|