FHWS/OTHER2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

many eval things

Browse Source 
moved obsolete code
TeX work
This commit is contained in:
FrankE
2017-04-10 18:20:11 +02:00
parent b2adb16b49
commit 32674e3fbb
21 changed files with 1063 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
pf/EvalWalk.h
View File

@@ -31,7 +31,7 @@
#include "Indoor/floorplan/v2/FloorplanHelper.h"
#include "Indoor/floorplan/v2/FloorplanCeilings.h"
#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
#include "Indoor/sensors/radio/model/WiFiModels.h"
#include "Indoor/sensors/offline/FileReader.h"
#include "../Helper.h"
@@ -50,7 +50,6 @@ class EvalWalk : public Offline::Listener {
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
std::string runName;
WiFiModelLogDistCeiling wifiModel;
Plotty plotty;
PlotWifiMeasurements plotWifi;
@@ -79,7 +78,7 @@ class EvalWalk : public Offline::Listener {
public:
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map), em(map) {
EvalWalk(Floorplan::IndoorMap* map) : plotty(map), map(map), em(map) {
const std::string saveFile = Settings::pathData + "/grid.dat";
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
@@ -104,14 +103,12 @@ public:
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );
// TODO: flexible model
//wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
// transition
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
//pf->setNEffThreshold(0.5);
//pf->setNEffThreshold(0.35);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setNEffThreshold(0.75);
@@ -120,17 +117,27 @@ public:
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05);
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05)) );
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
res->setDrawCallback([&] (K::Particle<MyState>& p) {
static std::minstd_rand gen;
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(-0.3, +0.3);
for (int j = 0; j < 2; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
p.state.heading.direction += distTurn(gen);
});
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.25f)));
}
@@ -152,8 +159,11 @@ public:
groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);
// wifi model
WiFiModelLogDistCeiling wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos);
//WiFiModelLogDistCeiling wifiModel(map);
//wifiModel.loadXML(Settings::wifiAllFixed);
//wifiModel.loadXML(Settings::wifiEachOptParPos);
WiFiModelPerFloor wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );

12
pf/PF.h
View File

@@ -184,7 +184,7 @@ public:
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
walker.addModule(&modActivity);
//walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
//walker.addModule(&modImportance);
@@ -219,8 +219,8 @@ public:
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.8);
p.weight = std::pow(p.weight, 0.5);
//p.weight = 1;
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
@@ -228,7 +228,7 @@ public:
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
//p.weight *= std::pow(prob, 0.7); // add grid-walk-probability
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
@@ -247,7 +247,7 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
WiFiModel& wifiModel;
EarthMapping& em;
@@ -260,7 +260,7 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel, EarthMapping& em) :
PFEval(Grid<MyGridNode>* grid, WiFiModel& wifiModel, EarthMapping& em) :
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free