diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 06eaa55..03ca2a5 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -89,7 +89,7 @@ namespace NM { const int total = (hits + misses); if (total % 10000 == 0) { - std::cout << "hits: " << (hits*100/total) << "%" << std::endl; + //std::cout << "hits: " << (hits*100/total) << "%" << std::endl; } // calculate probability diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h index 9987908..1e7b487 100644 --- a/smc/filtering/ParticleFilter.h +++ b/smc/filtering/ParticleFilter.h @@ -143,8 +143,8 @@ namespace SMC { // perform the evaluation step and calculate the sum of all particle weights evaluation->evaluation(particles, observation); - // normalize the particle weights and thereby calculate N_eff - lastNEff = normalize(); + // normalize the particle weights and thereby calculate N_eff + lastNEff = normalize(); //std::cout << "normalized. n_eff is " << lastNEff << std::endl; @@ -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); @@ -175,6 +175,9 @@ namespace SMC { Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() 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); } + // perform the evaluation step and calculate the sum of all particle weights evaluation->evaluation(particles, observation); @@ -184,14 +187,11 @@ 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 - const double neff = normalize(); + lastNEff = normalize(); // estimate the current state const State est = estimation->estimate(particles); - // if the number of efficient particles is too low, perform resampling - if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } - // done return est; diff --git a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h new file mode 100644 index 0000000..e115656 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h @@ -0,0 +1,104 @@ +#ifndef PARTICLEFILTERESTIMATIONBOXKDE_H +#define PARTICLEFILTERESTIMATIONBOXKDE_H + +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" +#include "../../../Assertions.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 "../../../grid/Grid.h"; +#include "../../../floorplan/v2/FloorplanHelper.h"; + +namespace SMC { + + /** + * calculate an estimation based on the fast + * boxed KDE of Bulli + */ + template + class ParticleFilterEstimationBoxKDE : public ParticleFilterEstimation { + + private: + /** boundingBox for the boxKDE */ + BoundingBox bb; + + /** histogram/grid holding the particles*/ + Grid2D grid; + + /** bandwith for KDE */ + Point2 bandwith; + + public: + + ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){ + + const Point3 maxBB = FloorplanHelper::getBBox(map).getMax(); + const Point3 minBB = FloorplanHelper::getBBox(map).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; + } + + State estimate(const 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: check for function getX() and getY() + + //TODO: fixed this hack + 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: fixed this hack + //get the z value by using the weighted average z! + tmpAVG += p.state * p.weight; + weightSum += p.weight; + } + //TODO: fixed this hack + 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); + + //TODO: this is pretty ugly... we should only use one basic point type + Point2D maxPos; + double weight = grid.maximum(maxPos); + + Assert::isTrue( (weight == weight), "the sum of particle weights is NaN!"); + Assert::isTrue( (weight != 0), "the sum of particle weights is null!"); + + //this depends on the given state + Point3 maxPos3(maxPos.X, maxPos.Y, tmpAVG.pos.pos.z); + State tmp(maxPos3); + + return tmp; + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONBOXKDE_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h new file mode 100644 index 0000000..cf901f0 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h @@ -0,0 +1,118 @@ +/* + * ParticleResamplingSimple.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENT_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENT_H_ + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include "../../../navMesh/NavMeshRandom.h" +#include "../../../navMesh/walk/NavMeshSub.h" + +namespace SMC { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight. + * O(log(n)) per particle + */ + template + class ParticleFilterResamplingSimpleImpoverishment : 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; + + public: + + /** ctor */ + ParticleFilterResamplingSimpleImpoverishment() { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::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; + } + + // 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 25m + 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; + } + + particles[i] = draw(cumWeight); + particles[i].weight = equalWeight; + } + + } + + 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 /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENT_H_ */