FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

changed visualisation

Browse Source 
added new eval code for new walkers
improved barometer (moving avg and median)
floorplan-staircase-fixes
disabled step-turn (now part of transition)
added abs-orientation-reader (for testing)
added beacons
This commit is contained in:
FrankE
2016-02-05 20:21:46 +01:00
parent 9e9b6882cd
commit 716b004f3c
15 changed files with 188 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
code/eval/Eval1.h
View File

@@ -7,7 +7,9 @@
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -25,7 +27,7 @@ public:
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
std::vector<int> wp = path1; std::reverse(wp.begin(), wp.end());
std::vector<int> wp = path1;// std::reverse(wp.begin(), wp.end());
MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) );
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) );
@@ -35,21 +37,22 @@ public:
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
//GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// path1
//sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
// sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
// srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
// srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
// path2
//sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
// sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
// srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
//sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward
//srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");
@@ -62,11 +65,22 @@ public:
// srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt");
// srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt");
// path4
// sr = new SensorReader("./measurements/path4/1454595382218.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path4/Turns.txt");
// srs = new SensorReaderStep("./measurements/path4/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, wp);
}
void setEvalFails() {
}
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
//steps hochzaehlen weil mehr als einer in einer transition??
//increase regional average region
@@ -87,9 +101,9 @@ public:
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// 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.50f)));
}

31
code/eval/EvalBase.h
View File

@@ -27,6 +27,7 @@
#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
class EvalBase {
@@ -57,6 +58,7 @@ protected:
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
public:
@@ -71,6 +73,12 @@ public:
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
}
static GridPoint conv(const Point3& p) {
@@ -110,7 +118,7 @@ public:
void run() {
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
@@ -198,8 +206,13 @@ public:
}
case s_linearAcceleration:{
baroSensorReader.readVerticalAcceleration(se);
break;
baroSensorReader.readVerticalAcceleration(se);
break;
}
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
break;
}
}
@@ -235,7 +248,7 @@ public:
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 0; // 750
const int offset = 750;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
@@ -258,11 +271,17 @@ public:
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.1;
vis.gp << "set arrow 111 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();