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

added code for advanced sample impoverishment using the mesh

Browse Source
This commit is contained in:
toni
2018-05-15 10:03:49 +02:00
parent 628aafaecd
commit d0b01b377d
11 changed files with 622 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
math/dsp/Convolution2D.h
View File

@@ -38,6 +38,11 @@ public:
}
/** TODO:
* Image2D Funktion make powertwo schreiben
* die Bild zeilenweise durchgeht und mit nullen füllt und in neuen vektor speichert.
* Ggf. Nen konstruktor dafür keine Funktion? Dann pfusch ich net im original rum.
*/
//calculate FFT for both

4
navMesh/NavMeshFactory.h
View File

@@ -140,13 +140,13 @@ namespace NM {
// line-obstacles
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
nmPoly.remove(getPolygon(line));
}
// object-obstacles
Floorplan::FloorObstacleObject* obj = dynamic_cast<Floorplan::FloorObstacleObject*>(obs);
if (obj != nullptr) {
nmPoly.remove(getPolygon(obj));
nmPoly.remove(getPolygon(obj));
}
}

176
navMesh/walk/NavMeshWalkKLD.h Normal file
View File

@@ -0,0 +1,176 @@
#ifndef NAVMESHWALKKLD_H
#define NAVMESHWALKKLD_H
#include <vector>
#include <algorithm>
#include <random>
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* simple walker that gives particles a slight chance to walk
* thru walls depending upon a given delta
*/
template <typename Tria> class NavMeshWalkKLD {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
Distribution::Uniform<float> distNewOne = Distribution::Uniform<float>(0,1);
int hits = 0;
int misses = 0;
int walls = 0;
int noTria = 0;
public:
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
/** list of results */
using ResultList = std::vector<ResultEntry>;
public:
/** ctor */
NavMeshWalkKLD(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
/**
* @brief getOne
* @param params - of the current sample
* @param kld - the kullback-leibler divergence
* @param lambda - some value to influence kld and wifi
* @param qualityWifi - current wifi quality between [0,1]
* @return a newly drawn sample based on the previous sample or random
*/
ResultEntry getOne(const NavMeshWalkParams<Tria>& params, const double kld, const double lambda, const double qualityWifi) {
// sanity checks
params.check();
ResultEntry re;
// get kld between [0,1] the lower this value, the more we need to
// increase the reachable radius and number of particles walking tru walls
const double wallProb = 1 - std::exp(-lambda * (kld * qualityWifi));
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
const float toBeWalkedDistKld = (kld * qualityWifi * 0.5);
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD...
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
//3D Destination for finding the location within the complete mesh
//TODO: better solution for z? seems to be a hack on stairs
const Point2 dsttt = params.start.pos.xy() + (dir * (toBeWalkedDistSafe + 1.0));
const Point3 dst3D = Point3(dsttt.x, dsttt.y, params.start.pos.z);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// 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
++hits;
}
//give the particle a slight chance to walk thru the wall
else if(distNewOne.draw() < wallProb){
try{
//check if there is a triangle for dst behind the wall
const Tria* dstTriaBehindWall = mesh.getLocation(dst3D).tria;
re.heading = params.heading;
re.location.pos = dst3D;
re.location.tria = dstTriaBehindWall;
++walls;
++misses;
} catch (...) {
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destination
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading
++noTria;
++misses;
}
++misses;
} else {
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;
}
const int total = (hits + misses);
const int totalWalls = (walls + noTria);
if (total % 10000 == 0) {
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
//std::cout << "walls: " << (walls*100/totalWalls) << "%" << std::endl;
}
// calculate probability
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
re.probability *= p1;
}
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
// sanity checks
params.check();
return {getOne(params)};
}
const NavMesh<Tria>& getMesh(){
return mesh;
}
};
}
#endif // NAVMESHWALKKLD_H

152
navMesh/walk/NavMeshWalkWifi.h Normal file
View File

@@ -0,0 +1,152 @@
#ifndef NAVMESHWALKWIFI_H
#define NAVMESHWALKWIFI_H
#include <vector>
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "../../sensors/radio/WiFiMeasurements.h"
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/** simple walker extended by a wifi anti impoverishment method
* draw 10000 random particles within the building equaly
* evaluate them with wifi and then draw cumulative from a list
* instead of killing the particles who walk against walls
* the number of new particles is restricted by ??? */
template <typename Tria> class NavMeshWalkWifi {
public:
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
std::vector<ResultEntry> wifiSamples;
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double wifiCumWeight;
DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
int hits = 0;
int misses = 0;
public:
/** list of results */
using ResultList = std::vector<ResultEntry>;
/** ctor */
NavMeshWalkWifi(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
wifiModel(wifiModel),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
/** update every transition step the WiFi */
void updateWiFi(const WiFiMeasurements& wifiObs, Timestamp currentTime){
this->wifiSamplesDrawList.reset();
//todo: restrict this to specific region
NavMeshRandom<Tria> rnd = mesh.getRandom();
wifiCumWeight = 0;
for(int i = 0; i < 10000; ++i){
NavMeshLocation<Tria> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
this->wifiSamplesDrawList.add(tmpLocation, weight);
}
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
// sanity checks
params.check();
ResultEntry re;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// 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.probability = 1.0;
++hits;
} else {
re.location = wifiSamplesDrawList.get();
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
re.probability = 0.1;
++misses;
}
const int total = (hits + misses);
if (total % 10000 == 0) {
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
// calculate probability
/*
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
re.probability *= p1;
}
*/
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
// sanity checks
params.check();
return {getOne(params)};
}
};
}
#endif // NAVMESHWALKWIFI_H

153
navMesh/walk/NavMeshWalkWifiRegional.h Normal file
View File

@@ -0,0 +1,153 @@
#ifndef NAVMESHWALKWIFIREGIONAL_H
#define NAVMESHWALKWIFIREGIONAL_H
#include <vector>
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "../../sensors/radio/WiFiMeasurements.h"
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/** simple walker extended by a wifi anti impoverishment method
* draw 10000 random particles within a specific region arround
* the last estimation equaly.
* evaluate them with wifi and then draw cumulative from a list
* instead of killing the particles who walk against walls
* the number of new particles is restricted by ??? */
template <typename Tria> class NavMeshWalkWifiRegional {
public:
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
std::vector<ResultEntry> wifiSamples;
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double wifiCumWeight;
DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
int hits = 0;
int misses = 0;
public:
/** list of results */
using ResultList = std::vector<ResultEntry>;
/** ctor */
NavMeshWalkWifiRegional(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
wifiModel(wifiModel),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
/** update every transition step the WiFi */
void updateWiFi(const WiFiMeasurements& wifiObs, const Timestamp currentTime, const Point3 lastEst){
this->wifiSamplesDrawList.reset();
//todo: restrict this to specific region
NavMeshRandom<Tria> rnd = mesh.getRandom();
wifiCumWeight = 0;
for(int i = 0; i < 10000; ++i){
NavMeshLocation<Tria> tmpLocation = rnd.drawWithin(lastEst, 10.0);
double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
this->wifiSamplesDrawList.add(tmpLocation, weight);
}
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
// sanity checks
params.check();
ResultEntry re;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// 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.probability = 1.0;
++hits;
} else {
re.location = wifiSamplesDrawList.get();
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
re.probability = 0.1;
++misses;
}
const int total = (hits + misses);
if (total % 10000 == 0) {
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
// calculate probability
/*
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
re.probability *= p1;
}
*/
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
// sanity checks
params.check();
return {getOne(params)};
}
};
}
#endif // NAVMESHWALKWIFIREGIONAL_H

2
sensors/activity/ActivityDetector.h
View File

@@ -141,7 +141,7 @@ private:
#endif
if (std::abs(delta_hPa) < 0.042) {
current = Activity::WALKING;
current = Activity::WALKING;
return;
} else if (delta_hPa > 0) {
current = Activity::WALKING_DOWN;

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

@@ -12,8 +12,10 @@
#include "../../../math/boxkde/Image2D.h"
#include "../../../math/boxkde/BoxGaus.h"
#include "../../../math/boxkde/Grid2D.h"
#include "../../../grid/Grid.h";
#include "../../../floorplan/v2/FloorplanHelper.h";
#include "../../../grid/Grid.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
#include "../../../navMesh/NavMesh.h"
namespace SMC {
@@ -36,6 +38,10 @@ namespace SMC {
public:
ParticleFilterEstimationBoxKDE(){
//fuck off
}
ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){
const Point3 maxBB = FloorplanHelper::getBBox(map).getMax();
@@ -50,6 +56,20 @@ namespace SMC {
this->bandwith = bandwith;
}
template <typename Tria> ParticleFilterEstimationBoxKDE(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;
}
State estimate(const std::vector<Particle<State>>& particles) override {
// compile-time sanity checks

109
smc/filtering/resampling/ParticleFilterResamplingSignalStrengthOnMesh.h Normal file
View File

@@ -0,0 +1,109 @@
#ifndef PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H
#define PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H
#include <algorithm>
#include <random>
#include "ParticleFilterResampling.h"
#include "../../ParticleAssertions.h"
#include "../../../navMesh/NavMeshRandom.h"
#include "../../../navMesh/walk/NavMeshSub.h"
namespace SMC {
/**
* uses simple probability resampling by drawing particles according
* to their current weight.
* O(log(n)) per particle
*/
template <typename State, typename Tria>
class ParticleFilterResamplingSignalStrengthOnMesh: 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;
public:
/** ctor */
ParticleFilterResamplingSimpleImpoverishment() {
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);
// 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;
}
particles[i] = draw(cumWeight);
particles[i].weight = equalWeight;
}
}
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 // PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H

3
smc/filtering/resampling/ParticleFilterResamplingSimple.h
View File

@@ -89,9 +89,6 @@ namespace SMC {
return *it;
}
};

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

@@ -74,7 +74,7 @@ namespace SMC {
// with the resampled particle-set
for (uint32_t i = 0; i < cnt; ++i) {
// slight chance to get a truely particle in range 25m
// 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);

2
wifi/estimate/ray3/ModelFactory.h
View File

@@ -22,7 +22,7 @@ namespace Ray3D {
public:
bool exportCeilings = true;
bool exportObstacles = true;
bool exportObstacles = true;
bool exportStairs = true;
bool fancyStairs = true;
bool exportHandrails = true;