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

added kde base resampling. doesn't work as expected atm.

Browse Source 
it seems i have soom tabbing problems xD... we should talk about that
This commit is contained in:
toni
2018-07-23 16:00:05 +02:00
parent f225788ac4
commit 1408709efe
3 changed files with 166 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
smc/filtering/ParticleFilter.h
View File

@@ -160,7 +160,7 @@ namespace SMC {
void updateTransitionOnly(const Control* control) {
// sanity checks (if enabled)
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!");
// perform the transition step
transition->transition(particles, control);
@@ -171,12 +171,13 @@ namespace SMC {
State updateEvaluationOnly(const Observation& observation) {
// sanity checks (if enabled)
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!");
Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!");
// if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
// if the number of efficient particles is too low, perform resampling
if (lastNEff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); }
//resampler->resample(particles);
// perform the evaluation step and calculate the sum of all particle weights
evaluation->evaluation(particles, observation);
@@ -187,7 +188,7 @@ namespace SMC {
//Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!");
// normalize the particle weights and thereby calculate N_eff
lastNEff = normalize();
lastNEff = normalize();
// estimate the current state
const State est = estimation->estimate(particles);

8
smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h
View File

@@ -39,7 +39,7 @@ namespace SMC {
public:
ParticleFilterEstimationBoxKDE(){
//fuck off
//keine boesen woerter
}
ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){
@@ -79,7 +79,7 @@ namespace SMC {
static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//TODO: check for function getX() and getY()
//TODO: fixed this hack
//TODO: fixe this hack
State tmpAVG;
double weightSum = 0;
@@ -89,12 +89,12 @@ namespace SMC {
//grid.add receives position in meter!
grid.add(p.state.getX(), p.state.getY(), p.weight);
//TODO: fixed this hack
//TODO: fixe this hack
//get the z value by using the weighted average z!
tmpAVG += p.state * p.weight;
weightSum += p.weight;
}
//TODO: fixed this hack
//TODO: fixe this hack
tmpAVG /= weightSum;
int nFilt = 3;

155
smc/filtering/resampling/ParticleFilterResamplingKDE.h Normal file
View File

@@ -0,0 +1,155 @@
#ifndef PARTICLEFILTERRESAMPLINGKDE_H
#define PARTICLEFILTERRESAMPLINGKDE_H
#include <algorithm>
#include <random>
#include "ParticleFilterResampling.h"
#include "../../ParticleAssertions.h"
#include "../../../math/boxkde/benchmark.h"
#include "../../../math/boxkde/DataStructures.h"
#include "../../../math/boxkde/Image2D.h"
#include "../../../math/boxkde/BoxGaus.h"
#include "../../../math/boxkde/Grid2D.h"
#include "../../../math/distribution/Normal.h"
#include "../../../navMesh/NavMesh.h"
namespace SMC {
/**
* Resample based on rapid KDE
*/
template <typename State, typename Tria>
class ParticleFilterResamplingKDE : 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;
/** boundingBox for the boxKDE */
BoundingBox<float> bb;
/** histogram/grid holding the particles*/
Grid2D<float> grid;
/** bandwith for KDE */
Point2 bandwith;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
public:
/** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const float gridsize_m, const Point2 bandwith) {
const Point3 maxBB = mesh->getBBox().getMax();
const Point3 minBB = mesh->getBBox().getMin();
this->bb = BoundingBox<float>(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10);
// Create histogram
size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_m);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_m);
this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->bandwith = bandwith;
this->mesh = mesh;
gen.seed(1234);
}
void resample(std::vector<Particle<State>>& particles) override {
// compile-time sanity checks
static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
//static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//todo: static assert for getx, gety, getz, setposition
State tmpAVG;
double weightSum = 0;
grid.clear();
for (Particle<State> p : particles){
//grid.add receives position in meter!
grid.add(p.state.getX(), p.state.getY(), p.weight);
//TODO: fixe this hack
//get the z value by using the weighted average z!
tmpAVG += p.state * p.weight;
weightSum += p.weight;
}
//TODO: fixe this hack with z...
tmpAVG /= weightSum;
int nFilt = 3;
float sigmaX = bandwith.x / grid.binSizeX;
float sigmaY = bandwith.y / grid.binSizeY;
BoxGaus<float> boxGaus;
boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
// fill a drawlist with 10k equal distributed particles
// assign them a weight based on the KDE density.
DrawList<Point3> dl;
Distribution::Normal<float> zProb(tmpAVG.getZ(), 4.0);
for (int i = 0; i < 10000; ++i){
//todo: wir ziehen natürlich hier aus dem ganzen gebäude, bekommen
// aber ein gewicht nur basierend auf 2D. deshalb kleiner hack und z
// mit einer normalverteilung gewichtet, wobei mean = avg_z der partikel
// menge ist
NM::NavMeshLocation<Tria> tmpPos = mesh->getRandom().draw();
double weight = grid.image().get(tmpPos.pos.x, tmpPos.pos.y);
//weight *= zProb.getProbability(tmpPos.pos.z);
dl.add(tmpPos.pos, weight);
}
int dummyyy = 0;
// used the same particles to not lose the heading.
// TODO: Summe aller Partikel wird relativ schnell 0! Ich vermute da am Anfang ein einzelner Partikel stark gewichtet ist und alleine
// die Dichte repräsentiert über die KDE. Jetzt wird beim nächsten zufälligen ziehen an dieser Stelle keiner der 10k partikel dort gezogen, d.h. alle
// haben ein Gewicht von 0 und ciao.
// Lösung: erstmal equal weight versuchen. ansonten: warum nehmen wir nochmal diese 10k? und nciht einfach die partikel die wir eh schon haben?
for (int i = 0; i < particles.size(); ++i){
double tmpWeight = 1;
particles[i].state.loc = 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;
}
};
}
#endif // PARTICLEFILTERRESAMPLINGKDE_H