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

started working on the tex-part

Browse Source 
started working on eval-graphics
ned helper methods
tested some new aspects
some fixes and changes
added some graphics
new test-floorplan
many cleanups
This commit is contained in:
FrankE
2016-02-03 21:17:15 +01:00
parent 8a57b4cdbd
commit c5a5acbbf6
40 changed files with 69163 additions and 275 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
-O0
-O1
-DWITH_TESTS
-DWITH_ASSERTIONS

28
code/DijkstraMapper.h
View File

@@ -4,7 +4,7 @@
#include "MyGridNode.h"
/**
* allows dijkstra calculation on top of our data-structure
* allows BETTER dijkstra calculation on top of our data-structure
*/
class DijkstraMapper {
@@ -19,9 +19,31 @@ public:
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2.0);
float d = ((Point3)n1 - (Point3)n2).length(2) ;
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
d /= std::pow(n2.imp, 3);
return d;
}
};
/**
* allows NORMAL dijkstra calculation on top of our data-structure
*/
class DijkstraMapperNormal {
Grid<MyGridNode>& grid;
public:
DijkstraMapperNormal(Grid<MyGridNode>& grid) : grid(grid) {;}
int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
return ((Point3)n1 - (Point3)n2).length();
}
};

40
code/Helper.h
View File

@@ -25,23 +25,23 @@ public:
// if (z_cm < 360+340) {return 1;}
// if (z_cm < 360+340+340) {return 2;}
// return 3;
if (z_cm < 180) {return 0;}
if (z_cm < 360+180) {return 1;}
if (z_cm < 360+340+180) {return 2;}
if (z_cm < 380) {return 0;}
if (z_cm < 380+340) {return 1;}
if (z_cm < 380+340+340) {return 2;}
return 3;
}
/** convert height (in cm) to floor-numbers */
static int getFloorNrFloat(float z_cm) {
return z_cm / 340.0f;
static int getFloorNrFloat(const float z_cm) {
return z_cm / 350.0f;
}
static int getHeight(const int floorNr) {
switch(floorNr) {
case 0: return 0;
case 1: return 360;
case 2: return 360+340;
case 3: return 360+340+340;
case 1: return 380;
case 2: return 380+340;
case 3: return 380+340+340;
default: throw "error";
}
}
@@ -86,14 +86,21 @@ public:
f.s23 = fpFac.getStairs("staircase_2_3");
OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222);
OldGroundTruth gtwp05(MiscSettings::floorplan, "ground_truth_0_5", 2.822222);
OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222);
OldGroundTruth gtwp15(MiscSettings::floorplan, "ground_truth_1_5", 2.822222);
OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222);
OldGroundTruth gtwp25(MiscSettings::floorplan, "ground_truth_2_5", 2.822222);
OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222);
for (auto it : gtwp0.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
for (auto it : gtwp1.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
for (auto it : gtwp2.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
for (auto it : gtwp3.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}
for (auto it : gtwp0.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
for (auto it : gtwp1.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
for (auto it : gtwp2.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
for (auto it : gtwp3.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}
for (auto it : gtwp05.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(0)+getHeight(1))/2);}
for (auto it : gtwp15.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(1)+getHeight(2))/2);}
for (auto it : gtwp25.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(2)+getHeight(3))/2);}
return f;
@@ -112,16 +119,15 @@ public:
gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
// maybe the two sides are wrong?
PlatformStair psUpperLeft;
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
psUpperLeft.s1 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
psUpperLeft.s2 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
// vis.gp << "set xrange [1100:1800]\n";
// vis.gp << "set yrange [4500:5200]\n";
// vis.gp << "set xrange [1100:1800]\n";
// vis.gp << "set yrange [4500:5200]\n";
PlatformStair psUpperRight;
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));

3
code/MyGridNode.h
View File

@@ -15,6 +15,9 @@ struct MyGridNode : public GridNode, public GridPoint {
/** node importance based on surroundings */
float imp = 1.0;
/** used for eval */
int cnt = 0;
public:
/** needed ctor */

7
code/Settings.h
View File

@@ -10,13 +10,14 @@
namespace MiscSettings {
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan_new.svg";
const std::string floorplanPlot = "/mnt/data/workspaces/Fusion2016/code/plan_plots.svg";
const int gridSize_cm = 40;
const int gridSize_cm = 20;
const int timeSteps = 500;
const int numParticles = 2500;
const int numParticles = 7500;
}

31
code/Vis.h
View File

@@ -9,9 +9,12 @@
#include <Indoor/geo/Length.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Angle.h>
#include <Indoor/grid/walk/GridWalkState.h>
#include "eval/GroundTruthWay.h"
class Vis {
public:
@@ -22,6 +25,7 @@ public:
K::GnuplotSplotElementColorPoints gridNodes;
K::GnuplotSplotElementLines gridEdges;
K::GnuplotSplotElementPoints particles;
K::GnuplotSplotElementLines particleDir;
K::GnuplotSplotElementLines groundTruth;
K::GnuplotSplotElementLines estPath;
@@ -42,12 +46,17 @@ public:
groundTruth.setLineWidth(2);
groundTruth.setColorHex("#666666");
particles.setColorHex("#0000ff");
particles.setPointSize(0.3);
particleDir.setColorHex("#444444");
estPath.setLineWidth(2);
// attach all layers
splot.add(&floors);
splot.add(&gridNodes);
splot.add(&gridEdges);
splot.add(&particleDir);
splot.add(&particles);
splot.add(&groundTruth);
splot.add(&estPath);
@@ -71,6 +80,8 @@ public:
/** add the grid to the plot */
template <typename T> Vis& addGrid(Grid<T>& grid) {
std::set<uint64_t> used;
float max = 0;
for (const T& n1 : grid) {
if (n1.distToTarget > max) {max = n1.distToTarget;}
@@ -84,10 +95,14 @@ public:
//const float color = n1.distToTarget/max;
const float color = 0;
gridNodes.add(p1, color);
// for (const T& n2 : grid.neighbors(n1)) {
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
// gridEdges.addSegment(p1, p2);
// }
for (const T& n2 : grid.neighbors(n1)) {
const uint64_t idx = n1.getIdx() * n2.getIdx();
if (used.find(idx) == used.end()) {
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
gridEdges.addSegment(p1, p2);
used.insert(idx);
}
}
}
return *this;
}
@@ -119,6 +134,7 @@ public:
void clearStates() {
particles.clear();
particleDir.clear();
}
void addObject(const int idx, const Point3& p) {
@@ -136,9 +152,14 @@ public:
}
template <typename T> void addState(const GridWalkState<T>& n) {
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
Point2 dir = Angle::getPointer(n.heading.getRAD());
K::GnuplotPoint3 p1(n.node->x_cm, n.node->y_cm, n.node->z_cm);
K::GnuplotPoint3 p2 = p1 + K::GnuplotPoint3(dir.x, dir.y, 0) * 85;
particles.add(p1);
particleDir.addSegment(p1, p2);
}
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
particles.clear();;
for (const GridWalkState<T>& n : states) {

63
code/eval/Eval.h
View File

@@ -18,50 +18,53 @@ public:
Eval() {
// Eval() {
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
// pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
// MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
// MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
// //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);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
// sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
// srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
// srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
// gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
}
// }
// //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";
void setEval1() {
// // the particle filter's evaluation method
// std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
// eval.get()->setUsage(true, false, false, 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>()) );
runName = "TODO";
// // 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.33f)));
// the particle filter's evaluation method
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval.get()->setUsage(true, false, false, 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>()) );
// 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>()));
}
// }

98
code/eval/Eval1.h Normal file
View File

@@ -0,0 +1,98 @@
#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 <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>();
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");
gtw = getGroundTruthWay(*sr, floors.gtwp, wp);
}
//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

111
code/eval/EvalBase.h
View File

@@ -47,9 +47,16 @@ protected:
std::string runName;
GroundTruthWay gtw;
std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// OLD
//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// NEW
std::vector<int> path1 = {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> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
public:
@@ -66,13 +73,17 @@ public:
}
static GridPoint conv(const Point3& p) {
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> path;
for (int id : ids) {
auto it = waypoints.find(id);
assert(it != waypoints.end());
if(it == waypoints.end()) {throw "not found";}
path.push_back(it->second);
}
@@ -138,6 +149,11 @@ public:
// the to-be-evaluated observation
MyObservation obs;
obs.step = new StepObservation(); obs.step->steps = 0;
obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
// control data
MyControl ctrl;
std::vector<Point3> pathEst;
@@ -148,7 +164,8 @@ public:
K::Statistics<double> stats;
int cnt = 0;
// process each sensor reading
// process each single sensor reading
while(sr->hasNext()) {
// get the next sensor reading from the CSV
@@ -187,58 +204,39 @@ public:
}
// process all occurred turns
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;
}
// scheduled transition every 500 ms
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
// process all occurred steps
while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
obs.turn->delta_heading += _to.delta_heading;
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
//Steps are sorted in the list by timestamp.
//If the current observation timestamp is bigger/equal
//to the current step timestamp, use this step as observation
//and remove it from the list.
//The new first timestamp in the list will be then be the next one (timestamp-wise)
StepObservation so;
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
so.step = true;
so.ts = current_time;
}
obs.step = &so;
step_observations.pop_front();
}
else {
so.step = false;
so.ts = current_time;
obs.step = &so;
}
TurnObservation to;
//same principal as for steps is applied for turns
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
to = turn_observations.front();
obs.turn = &to;
turn_observations.pop_front();
}
else {
to.delta_heading = 0.0;
to.delta_motion = 0.0;
obs.turn = &to;
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(nullptr, obs);
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
const int offset = 0; // 750
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 8 scans due to uniform distribution start
@@ -251,28 +249,31 @@ public:
// plot
vis.clearStates();
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
const K::Particle<MyState>& p = pf->getParticles()[i];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
vis.show();;
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.1;
vis.gp << "set arrow 111 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
}
}
{
// vis.setShowParticles(false);
// vis.setShowTime(false);
// vis.setShowCurPos(false);
// vis.debugProcess(0, pathEst, gtw, pf, layers);
// std::ofstream out("/tmp/" + runName + ".data");
// out << vis.getDataset();
// out.close();
}
sleep(1000);
}

95
code/eval/PaperPlot.h Normal file
View File

@@ -0,0 +1,95 @@
#ifndef PAPERPLOT_H
#define PAPERPLOT_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 <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot {
public:
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementColorPoints nodes;
public:
PaperPlot() {
floors.setLineWidth(2);
plot.add(&nodes);
plot.add(&floors);
gp << "set ticslevel 0\n";
//gp << "set zrange [0:0]\n";
}
void show() {
gp.draw(plot);
gp.flush();;
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f, const LengthF height) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
floors.addSegment(p1, p2);
}
}
/** add the grid to the plot */
template <typename T> void addGrid(Grid<T>& grid) {
// std::set<uint64_t> used;
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = n1.imp;
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
for (const T& n1 : grid) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
const float color = n1.imp;
//const float color = n1.distToTarget/max;
//const float color = 0;
nodes.add(p1, color);
// for (const T& n2 : grid.neighbors(n1)) {
// const uint64_t idx = n1.getIdx() * n2.getIdx();
// if (used.find(idx) == used.end()) {
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
// gridEdges.addSegment(p1, p2);
// used.insert(idx);
// }
// }
}
}
};
#endif // PAPERPLOT_H

141
code/eval/PaperPlot2D.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef PAPERPLOT2D_H
#define PAPERPLOT2D_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot2D {
public:
struct Size {
float w;
float h;
Size(const float w, const float h) : w(w), h(h) {;}
};
public:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines floors;
K::GnuplotPlotElementColorPoints nodes;
std::string file;
public:
PaperPlot2D(const std::string& file, Size s) : file(file) {
toFile(file, s);
setup();
}
PaperPlot2D() {
setup();
}
private:
void setup() {
floors.setLineWidth(1.5);
nodes.setPointType(7);
plot.add(&nodes);
plot.add(&floors);
gp << "unset border\n";
gp << "unset colorbox\n";
gp << "set tics scale 0,0\n"; // HACK! "unset tics\n" segfaults current gnuplot version...
gp << "set format x ' '\n";
gp << "set format y ' '\n";
gp << "set size ratio -1\n";
}
void toFile(const std::string& file, const Size s) {
gp << "set output '" << file << "'\n";
gp << "set terminal eps size " << s.w << "," << s.h << "\n";
}
public:
void setRanges(const float x1, const float x2, const float y1, const float y2) {
gp << "set xrange [" << x1 << ":" << x2 << "]\n";
gp << "set yrange [" << y1 << ":" << y2 << "]\n";
}
void show() {
gp.draw(plot);
if (file.length() != 0) {
std::string dataFile = file + ".dat";
std::ofstream os(dataFile.c_str());
os << gp.getBuffer();
os.close();
}
gp.flush();
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint2 p1(l.p1.x, l.p1.y);
const K::GnuplotPoint2 p2(l.p2.x, l.p2.y);
floors.addSegment(p1, p2);
}
}
// void removeGrid() {
// gp << "unset object\n";
// }
/** add the grid to the plot */
template <typename T, typename Colorizer> void addGrid(Grid<T>& grid, const Colorizer col) {
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = col.get(n1);
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
// for (const T& n1 : grid) {
// const K::GnuplotPoint2 p1(n1.x_cm, n1.y_cm);
// const float color = n1.imp;
// //const float color = n1.distToTarget/max;
// //const float color = 0;
// nodes.add(p1, color);
// }
int i = 0;
for (const T& n1 : grid) {
if (col.skip(n1)) {continue;}
gp << "set object " << (++i) << " rectangle center " << n1.x_cm << "," << n1.y_cm << " size 20,20 fs solid noborder fc palette cb " << col.get(n1) << "\n";
}
}
};
#endif // PAPERPLOT2D_H

4
code/eval/PaperVisDijkstra.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef PAPERVISDIJKSTRA_H
#define PAPERVISDIJKSTRA_H
#endif // PAPERVISDIJKSTRA_H

190
code/eval/PaperVisImportance.h Normal file
View File

@@ -0,0 +1,190 @@
#ifndef PAPERVISIMPORTANCE_H
#define PAPERVISIMPORTANCE_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>
#include "PaperPlot.h"
#include "PaperPlot2D.h"
#include "../MyGridNode.h"
#include "../Settings.h"
#include "../DijkstraMapper.h"
PaperPlot2D::Size s1 = PaperPlot2D::Size(2,4);
class PaperVisImportance {
public:
static float clamp(const float in, const float min, const float max) {
if (in < min) {return min;}
if (in > max) {return max;}
return in;
}
// 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;}
};
// 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;}
};
// use num-visited as grid-color
struct ColorizeHeat {
int maxCnt; int cutoff;
ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
bool skip(const MyGridNode& n) const {return n.cnt < cutoff;} // skip to reduce plot size
};
static void createImportance() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
{
PaperPlot2D plot("floorplan_importance.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.addGrid(grid, ColorizerImp());
plot.show();
}
}
static void createPath() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
// start and end
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
// build all shortest path to reach th target
Dijkstra<MyGridNode> dijkstra;
DijkstraMapper accImp(grid);
DijkstraMapperNormal accNormal(grid);
// path without importance
dijkstra.build(gnStart, gnStart, accNormal);
DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
// path WITH importance
dijkstra.build(gnStart, gnStart, accImp);
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// build plot
K::GnuplotPlotElementLines gpPath1; gpPath1.setLineWidth(2); gpPath1.setColorHex("#444444");
K::GnuplotPlotElementLines gpPath2; gpPath2.setLineWidth(2); gpPath2.setColorHex("#000000");
for (DijkstraNode<MyGridNode>* dn : pathNormal) {
gpPath1.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
for (DijkstraNode<MyGridNode>* dn : pathImp) {
gpPath2.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
// plot the 2 paths
{
PaperPlot2D plot("floorplan_paths.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.plot.add(&gpPath1); gpPath1.setCustomAttr("dashtype 3");
plot.plot.add(&gpPath2);
plot.show();
}
// stamp distance information onto the grid
// attach a corresponding weight-information to each user-grid-node
for (MyGridNode& node : grid) {
const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
node.distToTarget = dn->cumWeight;
}
// walk
GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);
for (int i = 0; i < 30000; ++i) {
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);
}
// plot the heat-map
{
PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.gp << "set palette gray negative\n";
plot.addFloor(f0);
plot.addGrid(grid, ColorizeHeat(7000, 50));
plot.show();
}
}
};
#endif // PAPERVISIMPORTANCE_H

4
code/frank/BeaconEvaluation.h
View File

@@ -44,7 +44,7 @@ public:
// floor difference?
//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
const float floorDist = std::round(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
const float floorDist = std::ceil(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
// estimate the rssi depending on above distance
const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
@@ -57,7 +57,7 @@ public:
// const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
// probability? (sigma grows with measurement's age)
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 2.0;
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);

71
code/frank/Settings.h
View File

@@ -25,7 +25,7 @@ public:
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);
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1);
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl);
@@ -52,25 +52,62 @@ public:
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
const int ibOff = +2;
const float ibPLE = 1.9;
addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3)); // id:16
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2)); // id:18
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1)); // id:11
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0)); // id:20
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2)); // idis switchboard
addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2)); // idis outside
addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2)); // idis toni
//addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47
//addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft
//addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft
// OLD
// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
addBeacon("78:A5:04:1F:91:08", -88, ibPLE, 1448, 4538, 3);
addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 2028, 4528, 3);
// const double pl = 2.7; // 2.7
// const double tx = -46;
// addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl-1);
// addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl);
// addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1);
// addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
// addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl);
// // 2nd floor (vague AP position)
// addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl);
// addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
// // 1st floor (vague AP position)
// addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, Helper::getHeight(1), tx, pl);
// addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
// // 0th floor (exact AP position)
// addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
// addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
// const int ibOff = +2;
// const float ibPLE = 1.9;
// addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3)); // id:16
// addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2)); // id:18
// addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1)); // id:11
// addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0)); // id:20
// addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2)); // idis switchboard
// addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2)); // idis outside
// addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2)); // idis toni
// addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47
// addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft
// addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft
//// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
//// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
// addBeacon("78:A5:04:1F:91:08", -88, ibPLE, 1448, 4538, 3);
// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 2028, 4528, 3);
}

20
code/frank/WiFiEvaluation.h
View File

@@ -37,7 +37,7 @@ public:
//const double tx = -48; // tablet
//const double pl = 3.15;
const float waf = 7;//10.0; // was 7 before?! has something todo with the floor heights / levels
const float waf = 8;//10.0; // was 7 before?! has something todo with the floor heights / levels
// get the ap the client had the strongest measurement for
//const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
@@ -55,9 +55,9 @@ public:
const float distToAP_m = state.pCur.getDistance(*ap) / 100.0;
// floor difference?
const float floorDiff = //std::ceil(
std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
//);
const float floorDiff = std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
//const float floorDiff = std::round(std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)));
//const float floorDiff = std::abs(ap->z - state.pCur.z) / 340;
// estimate the rssi depending on above distance
@@ -71,7 +71,7 @@ public:
//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
// probability? (sigma grows with measurement's age)
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
const double sigma = (8) + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.5;
const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI); // absolute
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); // relative
@@ -80,8 +80,14 @@ public:
}
const double lambda = 0.25; //0.12;
return lambda * exp(- lambda * (-prob));
//const double lambda = 0.05;//0.15; //0.12;
//return lambda * exp(- lambda * (-prob));
return std::pow(std::exp(prob), 0.1);
// if (prob < -30) {return 0.01;}
// if (prob < -15) {return 0.50;}
// else {return 1.00;}
//return prob;
}

60
code/lukas/StepEvaluation.h
View File

@@ -8,8 +8,8 @@
#include "StepObservation.h"
#include <math.h>
static constexpr double mu_walk = 40;
static constexpr double sigma_walk = 15;
static constexpr double mu_walk = 90;
static constexpr double sigma_walk = 30;
static constexpr double mu_stop = 0;
static constexpr double sigma_stop = 5;
@@ -21,29 +21,49 @@ public:
return 1;
// double distance = state.distanceWalkedCM;
const float mdlWalked_m = state.walkState.distanceWalked_m;
// double a = 1.0;
// double mu_distance = 0; //cm
// double sigma_distance = 10.0; //cm
((MyState&)state).walkState.distanceWalked_m = 0;
// if(obs->step) {
// a = 1.0;
// mu_distance = mu_walk;//80.0; //cm
// sigma_distance = sigma_walk;//40.0; //cm
// }
const float stepSize_m = 0.71;
const float sensSigma_m = 0.05 + (0.05 * obs->steps);
const float sensWalked_m = obs->steps * stepSize_m;
// else {
// a = 0.0;
// mu_distance = mu_stop; //cm
// sigma_distance = sigma_stop; //cm
// }
if (obs->steps > 1) {
int i = 0;
int j = i+1; ++j;
}
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
if (prob != prob) {
throw 1;
}
return prob;
// float a = 1.0;
// float mu_distance = 0;
// float sigma_distance = 0;
// if(obs->step) {
// a = 1.0;
// mu_distance = mu_walk;
// sigma_distance = sigma_walk;
// }
// else {
// a = 0.0;
// mu_distance = mu_stop;
// sigma_distance = sigma_stop;
// }
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
// return p;
// return p;
}
};

9
code/lukas/StepObservation.h
View File

@@ -2,11 +2,14 @@
#define STEPOBSERVATION_H
struct StepObservation {
float ts;
bool step;
float ts;
int steps = 0;
StepObservation() {;}
StepObservation(const float ts) : ts(ts), step(false){;}
StepObservation(const float ts) : ts(ts), steps(0) {;}
};

20
code/lukas/TurnEvaluation.h
View File

@@ -7,7 +7,7 @@
//#include <boost/math/special_functions/bessel.hpp>
#include <math.h>
static constexpr double sigma_heading = 35;
//static constexpr double sigma_heading = 5;
class TurnEvaluation {
@@ -19,6 +19,24 @@ public:
return 1;
// get the current heading-change
const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// and reset it as it was evaluated
((MyState&) state).walkState.headingChange_rad = 0;
// proability?
const float sigma = 15.0;
const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
if (prob != prob) {
throw 1;
}
return prob;
// //Particle's heading change
// double delta_heading_particle = state.heading - state.heading_old;

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(delta_motion) {;}
TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(motion_angle) {;}
};

30
code/main.cpp
View File

@@ -11,8 +11,12 @@
#include "DijkstraMapper.h"
#include "eval/Eval.h"
#include "eval/Eval1.h"
#include "eval/EvalBase.h"
#include "eval/PaperVisImportance.h"
#include "eval/PaperVisDijkstra.h"
Settings settings;
void testModelWalk() {
@@ -31,6 +35,10 @@ void testModelWalk() {
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.gp << "set xrange [1100:1800]\n";
vis.gp << "set yrange [4500:5200]\n";
// vis.gp << "set xrange [1000:4000]\n";
// vis.gp << "set yrange [1000:4000]\n";
// vis.gp << "set zrange [0:600]\n";
@@ -48,16 +56,17 @@ void testModelWalk() {
// track the number-of-visits for each node to draw something like a particle-heat-map?
// show the importance factors
// vis.addGrid(grid);
// vis.show();
// sleep(100);
// vis.removeGrid();
vis.addGrid(grid);
vis.show();
sleep(100);
vis.removeGrid();
Distribution::Normal<float> wDist(0.3, 0.3);
Distribution::Normal<float> wHead(0.3, 0.3);
while(true) {
for (GridWalkState<MyGridNode>& state : states) {
state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
state = walk.getDestination(grid, state, std::abs(wDist.draw()), wHead.draw());
}
usleep(1000*80);
vis.showStates(states);
@@ -74,11 +83,14 @@ void testModelWalk() {
int main(void) {
//testModelWalk();
// testModelWalk();
Eval eval;
eval.setEval1();
eval.run();
// Eval1 eval;
// eval.setEval1();
// eval.run();
PaperVisImportance::createImportance();
PaperVisImportance::createPath();
return 0;

4
code/particles/MyControl.h
View File

@@ -3,6 +3,10 @@
struct MyControl {
float walked_m = 0;
float headingChange_rad = 0;
};
#endif // MYCONTROL_H

18
code/particles/MyEvaluation.h
View File

@@ -61,14 +61,13 @@ public:
weight *= beaconEval.getProbability(p.state, observation);
}
// if (useStep) {
// weight *= stepEval.getProbability(p.state, observation.step);
// p.state.distanceWalkedCM = 0.0;
// }
if (useStep) {
weight *= stepEval.getProbability(p.state, observation.step);
}
// if (useTurn) {
// weight *= turnEval.getProbability(p.state, observation.turn, true);
// }
if (useTurn) {
weight *= turnEval.getProbability(p.state, observation.turn, true);
}
// set and accumulate
p.weight = weight;
@@ -76,6 +75,11 @@ public:
}
// reset
observation.step->steps = 0;
observation.turn->delta_heading = 0;
observation.turn->delta_motion = 0;
return sum;
}

1
code/particles/MyInitializer.h
View File

@@ -47,7 +47,6 @@ public:
p.state.pOld = p.state.pCur;
p.state.walkState.heading = Heading::rnd();
p.state.distanceWalkedCM = 0;
p.state.hPa = 0;
}

2
code/particles/MyState.h
View File

@@ -23,7 +23,7 @@ struct MyState {
// the grid-walk state
GridWalkState<MyGridNode> walkState;
int distanceWalkedCM;
//int distanceWalkedCM;
// double heading_old;

74
code/particles/MyTransition.h
View File

@@ -31,7 +31,7 @@ private:
/** a simple normal distribution */
K::UniformDistribution distWalkStop;
K::NormalDistribution distWalk;
K::NormalDistribution distWalkPerSec;
K::NormalDistribution distStop;
@@ -48,10 +48,10 @@ public:
*/
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
grid(grid), walker(walker),
distWalkStop(0.0, 1.0), distWalk(1.5, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
distWalkStop(0.0, 1.0), distWalkPerSec(1.0, 0.3), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
distWalkStop.setSeed(1234);
distWalk.setSeed(1234);
distWalkPerSec.setSeed(1234);
distStop.setSeed(1234);
distBaro.setSeed(5678);
@@ -90,20 +90,17 @@ public:
p.state.pOld = p.state.pCur;
// 10% stand still, 90% walk
double dist_m;
if (distWalkStop.draw() > 0.9) {
dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
} else {
dist_m = std::abs(distWalk.draw() * deltaMS / 1000.0);
}
// update cumulative distance
p.state.distanceWalkedCM += std::abs(dist_m * 100.0);
// // 10% stand still, 90% walk
// double dist_m;
// if (distWalkStop.draw() > 0.9) {
// dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
// } else {
// dist_m = std::abs(distWalkPerSec.draw() * deltaMS / 1000.0);
// }
// get new destination
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );
p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad );
// randomly move the particle within its target grid (box)
// (z remains unchanged!)
@@ -114,56 +111,9 @@ public:
p.state.pCur = (Point3) *p.state.walkState.node + noise;
// --- ATTENTION HORRIBLE CODE INCOMING. ---
// update the baromter
p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
// //how many floors are changed? and in what direction (given by the sign)
// double numFloorChanged = p.state.z_nr_old - p.state.z_nr;
// //The height of the single floor levels.
// const static double floor_height[3] = {4.1, 3.4, 3.4};
// //update barometer
// if(USE_BAROMETRIC_FORMULAR){
// //height the particle has climbed.
// double h_1 = 0.0;
// double mu = 0.0;
// //we need only the sign of the floors changed, since the pressure change between the floors
// //is calculated within s_getAtmosphericPressure
// numFloorChanged = sgn(numFloorChanged);
// for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
// h_1 += floor_height[i];
// }
// {
// // use the barometric formular to calculate the relative pressure
// // the calculation is done assuming sea level height at every floor.
// double mslp = BarometricFormular::s_getSeaLevelPressure();
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
// mu = std::abs(mslp - pressure);
// }
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
// p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
// else
// p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
// }
// else{
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
// p.state.hPa += numFloorChanged * distBaro.draw();
// else
// p.state.hPa = numFloorChanged * distBaro.draw();
// }
// // sanity check
// if (p.state.heading != p.state.heading) {throw "detected NaN";}
// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
// // ensure p.state.z_nr IS discreet
// if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}
}

84
code/plan.svg
View File

@@ -46,13 +46,13 @@
inkscape:window-height="845"
id="namedview3234"
showgrid="true"
inkscape:zoom="0.84948895"
inkscape:cx="272.89941"
inkscape:cy="1411.2257"
inkscape:zoom="3.3979558"
inkscape:cx="1962.4967"
inkscape:cy="1511.039"
inkscape:window-x="0"
inkscape:window-y="0"
inkscape:window-maximized="1"
inkscape:current-layer="layer8"
inkscape:current-layer="layer9"
inkscape:object-nodes="true"
units="px"
showborder="true"
@@ -1464,7 +1464,7 @@
inkscape:groupmode="layer"
id="layer18"
inkscape:label="ground_truth_2"
style="display:none">
style="display:inline">
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:16px;line-height:125%;font-family:Times;-inkscape-font-specification:Times;text-align:center;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -2022,7 +2022,7 @@
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 453.54331,485.43304 0,251.5748"
d="m 453.54331,460.62989 0,276.37795"
id="path4616"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
@@ -2283,6 +2283,12 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5901"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 453.54331,485.43304 53.1496,0"
id="path4954"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
</g>
<g
inkscape:groupmode="layer"
@@ -2950,12 +2956,28 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5905"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,485.43304 63.77952,0"
id="path4952"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,712.20469 63.77952,0"
id="path4958"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2097.6378,340.15744 0,60.23623"
id="path4968"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer9"
inkscape:label="floor_2"
style="display:none"
style="display:inline"
transform="translate(0,4.2364502e-5)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -3167,7 +3189,7 @@
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1984.252,340.1574 155.9055,4e-5 0,60.23623 -155.9055,-5e-5"
d="m 1927.5591,340.1574 212.5984,4e-5 0,60.23623 -212.5984,-5e-5"
id="path11661"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccc" />
@@ -3628,14 +3650,14 @@
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:16px;line-height:125%;font-family:Times;-inkscape-font-specification:Times;text-align:center;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none"
x="1980.7086"
y="368.50385"
x="2055.1182"
y="357.87393"
id="text3478"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
id="tspan3480"
x="1980.7086"
y="368.50385">a little shorter.. is a hack</tspan></text>
x="2055.1182"
y="357.87393">a little shorter.. is a hack</tspan></text>
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2207.4803,279.92122 17.7166,0"
@@ -3678,12 +3700,33 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5909"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,712.20465 63.77952,0"
id="path4948"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,485.433 63.77952,0"
id="path4950"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1700.7874,336.6141 0,67.32283"
id="path4962"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1927.5591,340.1574 0,60.23622"
id="path4966"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
</g>
<g
inkscape:groupmode="layer"
id="layer8"
inkscape:label="floor_3"
style="display:inline"
style="display:none"
transform="translate(0,4.2364502e-5)">
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -4458,6 +4501,17 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5913"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 446.45669,712.20465 60.23622,0"
id="path4956"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1573.2283,336.6141 0,67.32283"
id="path4964"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
@@ -4481,10 +4535,10 @@
inkscape:groupmode="layer"
id="layer11"
inkscape:label="staircase_1_2"
style="display:none">
style="display:inline">
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 2125.9843,343.70075 0,53.14961 -226.7717,0"
d="m 2125.9843,343.70075 0,53.14961 -212.5985,0"
id="path4998"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 224 KiB

After

Width:  |  Height:  |  Size: 226 KiB

83
code/plan_new.svg
View File

@@ -27,7 +27,7 @@
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<dc:title />
</cc:Work>
</rdf:RDF>
</metadata>
@@ -42,17 +42,17 @@
guidetolerance="10"
inkscape:pageopacity="0"
inkscape:pageshadow="2"
inkscape:window-width="3200"
inkscape:window-height="1671"
inkscape:window-width="1600"
inkscape:window-height="845"
id="namedview3234"
showgrid="true"
inkscape:zoom="0.6006794"
inkscape:cx="1327.5074"
inkscape:cy="994.75114"
inkscape:zoom="3.3979558"
inkscape:cx="1802.381"
inkscape:cy="1512.0287"
inkscape:window-x="0"
inkscape:window-y="55"
inkscape:window-y="0"
inkscape:window-maximized="1"
inkscape:current-layer="layer8"
inkscape:current-layer="layer9"
inkscape:object-nodes="true"
units="px"
showborder="true"
@@ -2081,7 +2081,7 @@
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 453.54331,485.43304 0,251.5748"
d="m 453.54331,460.62989 0,276.37795"
id="path4616"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
@@ -2342,6 +2342,16 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5901"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2657.4803,627.16532 116.9291,0"
id="path5824"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 453.54331,485.43304 53.1496,0"
id="path5826"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
@@ -3009,12 +3019,27 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5905"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2097.6378,400.39367 0,-60.23623"
id="path5822"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,485.43304 63.77952,0"
id="path5828"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,712.20469 63.77952,0"
id="path5836"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer9"
inkscape:label="floor_2"
style="display:none"
style="display:inline"
transform="translate(0,4.2364502e-5)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -3226,7 +3251,7 @@
sodipodi:nodetypes="cc" />
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1984.252,340.1574 155.9055,4e-5 0,60.23623 -155.9055,-5e-5"
d="m 1927.5591,340.1574 212.5984,4e-5 0,60.23623 -212.5984,-5e-5"
id="path11661"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccc" />
@@ -3726,12 +3751,34 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5909"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1700.7874,407.48024 0,-74.40945"
id="path5818"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1955.9055,403.93693 0,-67.32283"
id="path5820"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,485.433 63.77952,0"
id="path5830"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,712.20465 63.77952,0"
id="path5834"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer8"
inkscape:label="floor_3"
style="display:inline"
style="display:none"
transform="translate(0,4.2364502e-5)">
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -4506,6 +4553,16 @@
d="m 595.27559,1491.7322 60.23622,0"
id="path5913"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 1559.0551,403.93693 0,-67.32283"
id="path5816"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 446.45669,712.20465 60.23622,0"
id="path5832"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
@@ -4532,7 +4589,7 @@
style="display:none">
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 2125.9843,343.70075 0,53.14961 -226.7717,0"
d="m 2125.9843,343.70075 0,53.14961 -212.5985,0"
id="path4998"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 226 KiB

After

Width:  |  Height:  |  Size: 229 KiB

355
code/plan_plots.svg Normal file
View File

@@ -0,0 +1,355 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="724"
height="1800"
viewBox="0 0 724.00001 1800"
id="svg5100"
version="1.1"
inkscape:version="0.91 r13725"
sodipodi:docname="plan_plots.svg">
<defs
id="defs5102" />
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.98994949"
inkscape:cx="451.23575"
inkscape:cy="136.10259"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="true"
fit-margin-top="0"
fit-margin-left="0"
fit-margin-right="0"
fit-margin-bottom="0"
inkscape:object-nodes="true"
inkscape:window-width="1600"
inkscape:window-height="845"
inkscape:window-x="0"
inkscape:window-y="0"
inkscape:window-maximized="1"
units="px">
<inkscape:grid
type="xygrid"
id="grid5900"
originx="0"
originy="6.8902557e-05"
empspacing="4" />
</sodipodi:namedview>
<metadata
id="metadata5105">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="test1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(814.21766,568.50997)">
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,-557.59658 0.10236,1785.08658 318.89764,0 0,-162.9922"
id="path11563"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,1021.9782 0,-28.34651 -318.89764,0"
id="path11567"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,993.63169 0,-38.9764"
id="path11569"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,919.22219 0,-123.81877"
id="path11571"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,320.40335 0,-464.17327"
id="path11577"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,-179.203 0,-109.84247"
id="path11581"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,320.40342 -246,0"
id="path11587"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -711.32002,320.40342 -98.89764,-7e-5"
id="path11589"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,-288.59658 -319,0"
id="path11593"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -475.32002,-345.59665 -235.68504,-0.14174 0,-131.10236"
id="path11597"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -475.32002,-551.59665 0,206"
id="path11605"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -711.00506,-515.81713 0,-42.51968"
id="path11607"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,1227.49 287.00787,0 -0.008,-1117.08658"
id="path11619"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -425.32002,795.40342 150,0"
id="path11623"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,864.40342 209,0"
id="path11627"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.36727,1032.6081 209.05512,0"
id="path11631"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,864.40342 -0.0472,302.85038"
id="path11633"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,795.40342 0,-207"
id="path11637"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -420.45388,-345.73843 53.14961,0"
id="path11669"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -321.24128,-345.73843 38.97638,0"
id="path11671"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -409.82396,-427.23449 0,81.49606"
id="path11673"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -409.82396,-427.23449 173.62205,0"
id="path11675"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -321.24128,-345.73843 0,-81.49606"
id="path11677"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -236.20191,-558.33685 0,212.59842 139.88189,0.14185"
id="path11679"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,-223.59658 0,544"
id="path11757"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -123.32002,-223.59658 27,0"
id="path11761"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -310.61136,-491.01402 -102.75591,0"
id="path11822"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -563.32002,696.40342 150,0"
id="path4208"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -392.10742,-427.23449 0,-63.77953"
id="path5833"
inkscape:connector-curvature="0" />
<path
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -392.10742,-491.01402 0,-67.32283"
id="path5847"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,-557.59658 715,0"
id="path5902"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -355.32002,110.40342 260,0"
id="path5923"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -96.32002,-557.59658 0,668"
id="path5925"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -491.32002,660.40339 0,-139.99997"
id="path5929"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -810.32002,384.40342 183,0"
id="path5931"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -627.32002,384.40342 60,24 52,52"
id="path5933"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -515.32002,460.40342 24,60"
id="path5935"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,-223.59658 246,0"
id="path5937"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,795.40342 -78,0"
id="path5939"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,600.40339 134,3e-5"
id="path5941"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -563.32002,696.40342 72,98.99997"
id="path5945"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,528.40342 134,0"
id="path5947"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,320.40342 22,0"
id="path5951"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -359.32002,320.40342 155,0"
id="path5953"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,540.40342 0,-156"
id="path5955"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -239.32002,600.40339 35,3e-5"
id="path5959"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -239.32002,528.40339 35,3e-5"
id="path5961"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -413.32002,110.40342 22,0"
id="path5965"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m -235.32002,795.40342 31,-3e-5"
id="path5967"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 15 KiB

2
code/toni/BarometerEvaluation.h
View File

@@ -16,7 +16,7 @@ public:
//return 1;
// //rho_z
double barometerSigma = 0.09;
double barometerSigma = 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};

2
code/toni/BarometerSensorReader.h
View File

@@ -45,7 +45,7 @@ public:
obs->hpa = hPa - first_hPa;
std::cout << obs->hpa << std::endl;
//std::cout << obs->hpa << std::endl;
// done
return obs;