added fixelag smoothing

code/particles/smoothing/MySmoothingTransition.h

@@ -0,0 +1,145 @@
+#ifndef MYSMOOTHINGTRANSITION_H
+#define MYSMOOTHINGTRANSITION_H
+
+#include <KLib/math/filter/particles/ParticleFilterTransition.h>
+#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Uniform.h>
+
+#include <Indoor/nav/dijkstra/Dijkstra.h>
+#include <Indoor/grid/Grid.h>
+
+#include "../MyState.h"
+#include "../MyControl.h"
+
+#include "../../DijkstraMapper.h"
+
+#include "../../toni/barometric.h"
+#include <map>
+
+static double smoothing_walk_mu = 0.7;
+static double smoothing_walk_sigma = 0.5;
+static double smoothing_heading_sigma = 15.0;
+static double smoothing_baro_sigma = 0.2;
+static bool smoothing_baro_use_hPa = false;
+static double smoothing_baro_with_measurement = false;
+
+class MySmoothingTransition : public K::BackwardFilterTransition<MyState> {
+
+private:
+
+    /** the created grid to draw transitions from */
+    Grid<MyGridNode>* grid;
+
+	/** a simple normal distribution */
+	K::NormalDistribution distWalk;
+
+
+public:
+
+	/**
+	 * ctor
+	 * @param choice the choice to use for randomly drawing nodes
+	 * @param fp the underlying floorplan
+	 */
+    MySmoothingTransition(Grid<MyGridNode>* grid) :
+        grid(grid), distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
+        distWalk.setSeed(4321);
+	}
+
+public:
+
+	uint64_t ts = 0;
+	uint64_t deltaMS = 0;
+
+	/** set the current time in millisconds */
+	void setCurrentTime(const uint64_t ts) {
+		if (this->ts == 0) {
+			this->ts = ts;
+			deltaMS = 0;
+		} else {
+            deltaMS = this->ts - ts;
+			this->ts = ts;
+		}
+	}
+
+    /**
+     * smoothing transition starting at T with t, t-1,...0
+     * @param particles_new p_t (Forward Filter)
+     * @param particles_old p_t+1 (Smoothed Particles from Step before)
+     */
+    std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
+                                                std::vector<K::Particle<MyState>>const& particles_old ) override {
+
+
+        // calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
+        // this means, predict all possible states q_t+1 with all passible states q_t
+        // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
+        // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
+        std::vector<std::vector<double>> predictionProbabilities;
+
+        auto p1 = particles_old.begin();
+        auto p2 = particles_new.begin();
+
+        #pragma omp parallel for private(p2) shared(predictionProbabilities)
+        for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) {
+            std::vector<double> innerVector;
+            for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){
+
+                // find the node (square) the particle is within
+                // just to be safe, we round z to the nearest floor
+
+                //TODO:: Nullptr check! sometimes src/dst can be nullptr
+                //const Node3* dst = graph->getNearestNode(p1->state.x_cm, p1->state.y_cm, std::round(p1->state.z_nr));
+                //const Node3* src = graph->getNearestNode(p2->state.x_cm, p2->state.y_cm, std::round(p2->state.z_nr));
+
+                const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z));
+                const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z));
+
+                Dijkstra<MyGridNode> dijkstra;
+                dijkstra.build(src, dst, DijkstraMapper(*grid));
+
+                double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
+
+                const double distProb = distWalk.getProbability(distDijkstra_m);
+
+                //getProb using the angle(heading) between src and dst
+                double angle = 0.0;
+                if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
+                    angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
+                }
+                const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
+
+                //assert(headingProb != 0.0);
+                //assert(distProb != 0.0);
+
+                //check how near we are to the measurement
+                double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
+
+                //combine the probabilities
+                double prob = distProb * floorProb * headingProb;
+                innerVector.push_back(prob);
+
+                //if(distance_m != distance_m) {throw "detected NaN";}
+                //if(distProb != distProb) {throw "detected NaN";}
+                if(angle != angle) {throw "detected NaN";}
+                if(headingProb != headingProb) {throw "detected NaN";}
+                if(floorProb != floorProb) {throw "detected NaN";}
+                if(floorProb == 0) {throw "detected NaN";}
+                if(prob != prob) {throw "detected NaN";}
+
+                //assert(prob != 0.0);
+
+
+            }
+            #pragma omp critical
+            predictionProbabilities.push_back(innerVector);
+        }
+        return predictionProbabilities;
+
+    }
+
+
+};
+
+#endif // MYTRANSITION_H