added smoothing files

2016-03-17 19:26:15 +01:00
parent 89bb0b8b7a
commit ee0c778728
2 changed files with 463 additions and 0 deletions
--- a/code/eval/SmoothingEval1.h
+++ b/code/eval/SmoothingEval1.h
@@ -0,0 +1,85 @@
+#ifndef SMOOTHINGEVAL1_H
+#define SMOOTHINGEVAL1_H
+
+#include "SmoothingEvalBase.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 SmoothingEval1 : public SmoothingEvalBase {
+
+public:
+
+    SmoothingEval1() {
+
+        int initX = 1120;
+        int initY = 150;
+        int initZ = 3*350;
+        int initHeading = 90;
+
+        // create the particle filterp.state.x_cm = 1120;
+        pf = new K::ParticleFilterHistory<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, initX, initY, initZ, initHeading)));
+
+		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)));
+
+
+        //create the backward smoothing filter
+        bf = new K::BackwardSimulation<MyState>(500);
+
+
+	}
+
+
+    void fixedIntervallSimpleTrans() {
+
+        runName = "fixedIntervallSimpleTrans";
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        BarometerEvaluation::barometerSigma = 0.105;
+        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)) );
+
+        //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)) );
+
+	}
+
+
+};
+
+#endif // EVAL1_H