diff --git a/navMesh/walk/NavMeshWalkKLD.h b/navMesh/walk/NavMeshWalkKLD.h index ca7d31e..086e5b4 100644 --- a/navMesh/walk/NavMeshWalkKLD.h +++ b/navMesh/walk/NavMeshWalkKLD.h @@ -81,7 +81,7 @@ namespace NM { // to-be-walked distance; const float toBeWalkedDist = params.getToBeWalkedDistance(); const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; - const float toBeWalkedDistKld = (kld * qualityWifi); + const float toBeWalkedDistKld = (kld * qualityWifi); // construct reachable region NavMeshSub reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD... diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 03ca2a5..c224427 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -59,7 +59,7 @@ namespace NM { // to-be-walked distance; const float toBeWalkedDist = params.getToBeWalkedDistance(); - const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; + const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; // construct reachable region NavMeshSub reachable(params.start, toBeWalkedDistSafe); @@ -73,15 +73,15 @@ namespace NM { // is above destination reachable? if (dstTria) { - re.heading = params.heading; // heading was OK -> keep - re.location.pos = dstTria->toPoint3(dst); // new destination position - re.location.tria = dstTria; // new destination triangle + re.heading = params.heading; // heading was OK -> keep + re.location.pos = dstTria->toPoint3(dst); // new destination position + re.location.tria = dstTria; // new destination triangle ++hits; } else { - NavMeshRandom rnd = reachable.getRandom(); // random-helper - re.location = rnd.draw(); // get a random destianation + NavMeshRandom rnd = reachable.getRandom(); // random-helper + re.location = rnd.draw(); // get a random destianation re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading ++misses; 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/ParticleFilterResampling.h b/smc/filtering/resampling/ParticleFilterResampling.h index 906e91f..b62bfc3 100644 --- a/smc/filtering/resampling/ParticleFilterResampling.h +++ b/smc/filtering/resampling/ParticleFilterResampling.h @@ -24,10 +24,10 @@ namespace SMC { public: - /** - * perform resampling on the given particle-vector - * @param particles the vector of all particles to resample - */ + /** + * perform resampling on the given particle-vector + * @param particles the vector of all particles to resample + */ virtual void resample(std::vector>& particles) = 0; /** 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 diff --git a/smc/filtering/resampling/ParticleFilterResamplingPercent.h b/smc/filtering/resampling/ParticleFilterResamplingPercent.h index 42ec764..c992d5a 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingPercent.h +++ b/smc/filtering/resampling/ParticleFilterResamplingPercent.h @@ -42,29 +42,25 @@ namespace SMC { // to-be-removed region - const int start = particles.size() * (1-percent); - const int end = particles.size(); + const int start = particles.size() * (1-percent); + const int end = particles.size(); std::uniform_int_distribution dist(0, start-1); // remove by re-drawing for (uint32_t i = start; i < end; ++i) { const int rnd = dist(gen); particles[i] = particles[rnd]; - particles[i].weight /= 2; - particles[rnd].weight /= 2; + //particles[i].weight /= 2; + //particles[rnd].weight /= 2; } // calculate weight-sum double weightSum = 0; - for (const auto& p : particles) { + double equalweight = 1.0 / (double) cnt; + for (auto& p : particles) { weightSum += p.weight; + p.weight = equalweight; } - - - int i = 0; - - - } private: diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h index aec9329..5144135 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h +++ b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h @@ -46,7 +46,7 @@ namespace SMC { // compile-time sanity checks // TODO: this solution requires EXPLICIT overloading which is bad... - // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); const uint32_t cnt = (uint32_t) particles.size(); @@ -67,20 +67,21 @@ namespace SMC { particlesCopy[i].weight = cumWeight; } - // randomness for drawing particles - std::uniform_real_distribution distNewOne(0.0, 1.0); + // randomness for drawing particles + std::uniform_real_distribution distNewOne(0.0, 1.0); // 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 particle in range X m - if (distNewOne(gen) < 0.001) { - const NM::NavMeshSub reachable(particlesCopy[i].state.pos, 10.0); - particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0); - particles[i].weight = equalWeight; - continue; - } + // slight chance to get a truely random particle in range X m + if (distNewOne(gen) < 0.001) { + const double radius = 50.0; + const NM::NavMeshSub reachable(particlesCopy[i].state.loc, radius); + particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius); + particles[i].weight = equalWeight; + continue; + } particles[i] = draw(cumWeight); particles[i].weight = equalWeight; diff --git a/smc/smoothing/FastKDESmoothing.h b/smc/smoothing/FastKDESmoothing.h index 55c55c3..8e00464 100644 --- a/smc/smoothing/FastKDESmoothing.h +++ b/smc/smoothing/FastKDESmoothing.h @@ -210,6 +210,15 @@ namespace SMC { BoxGaus boxGaus; boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + /** + * Das hier wird nicht funktionieren! + * Wir machen eine Transition von t zu t+1 und nehmen dann diese position um von der kde aus t+1 ein + * Gewicht zu erhalten... aber das bringt ja nichts, diese Gewicht haben wir doch schon auf den partikeln in t+1 + * die transition macht ja nicht 2x was anderes, sondern wieder genau das gleiche. + * was das smoothing gut macht, sind die Faltungen mit allen Partikeln (jeder mit jedem vergleichen). + **/ + + // Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T) // Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation smoothedParticles = forwardHistory.getParticleSet(i);