diff --git a/grid/walk/v2/modules/WalkModule.h b/grid/walk/v2/modules/WalkModule.h index 2d82dc9..f821dd6 100644 --- a/grid/walk/v2/modules/WalkModule.h +++ b/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 { diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 0eb0651..bfb3656 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/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(); diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index c224427..103d033 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -82,7 +82,7 @@ namespace NM { NavMeshRandom 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; } diff --git a/sensors/activity/Activity.h b/sensors/activity/Activity.h index bcacc55..ca7b4e6 100644 --- a/sensors/activity/Activity.h +++ b/sensors/activity/Activity.h @@ -3,9 +3,9 @@ enum class Activity { - STANDING, + WALKING, - WALKING, + STANDING, WALKING_UP, diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 569c5e7..cdd1d3a 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -43,6 +43,7 @@ namespace Offline { /** all entries grouped by sensor */ std::vector> groundTruth; + std::vector> activity; std::vector> wifi; std::vector> beacon; std::vector> acc; @@ -95,6 +96,7 @@ namespace Offline { const std::vector& getEntries() const {return entries;} + const std::vector>& getActivity() const {return activity;} const std::vector>& 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 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; diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h index 80971b9..328302c 100644 --- a/sensors/offline/Sensors.h +++ b/sensors/offline/Sensors.h @@ -14,6 +14,7 @@ namespace Offline { WIFI = 8, BEACON = 9, GPS = 16, + ACTIVITY = 50, GROUND_TRUTH = 99, POS = 1001, // IPIN2016 }; diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h index 1a0b668..b48d972 100644 --- a/smc/filtering/ParticleFilter.h +++ b/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); diff --git a/smc/filtering/ParticleFilterInitializer.h b/smc/filtering/ParticleFilterInitializer.h index 17e9087..2c2a77e 100644 --- a/smc/filtering/ParticleFilterInitializer.h +++ b/smc/filtering/ParticleFilterInitializer.h @@ -1,5 +1,5 @@ -#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H -#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H +#ifndef SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H +#define SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H #include #include "../Particle.h" @@ -22,4 +22,4 @@ namespace SMC { } -#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H +#endif // SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h index aec2bbc..a90e416 100644 --- a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h +++ b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h @@ -2,6 +2,7 @@ #define PARTICLEFILTERESTIMATIONBOXKDE3D_H #include +#include #include "../../Particle.h" #include "../../ParticleAssertions.h" #include "ParticleFilterEstimation.h" diff --git a/smc/filtering/resampling/ParticleFilterResamplingKDE.h b/smc/filtering/resampling/ParticleFilterResamplingKDE.h index 349d61f..3fc3ed3 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingKDE.h +++ b/smc/filtering/resampling/ParticleFilterResamplingKDE.h @@ -77,7 +77,11 @@ namespace SMC { grid->clear(); for (Particle 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 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 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& draw(const double cumWeight) { - - // generate random values between [0:cumWeight] - std::uniform_real_distribution 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& s, const float d) {return s.weight < d;}; - auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); - return *it; - } }; diff --git a/smc/filtering/resampling/ParticleFilterResamplingPercent.h b/smc/filtering/resampling/ParticleFilterResamplingPercent.h index c992d5a..29aba41 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingPercent.h +++ b/smc/filtering/resampling/ParticleFilterResamplingPercent.h @@ -42,8 +42,8 @@ namespace SMC { // to-be-removed region - const int start = particles.size() * (1-percent); - const int end = particles.size(); + const int start = particles.size() * (1-percent); + const int end = particles.size(); std::uniform_int_distribution 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; - for (auto& p : particles) { + double equalweight = 1.0 / (double) cnt; + for (auto& p : particles) { weightSum += p.weight; - p.weight = equalweight; + p.weight = equalweight; } } diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h index cfd89c1..e67adb4 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h +++ b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h @@ -45,8 +45,8 @@ namespace SMC { void resample(std::vector>& particles) override { // compile-time sanity checks - // TODO: this solution requires EXPLICIT overloading which is bad... - // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::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 - std::uniform_real_distribution distNewOne(0.0, 1.0); + // randomness for drawing particles + std::uniform_real_distribution 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 - if (distNewOne(gen) < 0.005) { - const double radius = 50.0; - const NM::NavMeshSub reachable(particlesCopy[i].state.loc, radius); - particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius); - particles[i].weight = equalWeight; - continue; - } + // slight chance to get a truely random particle in range X m + if (distNewOne(gen) < 0.005) { + const double radius = 50.0; + const NM::NavMeshSub reachable(particlesCopy[i].state.pos, radius); + particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, radius); + particles[i].weight = equalWeight; + continue; + } particles[i] = draw(cumWeight); particles[i].weight = equalWeight; diff --git a/smc/filtering/resampling/particlefilterresamplingsimpleimpoverishmentgrid.h b/smc/filtering/resampling/particlefilterresamplingsimpleimpoverishmentgrid.h new file mode 100644 index 0000000..ca3017a --- /dev/null +++ b/smc/filtering/resampling/particlefilterresamplingsimpleimpoverishmentgrid.h @@ -0,0 +1,129 @@ +#ifndef PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H +#define PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H + + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include + +namespace SMC { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight. + * O(log(n)) per particle + */ + template + class ParticleFilterResamplingSimpleImpoverishmentGrid : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + Grid& grid; + + public: + + /** ctor */ + ParticleFilterResamplingSimpleImpoverishmentGrid(Grid& grid) : grid(grid) { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::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 distNewOne(0.0, 1.0); + std::uniform_int_distribution 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 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& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution 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& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + + }; + + +} + +#endif // PARTICLEFILTERRESAMPLINGSIMPLEIMPOVERISHMENTGRID_H