FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

fixed baraomter issue (skip first few readings due to sensor errors)

Browse Source 
added new eval using shortest-path + plotting
removed compiler warnings for clean-code
fixed some minor issues
added new TeX code and new graphics
This commit is contained in:
FrankE
2016-02-07 13:30:04 +01:00
parent 004d1f48fd
commit deb21fc550
23 changed files with 4480 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt
View File

@@ -64,7 +64,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g
-O2
-O0
-DWITH_TESTS
-DWITH_ASSERTIONS

62
code/eval/DebugShortestPath.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef DEBUGSHORTESTPATH_H
#define DEBUGSHORTESTPATH_H
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include "../Helper.h"
#include "../Vis.h"
template <typename T> class DebugShortestPath : public GridWalkShortestPathControl<T> {
private:
Vis vis;
public:
/** ctor */
template <typename Access> DebugShortestPath(Grid<T>& grid, const Access& acc, const T& target, Helper::FHWSFloors& floors) : GridWalkShortestPathControl<T>(grid, acc, target) {
vis.particles.setColorHex("#0000ff");
vis.particles.setPointSize(1.5);
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);
if (this->recalc == 0){
vis.estPath.clear();
vis.particles.clear();
vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z));
for (int i = 0; i < (int)this->path->size()-1; ++i) {
const DijkstraNode<T>& dn1 = (*this->path)[i+0];
const DijkstraNode<T>& dn2 = (*this->path)[i+1];
K::GnuplotPoint3 p1 (dn1.element->x_cm, dn1.element->y_cm, dn1.element->z_cm);
K::GnuplotPoint3 p2 (dn2.element->x_cm, dn2.element->y_cm, dn2.element->z_cm);
vis.estPath.addSegment(p1, p2);
}
vis.show();
}
return s;
}
};
#endif // DEBUGSHORTESTPATH_H

48
code/eval/Eval1.h
View File

@@ -10,6 +10,8 @@
#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>
@@ -106,6 +108,7 @@ public:
// 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");
@@ -121,6 +124,7 @@ public:
// 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");
@@ -142,6 +146,7 @@ public:
// 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");
@@ -161,6 +166,7 @@ public:
// 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");
@@ -176,6 +182,7 @@ public:
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");
@@ -193,6 +200,7 @@ public:
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");
@@ -207,6 +215,7 @@ public:
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");
@@ -218,32 +227,37 @@ public:
}
void path4_nexus_path_b() {
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
//steps hochzaehlen weil mehr als einer in einer transition??
//increase regional average region
runName = "path4_nexus_path";
// void setEval1() {
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() {
// // 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) );
runName = "bergwerk_path1_nexus_simple";
// // 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)) );
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);
// // 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)));
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
// }
};

15
code/eval/EvalBase.h
View File

@@ -56,8 +56,11 @@ protected:
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
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!
@@ -120,7 +123,7 @@ public:
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
const int s_linearAcceleration = 2;
//const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
@@ -206,10 +209,10 @@ public:
break;
}
case s_linearAcceleration:{
baroSensorReader.readVerticalAcceleration(se);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
@@ -287,7 +290,7 @@ public:
vis.show();
// prevent gnuplot errors
usleep(1000*33);
usleep(1000*333);
}

6
code/eval/PaperVisImportance.h
View File

@@ -38,13 +38,13 @@ public:
// use node-importance as grid-color
struct ColorizerImp {
float get(const MyGridNode& n) const {return n.imp;}
bool skip(const MyGridNode& n) const {return false;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use node-distance as grid-color
struct ColorizerDist {
float get(const MyGridNode& n) const {return n.distToTarget;}
bool skip(const MyGridNode& n) const {return false;}
bool skip(const MyGridNode& n) const {(void) n; return false;}
};
// use num-visited as grid-color
@@ -166,7 +166,7 @@ public:
if (i % 250 == 0) {std::cout << i << std::endl;}
const MyGridNode& nStart = gnEnd;
GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
//GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
}

2
code/frank/Settings.h
View File

@@ -22,7 +22,7 @@ public:
const double pl = 2.7; // 2.7
const double tx = -46;
const float ibPLE = 1.9;
const float ibPLE = 1.5;
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl);

3
code/lukas/StepEvaluation.h
View File

@@ -19,6 +19,9 @@ public:
double getProbability(const MyState& state, const StepObservation* obs) const {
(void) state;
(void) obs;
return 1;
// see: particle-filter-control-data

4
code/lukas/TurnEvaluation.h
View File

@@ -17,6 +17,10 @@ public:
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
(void) state;
(void) obs;
(void) simple;
return 1;
// see: particle-filter-control-data

12
code/main.cpp
View File

@@ -18,6 +18,9 @@
#include "eval/PaperVisDijkstra.h"
#include "eval/PaperVisGrid.h"
float BarometerEvaluation::barometerSigma = NAN;
Settings settings;
void testModelWalk() {
@@ -28,7 +31,7 @@ void testModelWalk() {
Helper::buildTheGrid(grid, floors);
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
//MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
Vis vis;
vis.addFloor(floors.f0, floors.h0);
@@ -89,9 +92,14 @@ int main(void) {
//eval.path2_forward_path();
//eval.path3_forward_simple();
//eval.path3_forward_path();
//eval.path4_nexus_simple();
eval.path4_nexus_imp();
//eval.path4_nexus_imp();
//eval.path4_nexus_path();
//eval.path4_nexus_path_b();
eval.bergwerk_path1_nexus_simple();
eval.run();
// PaperVisGrid::showStairs();

6
code/particles/MyInitializer.h
View File

@@ -9,12 +9,16 @@
class MyInitializer : public K::ParticleFilterInitializer<MyState> {
private:
Grid<MyGridNode>& grid;
int x_cm;
int y_cm;
int z_cm;
int heading;
Grid<MyGridNode>& grid;
public:

2
code/particles/MyTransition.h
View File

@@ -104,7 +104,7 @@ public:
// randomly move the particle within its target grid (box)
// (z remains unchanged!)
const int grid_size_cm = grid.getGridSize_cm();
//const int grid_size_cm = grid.getGridSize_cm();
// new position (x,y) is randomly distributed within the target node
Point3 noise = Point3(0,0,0); // TODO

2
code/reader/SensorReaderStep.h
View File

@@ -34,12 +34,10 @@ public:
/** read the next sensor entry */
SensorEntryStep getNext() {
char delim;
SensorEntryStep entry;
fp >> entry.ts;
int i = 0;
return entry;

6
code/toni/BarometerEvaluation.h
View File

@@ -9,13 +9,17 @@ static constexpr double g_BarometerObservation = 0.0;
class BarometerEvaluation {
public:
static float barometerSigma;//= 0.12+0.04;
public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
// //rho_z
double barometerSigma = 0.06;//0.12+0.04;//0.09;
//double barometerSigma = 0.06;//0.12+0.04;//0.09;
// //The height of the single floor levels.
// const static double floor_height[3] = {4.1, 3.4, 3.4};

24
code/toni/BarometerSensorReader.h
View File

@@ -1,6 +1,6 @@
#pragma once
#include "circular.h"
//#include "circular.h"
#include "BarometerObservation.h"
#include "../reader/SensorReader.h"
#include <sstream>
@@ -23,6 +23,9 @@ private:
static constexpr int avgSize = 10;
static constexpr int startAvgSize = 10;
// skip the first 1-2 seconds and let the sensor settle itself
uint64_t skipTS = 0;
public:
BarometerSensorReader(): avg(avgSize), avgStart(startAvgSize) {
@@ -30,16 +33,21 @@ public:
}
BarometerObservation* readBarometer(const SensorEntry& se) {
// skip the first few 1.5 seconds
if (skipTS == 0) {skipTS = se.ts;}
if (se.ts - skipTS < 3000) {return nullptr;}
std::string tmp = se.data;
BarometerObservation* obs = new BarometerObservation();
const float cur = std::stof(tmp);
// get the next hPa reading and average it
avg.add(std::stof(tmp));
avg.add(cur);
// average the first few readings as reference
if (avgStart.getNumUsed() < startAvgSize) {
avgStart.add(std::stof(tmp));
avgStart.add(cur);
}
// current average relative to the start-average
@@ -50,13 +58,13 @@ public:
}
//TODO
void readVerticalAcceleration(const SensorEntry& se){
// //TODO
// void readVerticalAcceleration(const SensorEntry& se){
//Problem: Koordinatensystem LinearAcceleraton ist relativ zum Telefon und nicht zum
//Weltkoordinatensystem. Brauchen die Beschleunigung nach Oben in Weltkoordinaten.
// //Problem: Koordinatensystem LinearAcceleraton ist relativ zum Telefon und nicht zum
// //Weltkoordinatensystem. Brauchen die Beschleunigung nach Oben in Weltkoordinaten.
}
// }
};

2
code/toni/barometric.h
View File

@@ -1,3 +1,5 @@
#define BAROMETRIC
#ifndef BAROMETRIC
#define BAROMETRIC