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

fixed baraomter issue (skip first few readings due to sensor errors)

Browse Source 
added new eval using shortest-path + plotting
removed compiler warnings for clean-code
fixed some minor issues
added new TeX code and new graphics
This commit is contained in:
FrankE
2016-02-07 13:30:04 +01:00
parent 004d1f48fd
commit deb21fc550
23 changed files with 4480 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
code/eval/DebugShortestPath.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef DEBUGSHORTESTPATH_H
#define DEBUGSHORTESTPATH_H
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include "../Helper.h"
#include "../Vis.h"
template <typename T> class DebugShortestPath : public GridWalkShortestPathControl<T> {
private:
Vis vis;
public:
/** ctor */
template <typename Access> DebugShortestPath(Grid<T>& grid, const Access& acc, const T& target, Helper::FHWSFloors& floors) : GridWalkShortestPathControl<T>(grid, acc, target) {
vis.particles.setColorHex("#0000ff");
vis.particles.setPointSize(1.5);
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);
if (this->recalc == 0){
vis.estPath.clear();
vis.particles.clear();
vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z));
for (int i = 0; i < (int)this->path->size()-1; ++i) {
const DijkstraNode<T>& dn1 = (*this->path)[i+0];
const DijkstraNode<T>& dn2 = (*this->path)[i+1];
K::GnuplotPoint3 p1 (dn1.element->x_cm, dn1.element->y_cm, dn1.element->z_cm);
K::GnuplotPoint3 p2 (dn2.element->x_cm, dn2.element->y_cm, dn2.element->z_cm);
vis.estPath.addSegment(p1, p2);
}
vis.show();
}
return s;
}
};
#endif // DEBUGSHORTESTPATH_H

48
code/eval/Eval1.h
View File

@@ -10,6 +10,8 @@
#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include "DebugShortestPath.h"
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -106,6 +108,7 @@ public:
// forward
runName = "path2_forward_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
@@ -121,6 +124,7 @@ public:
// forward
runName = "path2_forward_path";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
@@ -142,6 +146,7 @@ public:
// forward
runName = "path3_forward_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
@@ -161,6 +166,7 @@ public:
// forward
runName = "path3_forward_path";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
@@ -176,6 +182,7 @@ public:
runName = "path4_nexus_simple";
BarometerEvaluation::barometerSigma = 0.16;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
@@ -193,6 +200,7 @@ public:
runName = "path4_nexus_importance";
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
@@ -207,6 +215,7 @@ public:
runName = "path4_nexus_path";
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
@@ -218,32 +227,37 @@ public:
}
void path4_nexus_path_b() {
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
//steps hochzaehlen weil mehr als einer in einer transition??
//increase regional average region
runName = "path4_nexus_path";
// void setEval1() {
BarometerEvaluation::barometerSigma = 0.05;
sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path1_nexus_simple() {
// // the particle filter's evaluation method
// std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
// eval.get()->setUsage(true, true, true, true, true); // TODO: STEP TURN
// pf->setEvaluation( std::move(eval) );
runName = "bergwerk_path1_nexus_simple";
// // resampling step?
// pf->setNEffThreshold(1.0);
// pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
// //pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
BarometerEvaluation::barometerSigma = 0.10;
sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward
srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt");
srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
// // state estimation step
// 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)));
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
// }
};

15
code/eval/EvalBase.h
View File

@@ -56,8 +56,11 @@ protected:
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path1dbl = {29, 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> path2dbl = {19, 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> path3dbl = {5, 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};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
@@ -120,7 +123,7 @@ public:
// sensor numbers
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;
//const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
@@ -206,10 +209,10 @@ public:
break;
}
case s_linearAcceleration:{
baroSensorReader.readVerticalAcceleration(se);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
@@ -287,7 +290,7 @@ public:
vis.show();
// prevent gnuplot errors
usleep(1000*33);
usleep(1000*333);
}

6
code/eval/PaperVisImportance.h
View File

@@ -38,13 +38,13 @@ public:
// use node-importance as grid-color
struct ColorizerImp {
float get(const MyGridNode& n) const {return n.imp;}
bool skip(const MyGridNode& n) const {return false;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use node-distance as grid-color
struct ColorizerDist {
float get(const MyGridNode& n) const {return n.distToTarget;}
bool skip(const MyGridNode& n) const {return false;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use num-visited as grid-color
@@ -166,7 +166,7 @@ public:
if (i % 250 == 0) {std::cout << i << std::endl;}
const MyGridNode& nStart = gnEnd;
GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
//GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
}