slow version for frank

code/Vis.h

@@ -28,6 +28,7 @@ public:
 	K::GnuplotSplotElementLines particleDir;
 
 	K::GnuplotSplotElementLines estPath;
+    K::GnuplotSplotElementLines smoothPath;
 	K::GnuplotSplotElementLines groundTruth;
 
 public:
@@ -52,6 +53,8 @@ public:
 		particleDir.setColorHex("#444444");
 
 		estPath.setLineWidth(2);
+        smoothPath.setLineWidth(2);
+        smoothPath.setColorHex("#59C1DA");
 
 		// attach all layers
 		splot.add(&floors);
@@ -61,6 +64,7 @@ public:
 		splot.add(&particles);
 		splot.add(&groundTruth);
 		splot.add(&estPath);
+        splot.add(&smoothPath);
 
 	}
 
@@ -124,6 +128,14 @@ public:
 		}
 	}
 
+    void addSmoothPath(std::vector<Point3>& smt) {
+        smoothPath.clear();;
+        for (const Point3& p : smt) {
+            K::GnuplotPoint3 gp(p.x, p.y, p.z);
+            smoothPath.add(gp);
+        }
+    }
+
 	void setTimestamp(uint64_t ts) {
 		static uint64_t firstTs = ts;
 		gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";

code/eval/Eval1.h

@@ -215,7 +215,7 @@ public:
 
 		runName = "path4_nexus_path";
 
-		BarometerEvaluation::barometerSigma = 0.05;
+        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");

code/eval/FixedLagEvalBase.h

@@ -318,7 +318,8 @@ public:
                     }
                     vis.setTimestamp(se.ts);
                     vis.addGroundTruth(gtw);
-                    vis.addEstPath(smoothedEst);
+                    vis.addEstPath(pathEst);
+                    vis.addSmoothPath(smoothedEst);
                     vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
 
                     if (obs.barometer != nullptr) {

code/eval/SmoothingEval1.h

@@ -21,7 +21,7 @@
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
 
-class SmoothingEval1 : public FixedLagEvalBase {
+class SmoothingEval1 : public SmoothingEvalBase {
 
 public:
 
@@ -57,20 +57,21 @@ public:
 	}
 
 
-    void fixedIntervallSimpleTrans() {
+    void fixedIntervallSimpleTransPath1() {
 
         runName = "fixedIntervallSimpleTrans";
         bool smoothing_resample = false;
         smoothing_time_delay = 1;
 
-        BarometerEvaluation::barometerSigma = 0.105;
+        BarometerEvaluation::barometerSigma = 0.05;
         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>();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
-		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
 
         //Smoothing using Simple Trans
         bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -80,6 +81,53 @@ public:
 
 	}
 
+    void fixedIntervallSimpleTransPath4(){
+
+        runName = "fixedIntervallSimpleTrans";
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        BarometerEvaluation::barometerSigma = 0.05;
+        sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/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)) );
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple()) );
+    }
+
+    // Hier dauert sehr laaaaaaaannnnnnnggge @Frank
+    void fixedIntervallDijkstraTransPath4(){
+
+        runName = "fixedIntervallSimpleTrans";
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/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)) );
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransition>( new MySmoothingTransition(&grid)) );
+    }
+
 
 };
 

code/main.cpp

@@ -89,7 +89,7 @@ int main(void) {
 
 //	testModelWalk();
     SmoothingEval1 eval;
-    eval.fixedIntervallSimpleTrans();
+    eval.fixedIntervallDijkstraTransPath4();
     eval.run();
 
 //	Eval1 eval;