From 21e26e6209e9f687cbeefc81bf952642aa26f575 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 23 Mar 2016 15:16:29 +0100 Subject: [PATCH] slow version for frank --- code/Vis.h | 12 ++++++++ code/eval/Eval1.h | 2 +- code/eval/FixedLagEvalBase.h | 3 +- code/eval/SmoothingEval1.h | 58 ++++++++++++++++++++++++++++++++---- code/main.cpp | 2 +- 5 files changed, 69 insertions(+), 8 deletions(-) diff --git a/code/Vis.h b/code/Vis.h index 3c63d08..c0ecf16 100644 --- a/code/Vis.h +++ b/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& 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"; diff --git a/code/eval/Eval1.h b/code/eval/Eval1.h index a8302aa..352836b 100644 --- a/code/eval/Eval1.h +++ b/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"); diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index 80e1ae5..79197ef 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/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) { diff --git a/code/eval/SmoothingEval1.h b/code/eval/SmoothingEval1.h index e945b0a..3c1e22d 100644 --- a/code/eval/SmoothingEval1.h +++ b/code/eval/SmoothingEval1.h @@ -21,7 +21,7 @@ #include #include -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* walk = new GridWalkSimpleControl(); - pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); //Smoothing using Simple Trans bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); @@ -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* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + //Smoothing using Simple Trans + bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + if(smoothing_resample) + bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + bf->setTransition(std::unique_ptr( 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* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + //Smoothing using Simple Trans + bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + if(smoothing_resample) + bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + bf->setTransition(std::unique_ptr( new MySmoothingTransition(&grid)) ); + } + }; diff --git a/code/main.cpp b/code/main.cpp index 845d289..f8d8abf 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -89,7 +89,7 @@ int main(void) { // testModelWalk(); SmoothingEval1 eval; - eval.fixedIntervallSimpleTrans(); + eval.fixedIntervallDijkstraTransPath4(); eval.run(); // Eval1 eval;