FrankE
716b004f3c
added new eval code for new walkers improved barometer (moving avg and median) floorplan-staircase-fixes disabled step-turn (now part of transition) added abs-orientation-reader (for testing) added beacons
113 lines
4.9 KiB
C++
113 lines
4.9 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 <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::vector<int> wp = path1;// 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() {
|
|
|
|
}
|
|
|
|
|
|
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
|
|
//steps hochzaehlen weil mehr als einer in einer transition??
|
|
//increase regional average region
|
|
|
|
void setEval1() {
|
|
|
|
|
|
runName = "TODO";
|
|
|
|
// the particle filter's evaluation method
|
|
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
|
|
eval.get()->setUsage(true, true, true, true, true); // TODO: STEP TURN
|
|
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)));
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // EVAL1_H
|