added estimation for BOXKDE

added resampling with simple random particle against sample impoverishment

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;
 

smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h

@@ -0,0 +1,104 @@
+#ifndef PARTICLEFILTERESTIMATIONBOXKDE_H
+#define PARTICLEFILTERESTIMATIONBOXKDE_H
+
+#include <vector>
+#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 <typename State>
+    class ParticleFilterEstimationBoxKDE : public ParticleFilterEstimation<State> {
+
+    private:
+        /** boundingBox for the boxKDE */
+        BoundingBox<float> bb;
+
+        /** histogram/grid holding the particles*/
+        Grid2D<float> 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<float>(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10);
+
+            // Create histogram
+            size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_m);
+            size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_m);
+            this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
+
+            this->bandwith = bandwith;
+        }
+
+        State estimate(const std::vector<Particle<State>>& particles) override {
+
+            // compile-time sanity checks
+            static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+            static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+            static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+            static_assert( std::is_constructible<State, Point3>::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<State> 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<float> boxGaus;
+            boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
+
+            //TODO: this is pretty ugly... we should only use one basic point type
+            Point2D<float> 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

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 <algorithm>
+#include <random>
+
+#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 <typename State, typename Tria>
+    class ParticleFilterResamplingSimpleImpoverishment : public ParticleFilterResampling<State> {
+
+	private:
+
+		/** this is a copy of the particle-set to draw from it */
+		std::vector<Particle<State>> particlesCopy;
+
+		/** random number generator */
+		std::minstd_rand gen;
+
+	public:
+
+		/** ctor */
+        ParticleFilterResamplingSimpleImpoverishment() {
+			gen.seed(1234);
+		}
+
+		void resample(std::vector<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;
+			}
+
+            // randomness for drawing particles
+            std::uniform_real_distribution<float> 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<Tria> 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<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 Particle<State>& 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_ */