135 lines
6.6 KiB
C++
135 lines
6.6 KiB
C++
#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/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 fixedIntervallSimpleTransPath1() {
|
|
|
|
runName = "fixedIntervallSimpleTrans";
|
|
bool smoothing_resample = false;
|
|
smoothing_time_delay = 1;
|
|
|
|
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);
|
|
|
|
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()) );
|
|
|
|
}
|
|
|
|
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)) );
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // EVAL1_H
|