added code from fusion2016

code/eval/DebugShortestPath.h

@@ -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

code/eval/Eval.h

@@ -0,0 +1,73 @@
+#ifndef EVAL_H
+#define EVAL_H
+
+#include "EvalBase.h"
+#include "../DijkstraMapper.h"
+#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
+#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
+#include <Indoor/grid/walk/GridWalkPushForward.h>
+#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
+
+class Eval : public EvalBase {
+
+public:
+
+
+
+//	Eval() {
+
+
+//		pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
+
+//		MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
+//		MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
+
+//		//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
+//		GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
+//		//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
+//		//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
+
+//		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+//		sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
+//		srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
+//		srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
+
+//		gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
+
+//	}
+
+//	//wifi also uniform dist 0/1 fuer bereiche die OK sind?
+//	//steps hochzaehlen weil mehr als einer in einer transition??
+//	//increase regional average region
+
+//	void setEval1() {
+
+
+//		runName = "TODO";
+
+//		// the particle filter's evaluation method
+//		std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
+//		eval.get()->setUsage(true, false, false, true, true);
+//		pf->setEvaluation( std::move(eval) );
+
+//		// resampling step?
+//		pf->setNEffThreshold(1.0);
+//		pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+
+//		// 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.33f)));
+
+//	}
+
+
+
+};
+
+#endif // EVAL_H

code/eval/Eval1.h

@@ -0,0 +1,264 @@
+#ifndef EVAL1_H
+#define EVAL1_H
+
+#include "EvalBase.h"
+#include "../DijkstraMapper.h"
+#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
+#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 <Indoor/grid/walk/GridWalkShortestPathControl.h>
+#include "DebugShortestPath.h"
+
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
+
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
+
+class Eval1 : public EvalBase {
+
+public:
+
+	Eval1() {
+
+
+		pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
+
+
+
+		std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
+		eval.get()->setUsage(true, true, true, true, true);
+		pf->setEvaluation( std::move(eval) );
+
+		// 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)) );
+
+		// 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)));
+
+
+		//		std::vector<int> wp = path2;// 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()]) );
+
+//		//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
+//		//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
+//		//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");
+
+//		// 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/2/1454346071347.csv"); // backward
+//		//srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
+//		//srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");
+
+//		// path3
+////		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");
+////		sr = new SensorReader("./measurements/path3/2/1454345622819.csv"); // backward
+////		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() {
+
+	}
+
+	/**
+	 * starting with bad barometer readings. takes some time to move upwards
+	 * will be fixed by using the path
+	 */
+	void path2_forward_simple() {
+
+		// 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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path2);
+
+		GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path2_forward_path() {
+
+		// 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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path2);
+
+		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2.back()]) );
+		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+
+
+//	TODO: plot grid-nodes for stairs for the paper (also look at z-transitions. some have NO x/y change even though they should have!)
+
+
+	void path3_forward_simple() {
+
+		// 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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path3);
+
+		GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path3_forward_path() {
+
+//		looks like the left stairs here are NOT working as expected?!
+//		plot them
+//		look at the probability-draw, maybe there is an issue there
+
+		// 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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path3);
+
+		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3.back()]) );
+		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path4_nexus_simple() {
+
+		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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
+
+		// remove importance
+		for (auto& n : grid) {n.imp = 1;}
+
+		GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path4_nexus_imp() {
+
+		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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
+
+		GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path4_nexus_path() {
+
+		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");
+		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
+
+		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+	void path4_nexus_path_b() {
+
+		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");
+		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() {
+
+		runName = "bergwerk_path1_nexus_simple";
+
+		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);
+
+		GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+	}
+
+
+};
+
+#endif // EVAL1_H

code/eval/EvalBase.h

@@ -0,0 +1,308 @@
+#ifndef EVALBASE_H
+#define EVALBASE_H
+
+#include "../Settings.h"
+#include "../Helper.h"
+#include "../Vis.h"
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <KLib/math/statistics/Statistics.h>
+
+#include "GroundTruthWay.h"
+
+#include "../particles/MyState.h"
+#include "../particles/MyObservation.h"
+#include "../particles/MyEvaluation.h"
+#include "../particles/MyTransition.h"
+#include "../particles/MyInitializer.h"
+
+#include "../reader/SensorReader.h"
+#include "../reader/SensorReaderStep.h"
+#include "../reader/SensorReaderTurn.h"
+
+#include "../lukas/TurnObservation.h"
+#include "../lukas/StepObservation.h"
+
+#include "../toni/BarometerSensorReader.h"
+
+#include "../frank/WiFiSensorReader.h"
+#include "../frank/BeaconSensorReader.h"
+#include "../frank/OrientationSensorReader.h"
+
+class EvalBase {
+
+protected:
+
+	Grid<MyGridNode> grid;
+	Helper::FHWSFloors floors;
+	Vis vis;
+
+	K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
+
+	SensorReader* sr;
+	SensorReaderTurn* srt;
+	SensorReaderStep* srs;
+
+
+
+	std::string runName;
+
+	GroundTruthWay gtw;
+
+	// OLD
+	//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
+	//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
+	//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
+
+	// 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!
+
+public:
+
+	EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
+
+		// build the grid
+		Helper::buildTheGrid(grid, floors);
+
+		// setup the visualisation
+		vis.addFloor(floors.f0, floors.h0);
+		vis.addFloor(floors.f1, floors.h1);
+		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) {
+		return GridPoint(p.x, p.y, p.z);
+	}
+
+	GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
+
+		// construct the ground-truth-path by using all contained waypoint ids
+		std::vector<Point3> path;
+		for (int id : ids) {
+			auto it = waypoints.find(id);
+			if(it == waypoints.end()) {throw "not found";}
+			path.push_back(it->second);
+		}
+
+		// new created the timed path
+		GroundTruthWay gtw;
+		int i = 0;
+		while (sr.hasNext()) {
+			const SensorEntry se = sr.getNext();
+			if (se.data.empty()) {continue;}		// why necessary??
+			if (se.idx == 99) {
+				gtw.add(se.ts, path[i]);
+				++i;
+			}
+		}
+
+		// ensure the sensor-data contained usable timestamps for the ground-truth mapping
+		assert(i>0);
+
+		sr.rewind();
+		return gtw;
+
+	}
+
+	void run() {
+
+			// 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;
+
+			std::list<TurnObservation> turn_observations;
+			std::list<StepObservation> step_observations;
+
+			//Create an BarometerSensorReader
+			BarometerSensorReader baroSensorReader;
+
+
+			//Read all turn Observations
+			while(srt->hasNext()) {
+
+				SensorEntryTurn set = srt->getNext();
+				TurnObservation to;
+
+				to.ts = set.ts;
+				to.delta_heading = set.delta_heading;
+				to.delta_motion = set.delta_motion;
+
+				turn_observations.push_back(to);
+			}
+
+
+			//Step Observations
+			while(srs->hasNext()) {
+
+				SensorEntryStep ses = srs->getNext();
+				StepObservation so;
+
+				so.ts = ses.ts;
+
+				step_observations.push_back(so);
+			}
+
+
+
+			// the to-be-evaluated observation
+			MyObservation obs;
+			obs.step = new StepObservation(); obs.step->steps = 0;
+			obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
+
+			// control data
+			MyControl ctrl;
+
+
+			std::vector<Point3> pathEst;
+
+			uint64_t lastTransitionTS = 0;
+			int64_t start_time = -1;
+
+			K::Statistics<double> stats;
+			int cnt = 0;
+
+
+			// process each single sensor reading
+			while(sr->hasNext()) {
+
+				// get the next sensor reading from the CSV
+				const SensorEntry se = sr->getNext();
+
+				//start_time needed for time calculation of steps and turns
+				obs.latestSensorDataTS = se.ts;
+				if (start_time == -1) {start_time = se.ts;}
+				int64_t current_time = se.ts - start_time;
+
+				switch(se.idx) {
+
+					case s_wifi: {
+						obs.wifi = WiFiSensorReader::readWifi(se);
+						break;
+					}
+
+					case s_beacons: {
+						BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
+						if (!boe.mac.empty()) {
+							obs.beacons.entries.push_back(boe);
+						}			// add the observed beacon
+						obs.beacons.removeOld(obs.latestSensorDataTS);
+						break;
+					}
+
+					case s_barometer: {
+						obs.barometer = baroSensorReader.readBarometer(se);
+						break;
+					}
+
+//					case s_linearAcceleration:{
+//						baroSensorReader.readVerticalAcceleration(se);
+//						break;
+//					}
+
+					case s_orientation: {
+						obs.orientation = OrientationSensorReader::read(se);
+						break;
+					}
+
+				}
+
+				// process all occurred turns
+				while (!step_observations.empty() && current_time > step_observations.front().ts) {
+					const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
+					obs.step->steps++;
+					ctrl.walked_m = obs.step->steps * 0.71;
+				}
+
+				// process all occurred steps
+				while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
+					const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
+					obs.turn->delta_heading += _to.delta_heading;
+					obs.turn->delta_motion += _to.delta_motion;
+					ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
+
+				}
+
+
+				// time for a transition?
+				if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
+
+					lastTransitionTS = se.ts;
+
+					// timed updates
+					((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
+
+
+					// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
+					const MyState est = pf->update(&ctrl, obs);
+					const Point3 curEst = est.pCur;
+
+					// error calculation. compare ground-truth to estimation
+					const int offset = 750;
+					const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
+					const Point3 diff = curEst - curGT;
+
+					// skip the first 10 scans due to uniform distribution start
+					if (++cnt > 10) {
+					pathEst.push_back(curEst);
+					const float err = diff.length();
+					stats.add(err);
+					std::cout << stats.asString() << std::endl;
+					}
+
+					// plot
+					vis.clearStates();
+					for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
+						const K::Particle<MyState>& p = pf->getParticles()[i];
+						vis.addState(p.state.walkState);
+					}
+					vis.setTimestamp(se.ts);
+					vis.addGroundTruth(gtw);
+					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.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();
+
+					// prevent gnuplot errors
+					usleep(1000*333);
+
+				}
+
+
+			}
+
+			sleep(1000);
+
+		}
+
+
+
+};
+
+#endif // EVALBASE_H

code/eval/GroundTruthWay.h

@@ -0,0 +1,24 @@
+#ifndef GROUNDTRUTHWAY_H
+#define GROUNDTRUTHWAY_H
+
+#include <Indoor/math/Interpolator.h>
+#include <Indoor/geo/Point3.h>
+
+/**
+ * interpolated ground-trouth based on timed check-points
+ */
+class GroundTruthWay : public Interpolator<uint64_t, Point3> {
+
+public:
+
+	Point3 getPosAtTime(const uint64_t ts) const {
+		return get(ts);
+	}
+
+	/** get the ground truth way */
+	const std::vector<InterpolatorEntry>& getWay() const {return entries;}
+
+};
+
+
+#endif // GROUNDTRUTHWAY_H

code/eval/PaperPlot.h

@@ -0,0 +1,134 @@
+#ifndef PAPERPLOT_H
+#define PAPERPLOT_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 <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+
+
+#include <Indoor/floorplan/Floor.h>
+#include <Indoor/geo/Length.h>
+
+class PaperPlot {
+
+public:
+
+	K::Gnuplot gp;
+	K::GnuplotSplot plot;
+
+	K::GnuplotSplotElementLines floors;
+	K::GnuplotSplotElementColorPoints nodes;
+	K::GnuplotSplotElementLines edges;
+
+public:
+
+	PaperPlot() {
+
+		floors.setLineWidth(2);
+
+		plot.add(&edges);
+		plot.add(&nodes);
+		plot.add(&floors);
+
+		nodes.setPointSize(0.7);
+		edges.setColorHex("#555555");
+
+		gp << "set ticslevel 0\n";
+
+
+		//gp << "set zrange [0:0]\n";
+
+	}
+
+	void show() {
+		gp.draw(plot);
+		gp.flush();;
+	}
+
+	/** add all obstacles of the given floor to the provided height */
+	void addFloor(const Floor& f, const LengthF height) {
+
+		// add each wall
+		for (const Line2& l : f.getObstacles()) {
+			const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
+			const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
+			floors.addSegment(p1, p2);
+		}
+
+	}
+
+	/** add the grid to the plot */
+	template <typename T> void addGrid(Grid<T>& grid) {
+
+//		std::set<uint64_t> used;
+
+		// get the min/max value
+		float max = -999999;
+		float min = +999999;
+		for (const T& n1 : grid) {
+			const float val = n1.imp;
+			//const float val = n1.distToTarget;
+			if (val > max) {max = val;}
+			if (val < min) {min = val;}
+		}
+		gp << "set cbrange["<<min<<":"<<max<<"]\n";
+
+		for (const T& n1 : grid) {
+			const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
+			const float color = n1.imp;
+			//const float color = n1.distToTarget/max;
+			//const float color = 0;
+			nodes.add(p1, color);
+//			for (const T& n2 : grid.neighbors(n1)) {
+//				const uint64_t idx = n1.getIdx() * n2.getIdx();
+//				if (used.find(idx) == used.end()) {
+//					const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
+//					gridEdges.addSegment(p1, p2);
+//					used.insert(idx);
+//				}
+//			}
+		}
+
+	}
+
+	/** show all nodes (and edges?) within the given region */
+	template <typename T> void debugGrid(Grid<T>& grid, const BBox3& bbox, const bool addNodes, const bool addEdges) {
+
+		std::set<uint64_t> used;
+
+		for (T& n1 : grid) {
+			if (bbox.contains(n1)) {
+				const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
+				if (addNodes) {
+					nodes.add(p1, 0);
+				}
+				if (addEdges) {
+					for (const T& n2 : grid.neighbors(n1)) {
+						if (n1.z_cm == n2.z_cm) {continue;}	// speedup
+						if (used.find(n2.getIdx()) == used.end()) {
+							const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
+							edges.addSegment(p1, p2);
+						}
+					}
+					used.insert(n1.getIdx());
+//					for (const T& n2 : grid.neighbors(n1)) {
+//						const uint64_t idx = n1.getIdx() * n2.getIdx();
+//						if (used.find(idx) == used.end()) {
+//							const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
+//							edges.addSegment(p1, p2);
+//							used.insert(idx);
+//						}
+//					}
+				}
+			}
+		}
+
+	}
+
+
+};
+
+#endif // PAPERPLOT_H

code/eval/PaperPlot2D.h

@@ -0,0 +1,141 @@
+#ifndef PAPERPLOT2D_H
+#define PAPERPLOT2D_H
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
+
+#include <Indoor/floorplan/Floor.h>
+#include <Indoor/geo/Length.h>
+
+class PaperPlot2D {
+
+public:
+
+	struct Size {
+		float w;
+		float h;
+		Size(const float w, const float h) : w(w), h(h) {;}
+	};
+
+public:
+
+	K::Gnuplot gp;
+	K::GnuplotPlot plot;
+
+	K::GnuplotPlotElementLines floors;
+	K::GnuplotPlotElementColorPoints nodes;
+
+	std::string file;
+
+public:
+
+	PaperPlot2D(const std::string& file, Size s) : file(file) {
+		toFile(file, s);
+		setup();
+	}
+
+	PaperPlot2D() {
+		setup();
+	}
+
+private:
+
+	void setup() {
+
+		floors.setLineWidth(1.5);
+
+		nodes.setPointType(7);
+
+		plot.add(&nodes);
+		plot.add(&floors);
+
+		gp << "unset border\n";
+		gp << "unset colorbox\n";
+
+		gp << "set tics scale 0,0\n";		// HACK! "unset tics\n" segfaults current gnuplot version...
+		gp << "set format x ' '\n";
+		gp << "set format y ' '\n";
+
+		gp << "set size ratio -1\n";
+
+	}
+
+
+	void toFile(const std::string& file, const Size s) {
+		gp << "set output '" << file << "'\n";
+		gp << "set terminal eps size " << s.w << "," << s.h << "\n";
+	}
+
+public:
+
+	void setRanges(const float x1, const float x2, const float y1, const float y2) {
+		gp << "set xrange [" << x1 << ":" << x2 << "]\n";
+		gp << "set yrange [" << y1 << ":" << y2 << "]\n";
+	}
+
+	void show() {
+		gp.draw(plot);
+		if (file.length() != 0) {
+			std::string dataFile = file + ".dat";
+			std::ofstream os(dataFile.c_str());
+			os << gp.getBuffer();
+			os.close();
+		}
+		gp.flush();
+	}
+
+	/** add all obstacles of the given floor to the provided height */
+	void addFloor(const Floor& f) {
+
+		// add each wall
+		for (const Line2& l : f.getObstacles()) {
+			const K::GnuplotPoint2 p1(l.p1.x, l.p1.y);
+			const K::GnuplotPoint2 p2(l.p2.x, l.p2.y);
+			floors.addSegment(p1, p2);
+		}
+
+	}
+
+
+//	void removeGrid() {
+//		gp << "unset object\n";
+//	}
+
+	/** add the grid to the plot */
+	template <typename T, typename Colorizer> void addGrid(Grid<T>& grid, const Colorizer col) {
+
+		// get the min/max value
+		float max = -999999;
+		float min = +999999;
+		for (const T& n1 : grid) {
+			const float val = col.get(n1);
+			//const float val = n1.distToTarget;
+			if (val > max) {max = val;}
+			if (val < min) {min = val;}
+		}
+		gp << "set cbrange["<<min<<":"<<max<<"]\n";
+
+//		for (const T& n1 : grid) {
+//			const K::GnuplotPoint2 p1(n1.x_cm, n1.y_cm);
+//			const float color = n1.imp;
+//			//const float color = n1.distToTarget/max;
+//			//const float color = 0;
+//			nodes.add(p1, color);
+//		}
+
+		int i = 0;
+		for (const T& n1 : grid) {
+			if (col.skip(n1)) {continue;}
+			gp << "set object " << (++i) << " rectangle center " << n1.x_cm << "," << n1.y_cm << " size 20,20 fs solid noborder fc palette cb " << col.get(n1) << "\n";
+		}
+
+	}
+
+
+};
+
+
+#endif // PAPERPLOT2D_H

code/eval/PaperVisDijkstra.h

@@ -0,0 +1,4 @@
+#ifndef PAPERVISDIJKSTRA_H
+#define PAPERVISDIJKSTRA_H
+
+#endif // PAPERVISDIJKSTRA_H

code/eval/PaperVisGrid.h

@@ -0,0 +1,111 @@
+#ifndef PAPERVISGRID_H
+#define PAPERVISGRID_H
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/grid/factory/GridFactory.h>
+#include <Indoor/grid/factory/GridImportance.h>
+
+#include <Indoor/floorplan/FloorplanFactorySVG.h>
+
+#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
+
+
+
+#include <Indoor/nav/dijkstra/Dijkstra.h>
+#include <Indoor/nav/dijkstra/DijkstraPath.h>
+
+#include "PaperPlot.h"
+#include "PaperPlot2D.h"
+
+#include "../MyGridNode.h"
+#include "../Settings.h"
+
+#include "../Helper.h"
+
+class PaperVisGrid {
+
+public:
+
+
+	static void showStairs() {
+
+		// the grid
+		Grid<MyGridNode> grid(20);
+
+		// floors
+		Helper::FHWSFloors floors = Helper::getFloors();
+		Helper::buildTheGrid(grid, floors);
+
+//		// load the floorplan
+//		FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
+//		Floor f0 = fpFac.getFloor("floor_0");
+//		Floor f0 = fpFac.getFloor("floor_1");
+//		const LengthF h0 = LengthF::cm(0);
+//		const LengthF h1 = LengthF::cm(400);
+//		const LengthF h2 = LengthF::cm(400+340);
+//		const LengthF h3 = LengthF::cm(400+340+340);
+
+		// add the floorplan to the grid
+
+//		GridFactory<MyGridNode> gridFac(grid);
+
+//		gridFac.addFloor(f0, h0.cm());
+//		gridFac.addFloor(f1, h1.cm());
+//		gridFac.addFloor(f2, h2.cm());
+//		gridFac.addFloor(f3, h3.cm());
+
+		// remove all isolated nodes not attached to 300,300,floor0
+//		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
+
+		PaperPlot plot;
+
+
+
+		// stairwell low left
+		{BBox3 bbox;
+		bbox.add(Point3(1300, 650,000));
+		bbox.add(Point3(2000,1300,1400));
+		plot.debugGrid(grid, bbox, true, true);}
+
+		// stairwell upper left
+		{BBox3 bbox;
+		bbox.add(Point3(1200,4758,000));
+		bbox.add(Point3(1800,5158,1400));
+		plot.debugGrid(grid, bbox, true, true);}
+
+		// stairwell upper right
+		{BBox3 bbox;
+		bbox.add(Point3(6240,4718,000));
+		bbox.add(Point3(6830,5158,1400));
+		plot.debugGrid(grid, bbox, true, true);}
+
+		// stair left
+		{BBox3 bbox;
+		bbox.add(Point3(1200,3200,000));
+		bbox.add(Point3(1440,4078,1400));
+		plot.debugGrid(grid, bbox, true, true);}
+
+		// stair center
+		{BBox3 bbox;
+		bbox.add(Point3(4200,4118,100));
+		bbox.add(Point3(6120,4438,1400));
+		plot.debugGrid(grid, bbox, true, true);}
+
+		//stair lower right
+		{BBox3 bbox;
+		bbox.add(Point3(7360,3358,000));
+		bbox.add(Point3(7880,4300,500));
+		plot.debugGrid(grid, bbox, true, true);}
+
+
+		plot.show();
+
+		sleep(1000);
+
+	}
+
+
+
+};
+
+#endif // PAPERVISGRID_H

code/eval/PaperVisImportance.h

@@ -0,0 +1,190 @@
+#ifndef PAPERVISIMPORTANCE_H
+#define PAPERVISIMPORTANCE_H
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/grid/factory/GridFactory.h>
+#include <Indoor/grid/factory/GridImportance.h>
+
+#include <Indoor/floorplan/FloorplanFactorySVG.h>
+
+#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
+
+
+
+#include <Indoor/nav/dijkstra/Dijkstra.h>
+#include <Indoor/nav/dijkstra/DijkstraPath.h>
+
+#include "PaperPlot.h"
+#include "PaperPlot2D.h"
+
+#include "../MyGridNode.h"
+#include "../Settings.h"
+#include "../DijkstraMapper.h"
+
+PaperPlot2D::Size s1 = PaperPlot2D::Size(2,4);
+
+class PaperVisImportance {
+
+public:
+
+
+
+	static float clamp(const float in, const float min, const float max) {
+		if (in < min) {return min;}
+		if (in > max) {return max;}
+		return in;
+	}
+
+	// use node-importance as grid-color
+	struct ColorizerImp {
+		float get(const MyGridNode& n) const {return n.imp;}
+		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 {(void) n; return false;}
+	};
+
+	// use num-visited as grid-color
+	struct ColorizeHeat {
+		int maxCnt; int cutoff;
+		ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
+		float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
+		bool skip(const MyGridNode& n) const {return n.cnt < cutoff;}		// skip to reduce plot size
+	};
+
+
+	static void createImportance() {
+
+		// load the floorplan
+		FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
+		Floor f0 = fpFac.getFloor("test1");
+		const LengthF h0 = LengthF::cm(0);
+
+		// add the floorplan to the grid
+		Grid<MyGridNode> grid(20);
+		GridFactory<MyGridNode> gridFac(grid);
+
+		gridFac.addFloor(f0, h0.cm());
+
+		// remove all isolated nodes not attached to 300,300,floor0
+		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );
+
+		// stamp importance information onto the grid-nodes
+		GridImportance gridImp;
+		gridImp.addImportance(grid, h0.cm());
+
+
+		{
+			PaperPlot2D plot("floorplan_importance.eps", s1);
+			plot.setRanges(0,2100, 0,5100);
+			plot.addFloor(f0);
+			plot.addGrid(grid, ColorizerImp());
+			plot.show();
+		}
+
+
+	}
+
+
+
+
+	static void createPath() {
+
+		// load the floorplan
+		FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
+		Floor f0 = fpFac.getFloor("test1");
+		const LengthF h0 = LengthF::cm(0);
+
+		// add the floorplan to the grid
+		Grid<MyGridNode> grid(20);
+		GridFactory<MyGridNode> gridFac(grid);
+
+		gridFac.addFloor(f0, h0.cm());
+
+		// remove all isolated nodes not attached to 300,300,floor0
+		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
+
+
+
+		// start and end
+		const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
+		const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
+
+		// build all shortest path to reach th target
+		Dijkstra<MyGridNode> dijkstra;
+		DijkstraMapper accImp(grid);
+		DijkstraMapperNormal accNormal(grid);
+
+		// path without importance
+		dijkstra.build(gnStart, gnStart, accNormal);
+		DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
+
+		// stamp importance information onto the grid-nodes
+		GridImportance gridImp;
+		gridImp.addImportance(grid, h0.cm());
+
+		// path WITH importance
+		dijkstra.build(gnStart, gnStart, accImp);
+		DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
+
+		// build plot
+		K::GnuplotPlotElementLines gpPath1;		gpPath1.setLineWidth(2);	gpPath1.setColorHex("#444444");
+		K::GnuplotPlotElementLines gpPath2;		gpPath2.setLineWidth(2);	gpPath2.setColorHex("#000000");
+
+		for (DijkstraNode<MyGridNode>* dn : pathNormal) {
+			gpPath1.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
+		}
+		for (DijkstraNode<MyGridNode>* dn : pathImp) {
+			gpPath2.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
+		}
+
+		// plot the 2 paths
+		{
+			PaperPlot2D plot("floorplan_paths.eps", s1);
+			plot.setRanges(0,2100, 0,5100);
+			plot.addFloor(f0);
+			plot.plot.add(&gpPath1);		gpPath1.setCustomAttr("dashtype 3");
+			plot.plot.add(&gpPath2);
+			plot.show();
+		}
+
+		// stamp distance information onto the grid
+		// attach a corresponding weight-information to each user-grid-node
+		for (MyGridNode& node : grid) {
+			const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
+			node.distToTarget = dn->cumWeight;
+		}
+
+		// walk
+		GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);
+
+		for (int i = 0; i < 30000; ++i) {
+
+			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);
+
+		}
+
+		// plot the heat-map
+		{
+			PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1);
+			plot.setRanges(0,2100, 0,5100);
+			plot.gp << "set palette gray negative\n";
+			plot.addFloor(f0);
+			plot.addGrid(grid, ColorizeHeat(7000, 50));
+			plot.show();
+		}
+
+
+	}
+
+
+
+};
+
+#endif // PAPERVISIMPORTANCE_H