FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added boxkde resamplin in 2D and 3D

Browse Source 
read activity out of sensor offline file
This commit is contained in:
toni
2018-11-24 17:09:41 +01:00
parent 9388e6e725
commit 1dac907004
13 changed files with 180 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
grid/walk/v2/modules/WalkModule.h
View File

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

2
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -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();

2
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -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;
}

4
sensors/activity/Activity.h
View File

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

17
sensors/offline/FileReader.h
View File

@@ -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;

1
sensors/offline/Sensors.h
View File

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

2
smc/filtering/ParticleFilter.h
View File

@@ -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);

6
smc/filtering/ParticleFilterInitializer.h
View File

@@ -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 <vector>
#include "../Particle.h"
@@ -22,4 +22,4 @@ namespace SMC {
}
#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
#endif // SMC_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H

1
smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h
View File

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

29
smc/filtering/resampling/ParticleFilterResamplingKDE.h
View File

@@ -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;
}
};

10
smc/filtering/resampling/ParticleFilterResamplingPercent.h
View File

@@ -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<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;
for (auto& p : particles) {
double equalweight = 1.0 / (double) cnt;
for (auto& p : particles) {
weightSum += p.weight;
p.weight = equalweight;
p.weight = equalweight;
}
}

24
smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h
View File

@@ -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...
// static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
// 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();
@@ -67,21 +67,21 @@ namespace SMC {
particlesCopy[i].weight = cumWeight;
}
// randomness for drawing particles
std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
// 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 random particle in range X m
if (distNewOne(gen) < 0.005) {
const double radius = 50.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;
}
// 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<Tria> 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;

129
smc/filtering/resampling/particlefilterresamplingsimpleimpoverishmentgrid.h Normal file
View File

@@ -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