643 lines
26 KiB
C++
643 lines
26 KiB
C++
#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);
|
|
|
|
for (auto& n : grid) {n.imp = 1;}
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
|
|
void bergwerk_path1_nexus_imp() {
|
|
|
|
runName = "bergwerk_path1_nexus_importance";
|
|
|
|
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)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path1_nexus_multi() {
|
|
|
|
runName = "bergwerk_path1_nexus_multi";
|
|
|
|
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);
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path1_nexus_shortest() {
|
|
|
|
runName = "bergwerk_path1_nexus_shortest";
|
|
|
|
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);
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.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_path2_nexus() {
|
|
BarometerEvaluation::barometerSigma = 0.10;
|
|
sr = new SensorReader("./measurements/bergwerk/path2/nexus/vor/1454779863041.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path2/nexus/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path2/nexus/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);
|
|
}
|
|
|
|
void bergwerk_path2_nexus_simple() {
|
|
|
|
runName = "bergwerk_path2_nexus_simple";
|
|
bergwerk_path2_nexus();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
|
|
void bergwerk_path2_nexus_imp() {
|
|
|
|
runName = "bergwerk_path2_nexus_importance";
|
|
bergwerk_path2_nexus();
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path2_nexus_multi() {
|
|
|
|
runName = "bergwerk_path2_nexus_multi";
|
|
bergwerk_path2_nexus();
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path2_nexus_shortest() {
|
|
|
|
runName = "bergwerk_path2_nexus_shortest";
|
|
bergwerk_path2_nexus();
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
|
|
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
|
|
walk->times = 4;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
/* ------------------------------------------------------------------------------------------------------------------------------- */
|
|
|
|
void bergwerk_path3_nexus() {
|
|
BarometerEvaluation::barometerSigma = 0.10;
|
|
sr = new SensorReader("./measurements/bergwerk/path3/nexus/vor/1454782562231.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path3/nexus/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path3/nexus/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);
|
|
}
|
|
|
|
void bergwerk_path3_nexus_simple() {
|
|
|
|
runName = "bergwerk_path3_nexus_simple";
|
|
bergwerk_path3_nexus();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
|
|
void bergwerk_path3_nexus_imp() {
|
|
|
|
runName = "bergwerk_path3_nexus_importance";
|
|
bergwerk_path3_nexus();
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path3_nexus_multi() {
|
|
|
|
runName = "bergwerk_path3_nexus_multi";
|
|
bergwerk_path3_nexus();
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path3_nexus_shortest() {
|
|
|
|
runName = "bergwerk_path3_nexus_shortest";
|
|
bergwerk_path3_nexus();
|
|
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.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_path4_nexus() {
|
|
BarometerEvaluation::barometerSigma = 0.10;
|
|
sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
|
|
//for (const K::Particle<MyState>& _p : pf->getParticles()) {
|
|
// K::Particle<MyState>& p = (K::Particle<MyState>&) _p;
|
|
// p.state.pCur = floors.gtwp[path4dbl.front()];
|
|
// p.state.pOld = p.state.pCur;
|
|
// p.state.walkState.node = grid.getNodePtrFor( conv(p.state.pCur) );
|
|
// p.state.walkState.heading = Angle::degToRad(90);
|
|
//}
|
|
}
|
|
|
|
void bergwerk_path4_nexus_simple() {
|
|
|
|
runName = "bergwerk_path4_nexus_simple";
|
|
bergwerk_path4_nexus();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
|
|
void bergwerk_path4_nexus_imp() {
|
|
|
|
runName = "bergwerk_path4_nexus_importance";
|
|
bergwerk_path4_nexus();
|
|
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
void bergwerk_path4_nexus_multi() {
|
|
|
|
runName = "bergwerk_path4_nexus_multi";
|
|
bergwerk_path4_nexus();
|
|
|
|
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 bergwerk_path4_nexus_shortest() {
|
|
|
|
runName = "bergwerk_path4_nexus_shortest";
|
|
bergwerk_path4_nexus();
|
|
|
|
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_galaxy() {
|
|
BarometerEvaluation::barometerSigma = 0.20;
|
|
stepSize = 0.9;
|
|
sr = new SensorReader("./measurements/bergwerk/path1/galaxy/vor/1454776168794.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path1/galaxy/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path1/galaxy/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
|
|
}
|
|
|
|
void bergwerk_path1_galaxy_simple() {
|
|
runName = "bergwerk_path1_galaxy_simple";
|
|
bergwerk_path1_galaxy();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path1_galaxy_multi() {
|
|
runName = "bergwerk_path1_galaxy_multi";
|
|
bergwerk_path1_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path1_galaxy_shortest() {
|
|
runName = "bergwerk_path1_galaxy_shortest";
|
|
bergwerk_path1_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
|
|
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
/* ------------------------------------------------------------------------------------------------------------------------------- */
|
|
|
|
void bergwerk_path2_galaxy() {
|
|
BarometerEvaluation::barometerSigma = 0.20;
|
|
stepSize = 0.9;
|
|
sr = new SensorReader("./measurements/bergwerk/path2/galaxy/vor/1454780113404.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path2/galaxy/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path2/galaxy/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);
|
|
}
|
|
|
|
void bergwerk_path2_galaxy_simple() {
|
|
runName = "bergwerk_path2_galaxy_simple";
|
|
bergwerk_path2_galaxy();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path2_galaxy_multi() {
|
|
runName = "bergwerk_path2_galaxy_multi";
|
|
bergwerk_path2_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path2_galaxy_shortest() {
|
|
runName = "bergwerk_path2_galaxy_shortest";
|
|
bergwerk_path2_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
|
|
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
/* ------------------------------------------------------------------------------------------------------------------------------- */
|
|
|
|
void bergwerk_path3_galaxy() {
|
|
BarometerEvaluation::barometerSigma = 0.20;
|
|
stepSize = 0.9;
|
|
sr = new SensorReader("./measurements/bergwerk/path3/galaxy/vor/1454782896548.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path3/galaxy/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path3/galaxy/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);
|
|
}
|
|
|
|
void bergwerk_path3_galaxy_simple() {
|
|
runName = "bergwerk_path3_galaxy_simple";
|
|
bergwerk_path3_galaxy();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path3_galaxy_multi() {
|
|
runName = "bergwerk_path3_galaxy_multi";
|
|
bergwerk_path3_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path3_galaxy_shortest() {
|
|
runName = "bergwerk_path3_galaxy_shortest";
|
|
bergwerk_path3_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
|
|
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
|
|
/* ------------------------------------------------------------------------------------------------------------------------------- */
|
|
|
|
|
|
void bergwerk_path4_galaxy() {
|
|
BarometerEvaluation::barometerSigma = 0.20;
|
|
stepSize = 0.9;
|
|
sr = new SensorReader("./measurements/bergwerk/path4/galaxy/vor/1454779020844.csv"); // forward
|
|
srt = new SensorReaderTurn("./measurements/bergwerk/path4/galaxy/vor/Turns.txt");
|
|
srs = new SensorReaderStep("./measurements/bergwerk/path4/galaxy/vor/Steps2.txt");
|
|
gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
|
|
}
|
|
|
|
void bergwerk_path4_galaxy_simple() {
|
|
runName = "bergwerk_path4_galaxy_simple";
|
|
bergwerk_path4_galaxy();
|
|
for (auto& n : grid) {n.imp = 1;} // remove importance
|
|
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path4_galaxy_multi() {
|
|
runName = "bergwerk_path4_galaxy_multi";
|
|
bergwerk_path4_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
|
|
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
}
|
|
|
|
void bergwerk_path4_galaxy_shortest() {
|
|
|
|
runName = "bergwerk_path4_galaxy_shortest";
|
|
bergwerk_path4_galaxy();
|
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
|
|
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
|
|
//walk->times = 2;
|
|
walk->pOther = 0.15;
|
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // EVAL1_H
|