toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

slow version for frank

Browse Source
This commit is contained in:
toni
2016-03-23 15:16:29 +01:00
parent 94fb34e6f9
commit 21e26e6209
5 changed files with 69 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
code/Vis.h
View File

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

2
code/eval/Eval1.h
View File

@@ -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");

3
code/eval/FixedLagEvalBase.h
View File

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

58
code/eval/SmoothingEval1.h
View File

@@ -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>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
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>()));
@@ -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)) );
}
};

2
code/main.cpp
View File

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