added code for advanced sample impoverishment using the mesh

smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h

@@ -12,8 +12,10 @@
 #include "../../../math/boxkde/Image2D.h"
 #include "../../../math/boxkde/BoxGaus.h"
 #include "../../../math/boxkde/Grid2D.h"
-#include "../../../grid/Grid.h";
-#include "../../../floorplan/v2/FloorplanHelper.h";
+#include "../../../grid/Grid.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+
+#include "../../../navMesh/NavMesh.h"
 
 namespace SMC {
 
@@ -36,6 +38,10 @@ namespace SMC {
 
     public:
 
+        ParticleFilterEstimationBoxKDE(){
+            //fuck off
+        }
+
         ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){
 
             const Point3 maxBB = FloorplanHelper::getBBox(map).getMax();
@@ -50,6 +56,20 @@ namespace SMC {
             this->bandwith = bandwith;
         }
 
+        template <typename Tria> ParticleFilterEstimationBoxKDE(const NM::NavMesh<Tria>* mesh, const float gridsize_m, const Point2 bandwith){
+
+            const Point3 maxBB = mesh->getBBox().getMax();
+            const Point3 minBB = mesh->getBBox().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

smc/filtering/resampling/ParticleFilterResamplingSignalStrengthOnMesh.h

@@ -0,0 +1,109 @@
+#ifndef PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H
+#define PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_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 ParticleFilterResamplingSignalStrengthOnMesh: 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 X m
+                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 // PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H

smc/filtering/resampling/ParticleFilterResamplingSimple.h

@@ -89,9 +89,6 @@ namespace SMC {
 			return *it;
 
 		}
-
-
-
 	};
 
 

smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h

@@ -74,7 +74,7 @@ namespace SMC {
 			// with the resampled particle-set
 			for (uint32_t i = 0; i < cnt; ++i) {
 
-                // slight chance to get a truely particle in range 25m
+                // slight chance to get a truely particle in range X m
                 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);