added boxkde resamplin in 2D and 3D

read activity out of sensor offline file

grid/walk/v2/modules/WalkModule.h

@@ -2,6 +2,7 @@
 #define WALKMODULE_H
 
 #include "../../../Grid.h"
+#include "../../../GridPoint.h"
 
 /** base-class for all WalkStates */
 struct WalkState {

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -40,7 +40,7 @@ public:
 
 		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
 		// reset this one after every transition!
-		Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
+                //Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
 
 		// sensor noise
 		float var = distNoise.draw();

navMesh/walk/NavMeshWalkSimple.h

@@ -82,7 +82,7 @@ namespace NM {
 
                                 NavMeshRandom<Tria> rnd = reachable.getRandom();			// random-helper
                                 re.location = rnd.draw();						// get a random destianation
-				re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
+                                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
 				++misses;
 
 			}

sensors/activity/Activity.h

@@ -3,9 +3,9 @@
 
 enum class Activity {
 
-	STANDING,
+        WALKING,
 
-	WALKING,
+        STANDING,
 
 	WALKING_UP,
 

sensors/offline/FileReader.h

@@ -43,6 +43,7 @@ namespace Offline {
 
 		/** all entries grouped by sensor */
 		std::vector<TS<int>> groundTruth;
+        std::vector<TS<int>> activity;
 		std::vector<TS<WiFiMeasurements>> wifi;
 		std::vector<TS<BeaconMeasurement>> beacon;
 		std::vector<TS<AccelerometerData>> acc;
@@ -95,6 +96,7 @@ namespace Offline {
 
 		const std::vector<Entry>& getEntries() const {return entries;}
 
+        const std::vector<TS<int>>& getActivity() const {return activity;}
 
 		const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
 
@@ -187,6 +189,7 @@ namespace Offline {
 				else if (idx == (int)Sensor::COMPASS)		{parseCompass(ts,data);}
 				else if (idx == (int)Sensor::GPS)			{parseGPS(ts,data);}
 				else if (idx == (int)Sensor::MAGNETOMETER)	{parseMagnetometer(ts,data);}
+                else if (idx == (int)Sensor::ACTIVITY)      {parseActivity(ts, data);}
 
 				// TODO: this is a hack...
 				// the loop is called one additional time after the last entry
@@ -327,6 +330,20 @@ namespace Offline {
 
 		}
 
+        void parseActivity(const uint64_t ts, const std::string& data) {
+
+            const auto pos1 = data.find(';');
+            const auto pos2 = data.find(';', pos1+1);
+
+            const std::string string = data.substr(0, pos1);
+            const std::string id = data.substr(pos1+1, pos2);
+
+            TS<int> elem(ts, std::stoi(id));
+            activity.push_back(elem);
+            entries.push_back(Entry(Sensor::ACTIVITY, ts, activity.size()-1));
+
+        }
+
 		void parseBarometer(const uint64_t ts, const std::string& data) {
 
 			BarometerData baro;

sensors/offline/Sensors.h

@@ -14,6 +14,7 @@ namespace Offline {
 		WIFI = 8,
 		BEACON = 9,
 		GPS = 16,
+        ACTIVITY = 50,
 		GROUND_TRUTH = 99,
 		POS = 1001,			// IPIN2016
 	};

smc/filtering/ParticleFilter.h

@@ -138,7 +138,7 @@ namespace SMC {
 			if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); }
 
 			// perform the transition step
-			transition->transition(particles, control);
+                        transition->transition(particles, control);
 
 			// perform the evaluation step and calculate the sum of all particle weights
 			evaluation->evaluation(particles, observation);

smc/filtering/ParticleFilterInitializer.h

@@ -1,5 +1,5 @@
-#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
+#ifndef SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
-#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
+#define SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
 
 #include <vector>
 #include "../Particle.h"
@@ -22,4 +22,4 @@ namespace SMC {
 
 }
 
-#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
+#endif // SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H

smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h

@@ -2,6 +2,7 @@
 #define PARTICLEFILTERESTIMATIONBOXKDE3D_H
 
 #include <vector>
+#include <memory>
 #include "../../Particle.h"
 #include "../../ParticleAssertions.h"
 #include "ParticleFilterEstimation.h"

smc/filtering/resampling/ParticleFilterResamplingKDE.h

@@ -77,7 +77,11 @@ namespace SMC {
             grid->clear();
             for (Particle<State> p : particles){
                 //grid.add receives position in meter!
-                grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
+
+                //if weight is to low, remove.
+                if((float) p.weight > 0 ){
+                    grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
+                }
             }
 
             int nFilt = 3;
@@ -88,7 +92,7 @@ namespace SMC {
             BoxGaus3D<float> boxGaus;
             boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
 
-            // fill a drawlist with 10k equal distributed particles
+            // fill a drawlist with N equal distributed particles
             // assign them a weight based on the KDE density.
             DrawList<Point3> dl;
             for (int i = 0; i < 10000; ++i){
@@ -110,27 +114,6 @@ namespace SMC {
                 particles[i].state.pos = 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;
-
         }
     };
 

smc/filtering/resampling/ParticleFilterResamplingPercent.h

@@ -42,8 +42,8 @@ namespace SMC {
 
 
 			// to-be-removed region
-                        const int start = particles.size() * (1-percent);
+            const int start = particles.size() * (1-percent);
-                        const int end = particles.size();
+            const int end = particles.size();
 			std::uniform_int_distribution<int> dist(0, start-1);
 
 			// remove by re-drawing
@@ -56,10 +56,10 @@ namespace SMC {
 
 			// calculate weight-sum
 			double weightSum = 0;
-                        double equalweight = 1.0 / (double) cnt;
+            double equalweight = 1.0 / (double) cnt;
-                        for (auto& p : particles) {
+            for (auto& p : particles) {
 				weightSum += p.weight;
-                                p.weight = equalweight;
+                p.weight = equalweight;
 			}
 		}
 

smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h

@@ -45,8 +45,8 @@ namespace SMC {
 		void resample(std::vector<Particle<State>>& particles) override {
 
 			// compile-time sanity checks
-			// TODO: this solution requires EXPLICIT overloading which is bad...
+            // TODO: this solution requires EXPLICIT overloading which is bad...
-                        // static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
+            // static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
 
 			const uint32_t cnt = (uint32_t) particles.size();
 
@@ -67,21 +67,21 @@ namespace SMC {
 				particlesCopy[i].weight = cumWeight;
 			}
 
-                        // randomness for drawing particles
+            // randomness for drawing particles
-                        std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
+            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 random particle in range X m
+                // slight chance to get a truely random particle in range X m
-                                if (distNewOne(gen) < 0.005) {
+                if (distNewOne(gen) < 0.005) {
-                                    const double radius = 50.0;
+                    const double radius = 50.0;
-                                    const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.loc, radius);
+                    const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.pos, radius);
-                                    particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius);
+                    particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, radius);
-                                    particles[i].weight = equalWeight;
+                    particles[i].weight = equalWeight;
-                                    continue;
+                    continue;
-                                }
+                }
 
 				particles[i] = draw(cumWeight);
 				particles[i].weight = equalWeight;

smc/filtering/resampling/particlefilterresamplingsimpleimpoverishmentgrid.h

@@ -0,0 +1,129 @@
+#ifndef PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H
+#define PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H
+
+
+#include <algorithm>
+#include <random>
+
+#include "ParticleFilterResampling.h"
+#include "../../ParticleAssertions.h"
+
+#include <Indoor/grid/Grid.h>
+
+namespace SMC {
+
+        /**
+         * uses simple probability resampling by drawing particles according
+         * to their current weight.
+         * O(log(n)) per particle
+         */
+    template <typename State, typename Node>
+    class ParticleFilterResamplingSimpleImpoverishmentGrid : 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;
+
+                Grid<Node>& grid;
+
+        public:
+
+                /** ctor */
+                ParticleFilterResamplingSimpleImpoverishmentGrid(Grid<Node>& grid) : grid(grid) {
+                        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);
+                        std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
+
+                        // 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 random particle in range X m
+                            if (distNewOne(gen) < 0.005) {
+                                particles[i].state.position = grid[distRndNode(gen)];
+                                particles[i].weight = equalWeight;
+                                continue;
+                            }
+
+                            particles[i] = draw(cumWeight);
+                            particles[i].weight = equalWeight;
+                        }
+
+                }
+
+        private:
+                /** draw a node randomly within a specific range **/
+                const Node drawNode(const Node baseNode, const double range){
+                    // we draw neighbors randomly until the range was reached
+
+                    Node* curNode = baseNode;
+
+                    while(range > 0){
+                        DrawList<const Node*> drawer(rnd); drawer.reserve(10);
+
+                        for (const Node& neighbor : grid.neighbors(*curNode)) {
+                                drawer.add(&neighbor, prob);
+                                maxEdgeProb.add(prob);
+                        }
+
+                    }
+
+                }
+
+
+                /** 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 // PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H