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 <KLib/math/filter/particles/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 K::ParticleFilterResampling<State> {
|
|
|
|
private:
|
|
|
|
/** this is a copy of the particle-set to draw from it */
|
|
std::vector<K::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<K::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 K::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 K::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
|