toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
01c77354c1a139aed8cd053669dfad09801efc45
IPIN2016/code/eval/SmoothingEval1.h
toni 00fca03a73 added introduction to tex
2016-04-14 13:03:16 +02:00

642 lines
29 KiB
C++
Raw Blame History

#ifndef SMOOTHINGEVAL1_H
#define SMOOTHINGEVAL1_H
#include "SmoothingEvalBase.h"
#include "FixedLagEvalBase.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/ParticleFilterEstimationWeightedAverageWithAngle.h>
class SmoothingEval1 : public FixedLagEvalBase {
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::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<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>(50);
//bf = new K::CondensationBackwardFilter<MyState>;
bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
}
void fixedIntervallSimpleTransPath1() {
runName = "fixedIntervallSimpleTransPath1";
bool smoothing_resample = false;
smoothing_time_delay = 1;
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)) );
//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()) );
}
void fixedIntervallSimpleTransPath4(){
runName = "fixedIntervallSimpleTransPath4";
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);
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 Variables
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
// ============================================================ Dijkstra ============================================== //
// Hier dauert sehr laaaaaaaannnnnnnggge @Frank
void fixedIntervallDijkstraTransPath4(){
runName = "fixedIntervallDijkstra";
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);
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 Variables
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Dijkstra
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)) );
}
/* ------------------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------- Bergwerk ------ Fusion 2016 --------------------------------------------------- */
/* ------------------------------------------------------------------------------------------------------------------------------- */
void bergwerk_path1_nexus() {
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);
//Smoothing Variables
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 15.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path1_nexus_simple() {
runName = "bergwerk_path1_nexus_simple_fixedlag";
bergwerk_path1_nexus();
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_fixedlag";
bergwerk_path1_nexus();
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_fixedlag";
bergwerk_path1_nexus();
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_fixedlag";
bergwerk_path1_nexus();
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path2_nexus_simple() {
runName = "bergwerk_path2_nexus_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path3_nexus_simple() {
runName = "bergwerk_path3_nexus_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path4_nexus_simple() {
runName = "bergwerk_path4_nexus_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path1_galaxy_simple() {
runName = "bergwerk_path1_galaxy_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path2_galaxy_simple() {
runName = "bergwerk_path2_galaxy_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path3_galaxy_simple() {
runName = "bergwerk_path3_galaxy_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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);
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//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)) );
}
void bergwerk_path4_galaxy_simple() {
runName = "bergwerk_path4_galaxy_simple_fixedlag";
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_fixedlag";
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_fixedlag";
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
Reference in New Issue View Git Blame Copy Permalink