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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
FrankE
2018-07-24 08:13:44 +02:00
parent 8dd1ba0be6 a9368ee0f0
commit 3d79fd5af0
9 changed files with 204 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
navMesh/walk/NavMeshWalkKLD.h
View File

@@ -81,7 +81,7 @@ namespace NM {
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
const float toBeWalkedDistKld = (kld * qualityWifi);
const float toBeWalkedDistKld = (kld * qualityWifi);
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD...

12
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -59,7 +59,7 @@ namespace NM {
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
@@ -73,15 +73,15 @@ namespace NM {
// is above destination reachable?
if (dstTria) {
re.heading = params.heading; // heading was OK -> keep
re.location.pos = dstTria->toPoint3(dst); // new destination position
re.location.tria = dstTria; // new destination triangle
re.heading = params.heading; // heading was OK -> keep
re.location.pos = dstTria->toPoint3(dst); // new destination position
re.location.tria = dstTria; // new destination triangle
++hits;
} else {
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destianation
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
++misses;

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;

8
smc/filtering/resampling/ParticleFilterResampling.h
View File

@@ -24,10 +24,10 @@ namespace SMC {
public:
/**
* perform resampling on the given particle-vector
* @param particles the vector of all particles to resample
*/
/**
* perform resampling on the given particle-vector
* @param particles the vector of all particles to resample
*/
virtual void resample(std::vector<Particle<State>>& particles) = 0;
/**

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

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

@@ -42,29 +42,25 @@ 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
for (uint32_t i = start; i < end; ++i) {
const int rnd = dist(gen);
particles[i] = particles[rnd];
particles[i].weight /= 2;
particles[rnd].weight /= 2;
//particles[i].weight /= 2;
//particles[rnd].weight /= 2;
}
// calculate weight-sum
double weightSum = 0;
for (const auto& p : particles) {
double equalweight = 1.0 / (double) cnt;
for (auto& p : particles) {
weightSum += p.weight;
p.weight = equalweight;
}
int i = 0;
}
private:

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

@@ -46,7 +46,7 @@ namespace SMC {
// compile-time sanity checks
// 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,20 +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 particle in range X m
if (distNewOne(gen) < 0.001) {
const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.pos, 10.0);
particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0);
particles[i].weight = equalWeight;
continue;
}
// slight chance to get a truely random particle in range X m
if (distNewOne(gen) < 0.001) {
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;
}
particles[i] = draw(cumWeight);
particles[i].weight = equalWeight;

9
smc/smoothing/FastKDESmoothing.h
View File

@@ -210,6 +210,15 @@ namespace SMC {
BoxGaus<float> boxGaus;
boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
/**
* Das hier wird nicht funktionieren!
* Wir machen eine Transition von t zu t+1 und nehmen dann diese position um von der kde aus t+1 ein
* Gewicht zu erhalten... aber das bringt ja nichts, diese Gewicht haben wir doch schon auf den partikeln in t+1
* die transition macht ja nicht 2x was anderes, sondern wieder genau das gleiche.
* was das smoothing gut macht, sind die Faltungen mit allen Partikeln (jeder mit jedem vergleichen).
**/
// Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T)
// Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation
smoothedParticles = forwardHistory.getParticleSet(i);