diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h index 1e7b487..1a0b668 100644 --- a/smc/filtering/ParticleFilter.h +++ b/smc/filtering/ParticleFilter.h @@ -160,7 +160,7 @@ namespace SMC { void updateTransitionOnly(const Control* control) { // sanity checks (if enabled) - Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); // perform the transition step transition->transition(particles, control); @@ -171,12 +171,13 @@ namespace SMC { State updateEvaluationOnly(const Observation& observation) { // sanity checks (if enabled) - Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); - Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); - // if the number of efficient particles is too low, perform resampling - if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + // if the number of efficient particles is too low, perform resampling + if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + //resampler->resample(particles); // perform the evaluation step and calculate the sum of all particle weights evaluation->evaluation(particles, observation); @@ -187,7 +188,7 @@ namespace SMC { //Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); // normalize the particle weights and thereby calculate N_eff - lastNEff = normalize(); + lastNEff = normalize(); // estimate the current state const State est = estimation->estimate(particles); diff --git a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h index f247403..4d4f21e 100644 --- a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h +++ b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h @@ -39,7 +39,7 @@ namespace SMC { public: ParticleFilterEstimationBoxKDE(){ - //fuck off + //keine boesen woerter } ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){ @@ -79,7 +79,7 @@ namespace SMC { static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); //TODO: check for function getX() and getY() - //TODO: fixed this hack + //TODO: fixe this hack State tmpAVG; double weightSum = 0; @@ -89,12 +89,12 @@ namespace SMC { //grid.add receives position in meter! grid.add(p.state.getX(), p.state.getY(), p.weight); - //TODO: fixed this hack + //TODO: fixe this hack //get the z value by using the weighted average z! tmpAVG += p.state * p.weight; weightSum += p.weight; } - //TODO: fixed this hack + //TODO: fixe this hack tmpAVG /= weightSum; int nFilt = 3; diff --git a/smc/filtering/resampling/ParticleFilterResamplingKDE.h b/smc/filtering/resampling/ParticleFilterResamplingKDE.h new file mode 100644 index 0000000..f038ab6 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingKDE.h @@ -0,0 +1,155 @@ +#ifndef PARTICLEFILTERRESAMPLINGKDE_H +#define PARTICLEFILTERRESAMPLINGKDE_H + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include "../../../math/boxkde/benchmark.h" +#include "../../../math/boxkde/DataStructures.h" +#include "../../../math/boxkde/Image2D.h" +#include "../../../math/boxkde/BoxGaus.h" +#include "../../../math/boxkde/Grid2D.h" +#include "../../../math/distribution/Normal.h" + +#include "../../../navMesh/NavMesh.h" + +namespace SMC { + + /** + * Resample based on rapid KDE + */ + template + class ParticleFilterResamplingKDE : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + /** boundingBox for the boxKDE */ + BoundingBox bb; + + /** histogram/grid holding the particles*/ + Grid2D grid; + + /** bandwith for KDE */ + Point2 bandwith; + + /** the current mesh */ + const NM::NavMesh* mesh; + + public: + + /** ctor */ + ParticleFilterResamplingKDE(const NM::NavMesh* mesh, const float gridsize_m, const Point2 bandwith) { + + const Point3 maxBB = mesh->getBBox().getMax(); + const Point3 minBB = mesh->getBBox().getMin(); + this->bb = BoundingBox(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10); + + // Create histogram + size_t nBinsX = static_cast((maxBB.x - minBB.x) / gridsize_m); + size_t nBinsY = static_cast((maxBB.y - minBB.y) / gridsize_m); + this->grid = Grid2D(bb, nBinsX, nBinsY); + + this->bandwith = bandwith; + this->mesh = mesh; + + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + //static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); + //todo: static assert for getx, gety, getz, setposition + + State tmpAVG; + double weightSum = 0; + + grid.clear(); + for (Particle p : particles){ + //grid.add receives position in meter! + grid.add(p.state.getX(), p.state.getY(), p.weight); + + //TODO: fixe this hack + //get the z value by using the weighted average z! + tmpAVG += p.state * p.weight; + weightSum += p.weight; + } + //TODO: fixe this hack with z... + tmpAVG /= weightSum; + + int nFilt = 3; + float sigmaX = bandwith.x / grid.binSizeX; + float sigmaY = bandwith.y / grid.binSizeY; + BoxGaus boxGaus; + boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + + // fill a drawlist with 10k equal distributed particles + // assign them a weight based on the KDE density. + DrawList dl; + Distribution::Normal zProb(tmpAVG.getZ(), 4.0); + for (int i = 0; i < 10000; ++i){ + + //todo: wir ziehen natürlich hier aus dem ganzen gebäude, bekommen + // aber ein gewicht nur basierend auf 2D. deshalb kleiner hack und z + // mit einer normalverteilung gewichtet, wobei mean = avg_z der partikel + // menge ist + NM::NavMeshLocation tmpPos = mesh->getRandom().draw(); + double weight = grid.image().get(tmpPos.pos.x, tmpPos.pos.y); + //weight *= zProb.getProbability(tmpPos.pos.z); + + dl.add(tmpPos.pos, weight); + } + + int dummyyy = 0; + // used the same particles to not lose the heading. + + // TODO: Summe aller Partikel wird relativ schnell 0! Ich vermute da am Anfang ein einzelner Partikel stark gewichtet ist und alleine + // die Dichte repräsentiert über die KDE. Jetzt wird beim nächsten zufälligen ziehen an dieser Stelle keiner der 10k partikel dort gezogen, d.h. alle + // haben ein Gewicht von 0 und ciao. + // Lösung: erstmal equal weight versuchen. ansonten: warum nehmen wir nochmal diese 10k? und nciht einfach die partikel die wir eh schon haben? + for (int i = 0; i < particles.size(); ++i){ + + double tmpWeight = 1; + particles[i].state.loc = mesh->getLocation(dl.get(tmpWeight)); + particles[i].weight = tmpWeight; + } + + //Todo:: equal weight? brauch ich das ueberhaupt? grundlage sind ja 10k zufällig + int dummy = 0; + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution 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 Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + }; + + +} + +#endif // PARTICLEFILTERRESAMPLINGKDE_H