Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

smc/filtering/ParticleFilter.h

@@ -177,6 +177,7 @@ namespace SMC {
 
             // 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);

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<State, Point3>::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;

smc/filtering/resampling/ParticleFilterResamplingKDE.h

@@ -0,0 +1,155 @@
+#ifndef PARTICLEFILTERRESAMPLINGKDE_H
+#define PARTICLEFILTERRESAMPLINGKDE_H
+
+#include <algorithm>
+#include <random>
+
+#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 <typename State, typename Tria>
+    class ParticleFilterResamplingKDE : 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;
+
+        /** boundingBox for the boxKDE */
+        BoundingBox<float> bb;
+
+        /** histogram/grid holding the particles*/
+        Grid2D<float> grid;
+
+        /** bandwith for KDE */
+        Point2 bandwith;
+
+        /** the current mesh */
+        const NM::NavMesh<Tria>* mesh;
+
+    public:
+
+        /** ctor */
+        ParticleFilterResamplingKDE(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;
+            this->mesh = mesh;
+
+            gen.seed(1234);
+        }
+
+        void resample(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: static assert for getx, gety, getz, setposition
+
+            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: 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<float> 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<Point3> dl;
+            Distribution::Normal<float> 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<Tria> 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<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 // PARTICLEFILTERRESAMPLINGKDE_H

smc/filtering/resampling/ParticleFilterResamplingPercent.h

@@ -50,21 +50,17 @@ namespace SMC {
 			for (uint32_t i = start; i < end; ++i) {
 				const int rnd = dist(gen);
 				particles[i] = particles[rnd];
-				particles[i].weight /= 2;
+                                //particles[i].weight /= 2;
-				particles[rnd].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:

smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h

@@ -74,10 +74,11 @@ namespace SMC {
 			// with the resampled particle-set
 			for (uint32_t i = 0; i < cnt; ++i) {
 
-                // slight chance to get a truely particle in range X m
+                                // slight chance to get a truely random particle in range X m
                                 if (distNewOne(gen) < 0.001) {
-                    const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.pos, 10.0);
+                                    const double radius = 50.0;
-                    particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0);
+                                    const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.loc, radius);
+                                    particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius);
                                     particles[i].weight = equalWeight;
                                     continue;
                                 }

smc/smoothing/FastKDESmoothing.h

@@ -210,6 +210,15 @@ namespace SMC {
                 BoxGaus<float> 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);