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

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