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

added tex template

Browse Source
This commit is contained in:
toni
2016-03-29 15:32:09 +02:00
parent 21e26e6209
commit f52947fae9
30 changed files with 13110 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.6.0, 2016-03-22T18:00:37. -->
<!-- Written by QtCreator 3.6.0, 2016-03-24T18:17:49. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

22
code/Vis.h
View File

@@ -2,6 +2,7 @@
#define VIS_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSize.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
@@ -31,6 +32,7 @@ public:
K::GnuplotSplotElementLines smoothPath;
K::GnuplotSplotElementLines groundTruth;
public:
Vis() {
@@ -141,6 +143,14 @@ public:
gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";
}
void setFilteringMedian(double median){
gp << "set label 2 \"" << median << "\" at screen 0.02,0.90\n";
}
void setSmoothingMedian(double median){
gp << "set label 3 \"" << median << "\" at screen 0.02,0.85\n";
}
void removeGrid() {
gridNodes.clear();;
}
@@ -150,20 +160,26 @@ public:
particleDir.clear();
}
void addObject(const int idx, const Point3& p) {
void addObject(const int idx, const Point3& p, const std::string& hex) {
gp << "set object " << idx << " polygon ";
gp << "from " << p.x << "," << p.y << "," << p.z;
gp << " to " << p.x << "," << p.y << "," << p.z + 200;
gp << " to " << p.x << "," << p.y << "," << p.z; // close
gp << " lw 2 ";
gp << "fc rgb '" + hex + "'";
gp << "\n";
}
void setEstAndShould(const Point3& est, const Point3& should) {
addObject(2,est);
addObject(3,should);
addObject(2,est, "#000000");
addObject(3,should, "#000000");
}
void setEstAndShould2(const Point3& est, const Point3& should) {
addObject(4,est, "#59C1DA");
addObject(5,should, "#59C1DA");
}
template <typename T> void addState(const GridWalkState<T>& n) {
Point2 dir = Angle::getPointer(n.heading.getRAD());
K::GnuplotPoint3 p1(n.node->x_cm, n.node->y_cm, n.node->z_cm);

67
code/eval/FixedLagEvalBase.h
View File

@@ -19,6 +19,7 @@
#include "../particles/MyInitializer.h"
#include "../particles/smoothing/MySmoothingTransition.h"
#include "../particles/smoothing/MySmoothingTransitionSimple.h"
#include "../particles/smoothing/MySmoothingTransitionExperimental.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
@@ -68,6 +69,8 @@ protected:
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
float stepSize = 0.71;
public:
FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
@@ -84,7 +87,7 @@ public:
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
vis.gp << "unset cbrange\n";
}
@@ -236,7 +239,7 @@ public:
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
ctrl.walked_m = obs.step->steps * stepSize;
}
// process all occurred steps
@@ -273,12 +276,14 @@ public:
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
}
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
if(cnt > 19){
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
@@ -291,7 +296,7 @@ public:
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
for(int i = 0; i < MiscSettings::lag; i += MiscSettings::fixedLagGap){
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);
}
@@ -309,18 +314,20 @@ public:
statsSmoothing.add(errSmoothed);
std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl;
// plot
//plot
vis.clearStates();
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
vis.setEstAndShould(curEst, curGT);
vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
@@ -343,6 +350,42 @@ public:
}
statsout.close();
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
//vis.setEstAndShould(curEst, curGT);
//vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
}
};

521
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 SmoothingEvalBase {
class SmoothingEval1 : public FixedLagEvalBase {
public:
@@ -59,17 +59,17 @@ public:
void fixedIntervallSimpleTransPath1() {
runName = "fixedIntervallSimpleTrans";
runName = "fixedIntervallSimpleTransPath1";
bool smoothing_resample = false;
smoothing_time_delay = 1;
BarometerEvaluation::barometerSigma = 0.05;
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[path4dbl.back()]) );
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)) );
@@ -83,27 +83,37 @@ public:
void fixedIntervallSimpleTransPath4(){
runName = "fixedIntervallSimpleTrans";
bool smoothing_resample = false;
smoothing_time_delay = 1;
runName = "fixedIntervallSimpleTransPath4";
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");
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 = 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<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
// ============================================================ Dijkstra ============================================== //
// Hier dauert sehr laaaaaaaannnnnnnggge @Frank
void fixedIntervallDijkstraTransPath4(){
@@ -129,6 +139,493 @@ public:
}
/* ------------------------------------------------------------------------------------------------------------------------------- */
/* ----------------------------------------------- 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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
}
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

2
code/lukas/TurnObservation.h
View File

@@ -11,7 +11,7 @@ struct TurnObservation {
float delta_motion; //measured change of motion direction (given by PCA)
TurnObservation() {;}
TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(motion_angle) {;}
TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(motion_angle) {;}
};

68
code/main.cpp
View File

@@ -89,57 +89,47 @@ int main(void) {
// testModelWalk();
SmoothingEval1 eval;
eval.fixedIntervallDijkstraTransPath4();
eval.fixedIntervallSimpleTransPath4();
eval.run();
// Eval1 eval;
// //eval.path2_forward_simple();
// //eval.path2_forward_path();
// //eval.path3_forward_simple();
// //eval.path3_forward_path();
// //eval.path4_nexus_simple();
// //eval.path4_nexus_imp();
// //eval.path4_nexus_path();
// //eval.path4_nexus_path_b();
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();}
// //{Eval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_nexus_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path2_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_nexus_simple(); eval.run();}
// //{Eval1 eval; eval.bergwerk_path2_nexus_imp(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_nexus_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path3_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_nexus_simple(); eval.run();}
// //{Eval1 eval; eval.bergwerk_path3_nexus_imp(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_nexus_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_nexus_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();}
//{Eval1 eval; eval.bergwerk_path4_nexus_imp(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_galaxy_simple(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_galaxy_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path1_galaxy_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_galaxy_simple(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_galaxy_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path2_galaxy_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_galaxy_simple(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_galaxy_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_galaxy_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_shortest(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_galaxy_simple(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_galaxy_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path4_galaxy_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_shortest(); eval.run();}

3
code/particles/MyEvaluation.h
View File

@@ -77,6 +77,9 @@ public:
if (useTurn) {
weight *= turnEval.getProbability(p.state, observation.turn, true);
//set
p.state.measurement_angle = observation.turn->delta_heading;
}
// set and accumulate

3
code/particles/MyState.h
View File

@@ -29,6 +29,9 @@ struct MyState {
// save last hPa measurement for the smoothing process
double measurement_pressure;
// save last angle measurement for the smoothing process
double measurement_angle;
//int distanceWalkedCM;

2
code/particles/smoothing/MySmoothingTransition.h
View File

@@ -21,8 +21,6 @@ static double smoothing_walk_mu = 0.7;
static double smoothing_walk_sigma = 0.5;
static double smoothing_heading_sigma = 15.0;
static double smoothing_baro_sigma = 0.2;
static bool smoothing_baro_use_hPa = false;
static double smoothing_baro_with_measurement = false;
class MySmoothingTransition : public K::BackwardFilterTransition<MyState> {

125
code/particles/smoothing/MySmoothingTransitionExperimental.h Normal file
View File

@@ -0,0 +1,125 @@
#ifndef MYSMOOTHINGTRANSITIONEXPERIMENTAL_H
#define MYSMOOTHINGTRANSITIONEXPERIMENTAL_H
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Uniform.h>
#include <Indoor/geo/Angle.h>
#include "../MyState.h"
#include "../MyControl.h"
#include "../../Helper.h"
#include "../../toni/barometric.h"
class MySmoothingTransitionExperimental : public K::BackwardFilterTransition<MyState> {
private:
/** a simple normal distribution */
K::NormalDistribution distWalk;
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
MySmoothingTransitionExperimental() :
distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
distWalk.setSeed(4321);
}
public:
uint64_t ts = 0;
uint64_t deltaMS = 0;
/** set the current time in millisconds */
void setCurrentTime(const uint64_t ts) {
if (this->ts == 0) {
this->ts = ts;
deltaMS = 0;
} else {
deltaMS = this->ts - ts;
this->ts = ts;
}
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_new p_t (Forward Filter)
* @param particles_old p_t+1 (Smoothed Particles from Step before)
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
std::vector<K::Particle<MyState>>const& particles_old ) override {
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
// this means, predict all possible states q_t+1 with all passible states q_t
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
std::vector<std::vector<double>> predictionProbabilities;
auto p1 = particles_old.begin(); //smoothed / backward filter p_t+1
auto p2 = particles_new.begin(); //forward filter p_t
#pragma omp parallel for private(p2) shared(predictionProbabilities)
for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) {
std::vector<double> innerVector;
for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){
//!!!distance kann hier zu groß werden!!!
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
//get distance walked and getProb using the walking model
//double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1));
const double distProb = distWalk.getProbability(distance_m);
//getProb using the angle(heading) between src and dst
double angle = 0.0;
if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
}
const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
//assert(headingProb != 0.0);
//assert(distProb != 0.0);
//check how near we are to the measurement
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
//combine the probabilities
double prob = distProb * headingProb * floorProb;
innerVector.push_back(prob);
if(distance_m != distance_m) {throw "detected NaN";}
if(distProb != distProb) {throw "detected NaN";}
if(angle != angle) {throw "detected NaN";}
if(headingProb != headingProb) {throw "detected NaN";}
if(floorProb != floorProb) {throw "detected NaN";}
if(floorProb == 0) {throw "detected NaN";}
if(prob != prob) {throw "detected NaN";}
//assert(prob != 0.0);
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}
};
#endif // MYTRANSITION_H