From 07d739ebb7a7f018e675b99de1762fcf1f098cd5 Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 27 Jan 2016 21:39:21 +0100 Subject: [PATCH 1/5] changed the floorplan (adjust stairs) added the new stairs to the code changed the visualisation --- code/CMakeLists.txt | 3 +- code/Vis.h | 39 ++++- code/main.cpp | 115 +++++++++++++- code/plan.svg | 377 +++++++++++++++++++++++--------------------- 4 files changed, 337 insertions(+), 197 deletions(-) diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index a138ae4..c5c9690 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,8 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O0 - + -O2 -DWITH_TESTS -DWITH_ASSERTIONS diff --git a/code/Vis.h b/code/Vis.h index bbe6ea7..8b185dd 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -16,8 +17,9 @@ public: K::Gnuplot gp; K::GnuplotSplot splot; K::GnuplotSplotElementLines floors; - K::GnuplotSplotElementPoints gridNodes; + K::GnuplotSplotElementColorPoints gridNodes; K::GnuplotSplotElementLines gridEdges; + K::GnuplotSplotElementPoints particles; public: @@ -26,11 +28,13 @@ public: gp << "set hidden3d front\n"; gp << "set view equal xy\n"; gp << "set ticslevel 0\n"; + gp << "set cbrange[0.8:2.0]\n"; // attach all layers splot.add(&floors); splot.add(&gridNodes); splot.add(&gridEdges); + splot.add(&particles); } @@ -50,13 +54,36 @@ public: /** add the grid to the plot */ template Vis& addGrid(Grid& grid) { + + float max = 0; + for (const T& n1 : grid) { + if (n1.distToTarget > max) {max = n1.distToTarget;} + } + gp << "set cbrange[0.0:1.0]\n"; + //gp << "set cbrange[0.8:1.3]\n"; + for (const T& n1 : grid) { const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); - gridNodes.add(p1); - for (const T& n2 : grid.neighbors(n1)) { - const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); - gridEdges.addSegment(p1, p2); - } + //const float color = n1.imp; + //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); +// } + } + return *this; + } + + void removeGrid() { + gridNodes.clear();; + } + + template Vis& showStates(std::vector>& states) { + particles.clear();; + for (const GridWalkState& n : states) { + particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm)); } return *this; } diff --git a/code/main.cpp b/code/main.cpp index 76cd605..6b10fdd 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,14 +1,21 @@ #include #include +#include +#include +#include +#include +#include #include "Vis.h" namespace Settings { const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg"; - const int gridSize_cm = 200; + const int gridSize_cm = 20; } struct MyNode : public GridNode, public GridPoint { + float distToTarget = 1.0; + float imp = 1.0; public: MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;} }; @@ -18,6 +25,20 @@ int align(const int val) { return val / Settings::gridSize_cm * Settings::gridSize_cm; } +// dijkstra mapper +class DijkstraMapper { + Grid& grid; +public: + DijkstraMapper(Grid& grid) : grid(grid) {;} + int getNumNeighbors(const MyNode& node) const {return node.getNumNeighbors();} + const MyNode* getNeighbor(const MyNode& node, const int idx) const {return &grid.getNeighbor(node, idx);} + float getWeightBetween(const MyNode& n1, const MyNode& n2) const { + float d = ((Point3)n1 - (Point3)n2).length(2.0); + //if (d > 20) {d*= 1.30;} + return d / std::pow(n2.imp, 3); + } +}; + int main(void) { Grid grid(Settings::gridSize_cm); @@ -31,9 +52,9 @@ int main(void) { Floor f2 = fpFac.getFloor("floor_2"); Floor f3 = fpFac.getFloor("floor_3"); - Stairs f01 = fpFac.getStairs("staircase_0_1"); - Stairs f12 = fpFac.getStairs("staircase_1_2"); - Stairs f23 = fpFac.getStairs("staircase_2_3"); + Stairs s01 = fpFac.getStairs("staircase_0_1"); + Stairs s12 = fpFac.getStairs("staircase_1_2"); + Stairs s23 = fpFac.getStairs("staircase_2_3"); const LengthF h0 = LengthF::cm(align(0)); const LengthF h1 = LengthF::cm(align(360)); @@ -44,12 +65,90 @@ int main(void) { gridFac.addFloor(f1, h1.cm()); gridFac.addFloor(f2, h2.cm()); gridFac.addFloor(f3, h3.cm()); - //gridFac.removeIsolated(); + + gridFac.addStairs(s01, h0.cm(), h1.cm()); + gridFac.addStairs(s12, h1.cm(), h2.cm()); + gridFac.addStairs(s23, h2.cm(), 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)); + gridFac.buildPlatformStair(psUpperLeft, h0.cm(), h1.cm()); + gridFac.buildPlatformStair(psUpperLeft, h1.cm(), h2.cm()); + gridFac.buildPlatformStair(psUpperLeft, h2.cm(), h3.cm()); +// 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)); + psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0)); + psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0)); + gridFac.buildPlatformStair(psUpperRight, h0.cm(), h1.cm()); + gridFac.buildPlatformStair(psUpperRight, h1.cm(), h2.cm()); + gridFac.buildPlatformStair(psUpperRight, h2.cm(), h3.cm()); +// vis.gp << "set xrange [6100:6900]\n"; +// vis.gp << "set yrange [4500:5200]\n"; + + PlatformStair psLowerLeft; + psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900)); + psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280)); + psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280)); + gridFac.buildPlatformStair(psLowerLeft, h0.cm(), h1.cm()); + gridFac.buildPlatformStair(psLowerLeft, h1.cm(), h2.cm()); + gridFac.buildPlatformStair(psLowerLeft, h2.cm(), h3.cm()); +// vis.gp << "set xrange [1300:2100]\n"; +// vis.gp << "set yrange [400:1400]\n"; + + gridFac.removeIsolated( (MyNode&)grid.getNodeFor(GridPoint(300,300,h0.cm())) ); + + GridImportance gridImp; + gridImp.addImportance(grid, h0.cm()); + gridImp.addImportance(grid, h1.cm()); + gridImp.addImportance(grid, h2.cm()); + gridImp.addImportance(grid, h3.cm()); + + MyNode& start = (MyNode&)grid.getNodeFor(GridPoint(500,300,h0.cm())); + MyNode& end = (MyNode&)grid.getNodeFor(GridPoint(7000,5000,h3.cm())); Vis vis; - vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3); - vis.addGrid(grid); - vis.show(); + vis.addFloor(f0, h0); + vis.addFloor(f1, h1); + vis.addFloor(f2, h2); + vis.addFloor(f3, h3); + + + + + //GridWalkRandomHeadingUpdate walk; + //GridWalkRandomHeadingUpdateAdv walk; + //GridWalkPushForward walk; + GridWalkLightAtTheEndOfTheTunnel walk(grid, DijkstraMapper(grid), end); + + + std::vector> states; + for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState(&start, Heading::rnd())); } + +// vis.addGrid(grid); +// vis.show(); +// sleep(100); +// vis.removeGrid(); + + Distribution::Normal wDist(0.3, 0.3); + //std::minstd_rand gen(1234); + //std::normal_distribution dist(0.6, 0.3); + + while(true) { + for (GridWalkState& state : states) { + state = walk.getDestination(grid, state, std::abs(wDist.draw()) ); + } + usleep(1000*80); + vis.showStates(states); + vis.show(); + } + + sleep(1000); diff --git a/code/plan.svg b/code/plan.svg index 71adfcd..8ec6368 100755 --- a/code/plan.svg +++ b/code/plan.svg @@ -42,23 +42,23 @@ guidetolerance="10" inkscape:pageopacity="0" inkscape:pageshadow="2" - inkscape:window-width="1366" - inkscape:window-height="672" + inkscape:window-width="1600" + inkscape:window-height="782" id="namedview3234" showgrid="true" inkscape:zoom="2.4027176" - inkscape:cx="422.22336" - inkscape:cy="1231.7432" + inkscape:cx="555.925" + inkscape:cy="388.39069" inkscape:window-x="0" - inkscape:window-y="33" + inkscape:window-y="0" inkscape:window-maximized="1" - inkscape:current-layer="layer11" + inkscape:current-layer="layer16" inkscape:object-nodes="true" units="px" showborder="true" inkscape:snap-to-guides="true" inkscape:snap-text-baseline="true" - showguides="true" + showguides="false" inkscape:guide-bbox="true" inkscape:snap-nodes="true" inkscape:snap-smooth-nodes="true"> @@ -238,7 +238,7 @@ inkscape:groupmode="layer" id="layer2" inkscape:label="roomnames_2" - style="display:inline"> + style="display:none"> + style="display:inline"> - + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> - + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + + + + + + + + + @@ -2867,21 +2909,6 @@ d="m 2462.5984,460.62989 0,276.37795" id="path12918" inkscape:connector-curvature="0" /> - - - + + + + + + @@ -3055,7 +3112,7 @@ inkscape:connector-curvature="0" /> @@ -3545,24 +3602,6 @@ inkscape:connector-curvature="0" transform="translate(0,-4.2364502e-5)" sodipodi:nodetypes="cc" /> - - - + + + + + + @@ -4352,30 +4421,42 @@ inkscape:connector-curvature="0" transform="translate(0,-4.2364502e-5)" sodipodi:nodetypes="cc" /> - - - + + + + + + - + sodipodi:nodetypes="ccc" /> - - + 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 503.14961,457.08658 -46.063,0 0,276.37795" + id="path4992" + inkscape:connector-curvature="0" + sodipodi:nodetypes="ccc" /> + style="display:none"> + sodipodi:nodetypes="ccc" /> - - - - + 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 503.14961,460.62989 -56.69292,0 0,276.37795" + id="path5000" + inkscape:connector-curvature="0" /> - - - - - + Date: Thu, 28 Jan 2016 21:49:36 +0100 Subject: [PATCH 2/5] began putting everything together --- code/CMakeLists.txt | 2 +- code/DijkstraMapper.h | 29 +++ code/Helper.h | 147 ++++++++++++++++ code/MyGridNode.h | 26 +++ code/Settings.h | 26 +++ code/Vis.h | 15 +- code/eval/Eval.h | 66 +++++++ code/eval/EvalBase.h | 262 ++++++++++++++++++++++++++++ code/eval/GroundTruthWay.h | 24 +++ code/frank/BeaconEvaluation.h | 6 +- code/frank/BeaconSensorReader.h | 2 +- code/frank/PositionedBeacon.h | 19 +- code/frank/PositionedWiFiAP.h | 16 +- code/frank/Settings.h | 66 +++---- code/frank/WiFiEvaluation.h | 23 +-- code/frank/WiFiSensorReader.h | 2 +- code/lukas/StepEvaluation.h | 46 ++--- code/lukas/TurnEvaluation.h | 64 +++---- code/lukas/TurnReader.h | 2 +- code/main.cpp | 164 +++++------------ code/particles/MyControl.h | 8 + code/particles/MyEvaluation.h | 85 +++++++++ code/particles/MyInitializer.h | 59 +++++++ code/particles/MyObservation.h | 49 ++++++ code/particles/MyState.h | 136 +++++++++++++++ code/particles/MyTransition.h | 189 ++++++++++++++++++++ code/particles/MyTransitionSimple.h | 83 +++++++++ code/particles/P3.h | 31 ++++ code/toni/BarometerEvaluation.h | 76 ++++---- code/toni/BarometerSensorReader.h | 2 +- 30 files changed, 1446 insertions(+), 279 deletions(-) create mode 100644 code/DijkstraMapper.h create mode 100644 code/Helper.h create mode 100644 code/MyGridNode.h create mode 100644 code/Settings.h create mode 100644 code/eval/Eval.h create mode 100644 code/eval/EvalBase.h create mode 100644 code/eval/GroundTruthWay.h create mode 100755 code/particles/MyControl.h create mode 100755 code/particles/MyEvaluation.h create mode 100755 code/particles/MyInitializer.h create mode 100755 code/particles/MyObservation.h create mode 100755 code/particles/MyState.h create mode 100755 code/particles/MyTransition.h create mode 100755 code/particles/MyTransitionSimple.h create mode 100755 code/particles/P3.h diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index c5c9690..9914982 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,7 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O2 + -O0 -DWITH_TESTS -DWITH_ASSERTIONS diff --git a/code/DijkstraMapper.h b/code/DijkstraMapper.h new file mode 100644 index 0000000..b4402a9 --- /dev/null +++ b/code/DijkstraMapper.h @@ -0,0 +1,29 @@ +#ifndef DIJKSTRAMAPPER_H +#define DIJKSTRAMAPPER_H + +#include "MyGridNode.h" + +/** + * allows dijkstra calculation on top of our data-structure + */ +class DijkstraMapper { + + Grid& grid; + +public: + + DijkstraMapper(Grid& 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 { + float d = ((Point3)n1 - (Point3)n2).length(2.0); + //if (d > 20) {d*= 1.30;} + return d / std::pow(n2.imp, 3); + } + +}; + +#endif // DIJKSTRAMAPPER_H diff --git a/code/Helper.h b/code/Helper.h new file mode 100644 index 0000000..dcf5fee --- /dev/null +++ b/code/Helper.h @@ -0,0 +1,147 @@ +#ifndef HELPER_H +#define HELPER_H + +#include +#include +#include + +#include + +#include "Settings.h" +#include "MyGridNode.h" + +class Helper { + +private: + + + +public: + + /** convert height (in cm) to floor-numbers */ + static int getFloorNr(float z_cm) { +// if (z_cm < 360) {return 0;} +// 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;} + return 3; + } + + /** convert height (in cm) to floor-numbers */ + static int getFloorNrFloat(float z_cm) { + return z_cm / 340.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; + default: throw "error"; + } + } + + /** align the given value onto the grid */ + static int align(const int val) { + return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm; + } + + /** all floors within the building */ + struct FHWSFloors { + + Floor f0, f1, f2, f3; + + Stairs s01, s12, s23; + + const LengthF h0 = LengthF::cm(align(getHeight(0))); + const LengthF h1 = LengthF::cm(align(getHeight(1))); + const LengthF h2 = LengthF::cm(align(getHeight(2))); + const LengthF h3 = LengthF::cm(align(getHeight(3))); + + FHWSFloors() {;} + + }; + + /** load the entire floorplan */ + static FHWSFloors getFloors() { + + FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222); + FHWSFloors f; + + f.f0 = fpFac.getFloor("floor_0"); + f.f1 = fpFac.getFloor("floor_1"); + f.f2 = fpFac.getFloor("floor_2"); + f.f3 = fpFac.getFloor("floor_3"); + + f.s01 = fpFac.getStairs("staircase_0_1"); + f.s12 = fpFac.getStairs("staircase_1_2"); + f.s23 = fpFac.getStairs("staircase_2_3"); + + return f; + + } + + template static void buildTheGrid(Grid& grid, FHWSFloors floors) { + + GridFactory gridFac(grid); + + gridFac.addFloor(floors.f0, floors.h0.cm()); + gridFac.addFloor(floors.f1, floors.h1.cm()); + gridFac.addFloor(floors.f2, floors.h2.cm()); + gridFac.addFloor(floors.f3, floors.h3.cm()); + + gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm()); + 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)); + 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"; + + PlatformStair psUpperRight; + psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098)); + psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0)); + psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0)); + gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm()); + gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm()); + gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm()); + // vis.gp << "set xrange [6100:6900]\n"; + // vis.gp << "set yrange [4500:5200]\n"; + + PlatformStair psLowerLeft; + psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900)); + psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280)); + psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280)); + gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm()); + gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm()); + gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm()); + // vis.gp << "set xrange [1300:2100]\n"; + // vis.gp << "set yrange [400:1400]\n"; + + // remove all isolated nodes not attached to 300,300,floor0 + gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) ); + + // stamp importance information onto the grid-nodes + GridImportance gridImp; + gridImp.addImportance(grid, floors.h0.cm()); + gridImp.addImportance(grid, floors.h1.cm()); + gridImp.addImportance(grid, floors.h2.cm()); + gridImp.addImportance(grid, floors.h3.cm()); + + + } + +}; + +#endif // HELPER_H diff --git a/code/MyGridNode.h b/code/MyGridNode.h new file mode 100644 index 0000000..7df8127 --- /dev/null +++ b/code/MyGridNode.h @@ -0,0 +1,26 @@ +#ifndef MYGRIDNODE_H +#define MYGRIDNODE_H + +#include +#include + +/** + * the nodes we add to our grid + */ +struct MyGridNode : public GridNode, public GridPoint { + + /** distance to the desired target */ + float distToTarget = 1.0; + + /** node importance based on surroundings */ + float imp = 1.0; + +public: + + /** needed ctor */ + MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;} + +}; + + +#endif // MYGRIDNODE_H diff --git a/code/Settings.h b/code/Settings.h new file mode 100644 index 0000000..1722a92 --- /dev/null +++ b/code/Settings.h @@ -0,0 +1,26 @@ +#ifndef OTHER_SETTINGS_H +#define OTHER_SETTINGS_H + +#define USE_STATIC_CIRCULAR_BUFFERING false +#define USE_BAROMETER_SMOOTHING_RC_LOWPASS false +#define USE_BAROMETER_SMOOTHING_HEAD_TAIL false +#define USE_BAROMETRIC_FORMULAR false + +#include + +namespace MiscSettings { + + const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg"; + + const int gridSize_cm = 40; + + const int timeSteps = 500; + + const int numParticles = 1000; + +} + + +#endif // OTHER_SETTINGS_H + + diff --git a/code/Vis.h b/code/Vis.h index 8b185dd..d90fbeb 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -26,10 +26,15 @@ public: Vis() { gp << "set hidden3d front\n"; - gp << "set view equal xy\n"; + //gp << "set view equal xy\n"; gp << "set ticslevel 0\n"; gp << "set cbrange[0.8:2.0]\n"; + gp << "unset xtics\n"; + gp << "unset ytics\n"; + gp << "unset ztics\n"; + gp << "unset border\n"; + // attach all layers splot.add(&floors); splot.add(&gridNodes); @@ -80,6 +85,14 @@ public: gridNodes.clear();; } + void clearStates() { + particles.clear(); + } + + template void addState(const GridWalkState& n) { + particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm)); + } + template Vis& showStates(std::vector>& states) { particles.clear();; for (const GridWalkState& n : states) { diff --git a/code/eval/Eval.h b/code/eval/Eval.h new file mode 100644 index 0000000..45b8b8d --- /dev/null +++ b/code/eval/Eval.h @@ -0,0 +1,66 @@ +#ifndef EVAL_H +#define EVAL_H + +#include "EvalBase.h" +#include "../DijkstraMapper.h" +#include +#include +#include +#include +#include +#include + +class Eval : public EvalBase { + +public: + + + + Eval() { + + + pf = new K::ParticleFilter( MiscSettings::numParticles, std::unique_ptr(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())); + + //GridWalkLightAtTheEndOfTheTunnel* walk = new GridWalkLightAtTheEndOfTheTunnel(grid, DijkstraMapper(grid), end); + //GridWalkRandomHeadingUpdate* walk = new GridWalkRandomHeadingUpdate(); + //GridWalkRandomHeadingUpdateAdv* walk = new GridWalkRandomHeadingUpdateAdv(); + GridWalkPushForward* walk = new GridWalkPushForward(); + + pf->setTransition( std::unique_ptr( 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"); + //gtw = getGroundTruthWay(*sr, gtwp, way2); + + } + + + + void setEval1() { + + + runName = "TODO"; + + // the particle filter's evaluation method + std::unique_ptr eval = std::unique_ptr( new MyEvaluation() ); + eval.get()->setUsage(true, false, false, false, false); + pf->setEvaluation( std::move(eval) ); + + // resampling step? + pf->setNEffThreshold(1.0); + pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + + // state estimation step + pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + + } + + + +}; + +#endif // EVAL_H diff --git a/code/eval/EvalBase.h b/code/eval/EvalBase.h new file mode 100644 index 0000000..41b0714 --- /dev/null +++ b/code/eval/EvalBase.h @@ -0,0 +1,262 @@ +#ifndef EVALBASE_H +#define EVALBASE_H + +#include "../Settings.h" +#include "../Helper.h" +#include "../Vis.h" + +#include +#include + +#include "GroundTruthWay.h" + +#include "../particles/P3.h" +#include "../particles/MyState.h" +#include "../particles/MyObservation.h" +#include "../particles/MyEvaluation.h" +#include "../particles/MyTransition.h" +#include "../particles/MyInitializer.h" + +#include "../reader/SensorReader.h" +#include "../reader/SensorReaderStep.h" +#include "../reader/SensorReaderTurn.h" + +#include "../lukas/TurnObservation.h" +#include "../lukas/StepObservation.h" + +#include "../toni/BarometerSensorReader.h" + +#include "../frank/WiFiSensorReader.h" +#include "../frank/BeaconSensorReader.h" + +class EvalBase { + +protected: + + Grid grid; + Helper::FHWSFloors floors; + Vis vis; + + K::ParticleFilter* pf; + + SensorReader* sr; + SensorReaderTurn* srt; + SensorReaderStep* srs; + + GroundTruthWay gtw; + + std::string runName; + +public: + + EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) { + + // build the grid + Helper::buildTheGrid(grid, floors); + + // setup the visualisation + vis.addFloor(floors.f0, floors.h0); + vis.addFloor(floors.f1, floors.h1); + vis.addFloor(floors.f2, floors.h2); + vis.addFloor(floors.f3, floors.h3); + + } + + void run() { + + // read CSV input + // const int s_wifi = 0; + //SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv"); + const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; + const int s_linearAcceleration = 2; + + std::list turn_observations; + std::list step_observations; + + //Create an BarometerSensorReader + BarometerSensorReader baroSensorReader; + + + //Read all turn Observations + while(srt->hasNext()) { + + SensorEntryTurn set = srt->getNext(); + TurnObservation to; + + to.ts = set.ts; + to.delta_heading = set.delta_heading; + to.delta_motion = set.delta_motion; + + turn_observations.push_back(to); + } + + + //Step Observations + while(srs->hasNext()) { + + SensorEntryStep ses = srs->getNext(); + StepObservation so; + + so.ts = ses.ts; + + step_observations.push_back(so); + } + + + + // the to-be-evaluated observation + MyObservation obs; + + + std::vector pathEst; + + uint64_t lastTransitionTS = 0; + bool firstReading = true; + int64_t start_time = -1; + + K::Statistics stats; + + // process each sensor reading + while(sr->hasNext()) { + + // get the next sensor reading from the CSV + const SensorEntry se = sr->getNext(); + + //start_time needed for time calculation of steps and turns + obs.latestSensorDataTS = se.ts; + if (start_time == -1) {start_time = se.ts;} + int64_t current_time = se.ts - start_time; + + // ensure the graph timestamp starts with the first reading + if (firstReading) { + //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); + firstReading = false; + } + + switch(se.idx) { + + case s_wifi: { + obs.wifi = WiFiSensorReader::readWifi(se); + break; + } + + case s_beacons: { + BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se); + if (!boe.mac.empty()) { + obs.beacons.entries.push_back(boe); + } // add the observed beacon + obs.beacons.removeOld(obs.latestSensorDataTS); + break; + } + + case s_barometer: { + obs.barometer = baroSensorReader.readBarometer(se); + break; + } + + case s_linearAcceleration:{ + baroSensorReader.readVerticalAcceleration(se); + break; + } + + } + + + // scheduled transition every 500 ms + if (lastTransitionTS == 0) {lastTransitionTS = se.ts;} + for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) { + + //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; + } + + + // let the transition know the current timestamp to determine the time since the last transition + //if (!useSimpleTrans) { + ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); + //} else { + // ((MyTransitionSimple*)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 Point3 curEst = est.pCur; + pathEst.push_back(curEst); + + // debug print current particle set. + //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); + + // error calculation. compare ground-truth to estimation +// const Point3 curGT = gtw.getPosAtTime(se.ts - 750); + +// // TODO +// const Point3 diff = curEst - curGT; + +// //if (std::abs(diff.z) < 0.1) { +// const float err = diff.length(); +// std::cout << err << std::endl; +// stats.add(err); +// std::cout << stats.asString() << std::endl; +// //} + + vis.clearStates(); + for (const K::Particle p : pf->getParticles()) {vis.addState(p.state.walkState);} + + vis.show();; + + } + + + } + + { +// 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); + + } + + + +}; + +#endif // EVALBASE_H diff --git a/code/eval/GroundTruthWay.h b/code/eval/GroundTruthWay.h new file mode 100644 index 0000000..b7631dc --- /dev/null +++ b/code/eval/GroundTruthWay.h @@ -0,0 +1,24 @@ +#ifndef GROUNDTRUTHWAY_H +#define GROUNDTRUTHWAY_H + +#include +#include + +/** + * interpolated ground-trouth based on timed check-points + */ +class GroundTruthWay : public Interpolator { + +public: + + Point3 getPosAtTime(const uint64_t ts) const { + return get(ts); + } + + /** get the ground truth way */ + const std::vector& getWay() const {return entries;} + +}; + + +#endif // GROUNDTRUTHWAY_H diff --git a/code/frank/BeaconEvaluation.h b/code/frank/BeaconEvaluation.h index 996461b..3ac5eda 100755 --- a/code/frank/BeaconEvaluation.h +++ b/code/frank/BeaconEvaluation.h @@ -39,10 +39,12 @@ public: if (!beacon) {continue;} // distance (in meter) between particle and AP - const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0; + //const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0; + const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0; // floor difference? - const double floorDist = std::abs(beacon->zNr - state.z_nr); + //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))); // estimate the rssi depending on above distance const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf); diff --git a/code/frank/BeaconSensorReader.h b/code/frank/BeaconSensorReader.h index d921c0d..cfd2d9a 100755 --- a/code/frank/BeaconSensorReader.h +++ b/code/frank/BeaconSensorReader.h @@ -1,7 +1,7 @@ #ifndef BEACONSENSORREADER_H #define BEACONSENSORREADER_H -#include "../SensorReader.h" +#include "../reader/SensorReader.h" #include "BeaconObservation.h" #include "Settings.h" #include diff --git a/code/frank/PositionedBeacon.h b/code/frank/PositionedBeacon.h index 6a99df1..534df8c 100755 --- a/code/frank/PositionedBeacon.h +++ b/code/frank/PositionedBeacon.h @@ -2,19 +2,26 @@ #define POSITIONEDBEACON_H #include "WiFiAP.h" -#include "Position3D.h" +//#include "Position3D.h" +#include -class PositionedBeacon : public Position3D { +class PositionedBeacon : public Point3 { public: MACAddress mac; - double tx; - double pl; + float tx; + float pl; + +// /** ctor */ +// PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) : +// mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) { +// ; +// } /** ctor */ - PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) : - mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) { + PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) : + mac(mac), tx(tx), pl(pl), Point3(x_cm, y_cm, z_cm) { ; } diff --git a/code/frank/PositionedWiFiAP.h b/code/frank/PositionedWiFiAP.h index 296503b..3dc93be 100755 --- a/code/frank/PositionedWiFiAP.h +++ b/code/frank/PositionedWiFiAP.h @@ -2,16 +2,22 @@ #define POSITIONEDWIFIAP_H #include "WiFiAP.h" -#include "Position3D.h" +//#include "Position3D.h" +#include - -class PositionedWifiAP : public WiFiAP, public Position3D { +class PositionedWifiAP : public WiFiAP, public Point3 { public: +// /** ctor */ +// PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) : +// WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) { +// ; +// } + /** ctor */ - PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) : - WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) { + PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) : + WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) { ; } diff --git a/code/frank/Settings.h b/code/frank/Settings.h index 74e07f3..cc82dbb 100755 --- a/code/frank/Settings.h +++ b/code/frank/Settings.h @@ -7,6 +7,8 @@ #include +#include "../Helper.h" + class Settings { private: @@ -21,49 +23,49 @@ public: const double pl = 2.7; const double tx = -46; - addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, 3, tx, pl-0.5); - addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, 3, tx, pl-0.5); - addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5); - addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl); - addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, 3, tx, pl); + addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl-0.5); + addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl-0.5); + addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.5); + 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, 2, tx, pl-0.5); - addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, 2, tx, pl); - addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, 2, tx, pl); - addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2, tx, pl-0.5); - addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 2, tx, pl-0.5); + addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl-0.5); + 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-0.5); + addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5); // 1st floor (vague AP position) - addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, 1, tx, pl); - addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1, tx, pl); - addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, 1, tx, pl-0.25); - addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1, tx, pl-0.25); - addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1, tx, pl); - addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl); + 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-0.25); + addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl-0.25); + 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, 0, tx, pl); - addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, 0, tx, pl); - addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, 0, tx, pl); - addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, 0, tx, pl); - addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, 0, tx, pl); - addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 0, tx, pl); // vague!! + 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, 3); // id:16 - addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2); // id:18 - addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1); // id:11 - addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0); // id:20 + 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, 2); // idis switchboard - addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2); // idis outside - addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2); // idis toni + 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, 1); // i.1.47 - addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1); // fachschaft - addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 1); // neben fachschaft + 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); diff --git a/code/frank/WiFiEvaluation.h b/code/frank/WiFiEvaluation.h index 85d715b..16ea667 100755 --- a/code/frank/WiFiEvaluation.h +++ b/code/frank/WiFiEvaluation.h @@ -37,14 +37,14 @@ public: //const double tx = -48; // tablet //const double pl = 3.15; - const double waf = 7;//10.0; - const double floor_height_cm = 350; + const float waf = 7;//10.0; // was 7 before?! has something todo with the floor heights / levels + // const int floor_height_cm = 350; // get the ap the client had the strongest measurement for const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); - const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; - const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); - const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); + //const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; + //const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); + //const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); // process each detected AP for (const WiFiObservationEntry& entry : obs.entries) { @@ -53,20 +53,23 @@ public: const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap); // distance (in meter) between particle and AP - const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0; + //const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0; + const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0; // floor difference? - const double floorDist = std::abs(ap->zNr - state.z_nr); + const float floorDiff = std::round( + std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)) + ); // estimate the rssi depending on above distance - const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDist * waf); + const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf); // the measured rssi const double realRSSI = entry.rssi; // the measured relative rssi - const double realRelRSSI = strongest.rssi - realRSSI; - const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI; + //const double realRelRSSI = strongest.rssi - realRSSI; + //const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI; // probability? (sigma grows with measurement's age) const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0; diff --git a/code/frank/WiFiSensorReader.h b/code/frank/WiFiSensorReader.h index aa3dc41..585b6b3 100755 --- a/code/frank/WiFiSensorReader.h +++ b/code/frank/WiFiSensorReader.h @@ -1,7 +1,7 @@ #ifndef WIFISENSORREADER_H #define WIFISENSORREADER_H -#include "../SensorReader.h" +#include "../reader/SensorReader.h" #include "WiFiObservation.h" #include diff --git a/code/lukas/StepEvaluation.h b/code/lukas/StepEvaluation.h index c7f4932..2ab54eb 100755 --- a/code/lukas/StepEvaluation.h +++ b/code/lukas/StepEvaluation.h @@ -8,10 +8,10 @@ #include "StepObservation.h" #include -static double mu_walk = 40; -static double sigma_walk = 15; -static double mu_stop = 0; -static double sigma_stop = 5; +static constexpr double mu_walk = 40; +static constexpr double sigma_walk = 15; +static constexpr double mu_stop = 0; +static constexpr double sigma_stop = 5; class StepEvaluation { @@ -19,29 +19,31 @@ public: double getProbability(const MyState& state, const StepObservation* obs) const { - double distance = state.distanceWalkedCM; + return 1; - double a = 1.0; - double mu_distance = 0; //cm - double sigma_distance = 10.0; //cm +// double distance = state.distanceWalkedCM; - if(obs->step) { - a = 1.0; - mu_distance = mu_walk;//80.0; //cm - sigma_distance = sigma_walk;//40.0; //cm - } +// double a = 1.0; +// double mu_distance = 0; //cm +// double sigma_distance = 10.0; //cm - else { - a = 0.0; - mu_distance = mu_stop; //cm - sigma_distance = sigma_stop; //cm - } +// if(obs->step) { +// a = 1.0; +// mu_distance = mu_walk;//80.0; //cm +// sigma_distance = sigma_walk;//40.0; //cm +// } - //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); +// else { +// a = 0.0; +// mu_distance = mu_stop; //cm +// sigma_distance = sigma_stop; //cm +// } - return p; +// //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; } }; diff --git a/code/lukas/TurnEvaluation.h b/code/lukas/TurnEvaluation.h index 26df7ee..a9e1ffb 100755 --- a/code/lukas/TurnEvaluation.h +++ b/code/lukas/TurnEvaluation.h @@ -4,10 +4,10 @@ #include "../particles/MyState.h" #include "TurnObservation.h" -#include +//#include #include -static double sigma_heading = 35; +static constexpr double sigma_heading = 35; class TurnEvaluation { @@ -17,55 +17,33 @@ public: double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const { - //Particle's heading change - double delta_heading_particle = state.heading - state.heading_old; + return 1; + +// //Particle's heading change +// double delta_heading_particle = state.heading - state.heading_old; - //Correct offset of the heading change - if (delta_heading_particle < -180) { - delta_heading_particle += 360; - } - else if (delta_heading_particle > 180) { - delta_heading_particle -= 360; - } +// //Correct offset of the heading change +// if (delta_heading_particle < -180) { +// delta_heading_particle += 360; +// } +// else if (delta_heading_particle > 180) { +// delta_heading_particle -= 360; +// } - //Switch between simple and improved evaluation - //"Simple" only evaluates the deviation between the measured heading and the particle heading change using - //normal distribution - if(simple) { +// //Switch between simple and improved evaluation +// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using +// //normal distribution +// //if(simple) { - double sigma_delta_heading = sigma_heading; +// double sigma_delta_heading = sigma_heading; - const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle); +// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle); - return p; - } - - //use the von Mises distribution - else { - //Here some calculations must be done in rad - - double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0; - double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0; - - //Equation for estimating kappa value of von Mises distribution - //empirically estimated - double kappa = 0.0; - - kappa = 5.0 / exp(2 * delta_motion_rad); - - double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0; - - - - //pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution) - const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa)); - - return p; - - } +// return p; +// // } } diff --git a/code/lukas/TurnReader.h b/code/lukas/TurnReader.h index 653a339..d130eba 100755 --- a/code/lukas/TurnReader.h +++ b/code/lukas/TurnReader.h @@ -1,7 +1,7 @@ #ifndef TURNREADER_H #define TURNREADER_H -#include "../SensorReaderTurn.h" +#include "../reader/SensorReaderTurn.h" #include "TurnObservation.h" class TurnReader { diff --git a/code/main.cpp b/code/main.cpp index 6b10fdd..32defe0 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,146 +1,62 @@ -#include -#include + #include #include #include #include -#include #include "Vis.h" +#include "Helper.h" +#include "MyGridNode.h" +#include "Helper.h" +#include "DijkstraMapper.h" -namespace Settings { - const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg"; - const int gridSize_cm = 20; -} +#include "eval/Eval.h" +#include "eval/EvalBase.h" -struct MyNode : public GridNode, public GridPoint { - float distToTarget = 1.0; - float imp = 1.0; -public: - MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;} -}; +Settings settings; +void testModelWalk() { -int align(const int val) { - return val / Settings::gridSize_cm * Settings::gridSize_cm; -} + Grid grid(MiscSettings::gridSize_cm); -// dijkstra mapper -class DijkstraMapper { - Grid& grid; -public: - DijkstraMapper(Grid& grid) : grid(grid) {;} - int getNumNeighbors(const MyNode& node) const {return node.getNumNeighbors();} - const MyNode* getNeighbor(const MyNode& node, const int idx) const {return &grid.getNeighbor(node, idx);} - float getWeightBetween(const MyNode& n1, const MyNode& n2) const { - float d = ((Point3)n1 - (Point3)n2).length(2.0); - //if (d > 20) {d*= 1.30;} - return d / std::pow(n2.imp, 3); - } -}; + Helper::FHWSFloors floors = Helper::getFloors(); + Helper::buildTheGrid(grid, floors); -int main(void) { - - Grid grid(Settings::gridSize_cm); - GridFactory gridFac(grid); - - - FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222); - - Floor f0 = fpFac.getFloor("floor_0"); - Floor f1 = fpFac.getFloor("floor_1"); - Floor f2 = fpFac.getFloor("floor_2"); - Floor f3 = fpFac.getFloor("floor_3"); - - Stairs s01 = fpFac.getStairs("staircase_0_1"); - Stairs s12 = fpFac.getStairs("staircase_1_2"); - Stairs s23 = fpFac.getStairs("staircase_2_3"); - - const LengthF h0 = LengthF::cm(align(0)); - const LengthF h1 = LengthF::cm(align(360)); - const LengthF h2 = LengthF::cm(align(360+340)); - const LengthF h3 = LengthF::cm(align(360+340+340)); - - gridFac.addFloor(f0, h0.cm()); - gridFac.addFloor(f1, h1.cm()); - gridFac.addFloor(f2, h2.cm()); - gridFac.addFloor(f3, h3.cm()); - - gridFac.addStairs(s01, h0.cm(), h1.cm()); - gridFac.addStairs(s12, h1.cm(), h2.cm()); - gridFac.addStairs(s23, h2.cm(), 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)); - gridFac.buildPlatformStair(psUpperLeft, h0.cm(), h1.cm()); - gridFac.buildPlatformStair(psUpperLeft, h1.cm(), h2.cm()); - gridFac.buildPlatformStair(psUpperLeft, h2.cm(), h3.cm()); -// 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)); - psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0)); - psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0)); - gridFac.buildPlatformStair(psUpperRight, h0.cm(), h1.cm()); - gridFac.buildPlatformStair(psUpperRight, h1.cm(), h2.cm()); - gridFac.buildPlatformStair(psUpperRight, h2.cm(), h3.cm()); -// vis.gp << "set xrange [6100:6900]\n"; -// vis.gp << "set yrange [4500:5200]\n"; - - PlatformStair psLowerLeft; - psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900)); - psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280)); - psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280)); - gridFac.buildPlatformStair(psLowerLeft, h0.cm(), h1.cm()); - gridFac.buildPlatformStair(psLowerLeft, h1.cm(), h2.cm()); - gridFac.buildPlatformStair(psLowerLeft, h2.cm(), h3.cm()); -// vis.gp << "set xrange [1300:2100]\n"; -// vis.gp << "set yrange [400:1400]\n"; - - gridFac.removeIsolated( (MyNode&)grid.getNodeFor(GridPoint(300,300,h0.cm())) ); - - GridImportance gridImp; - gridImp.addImportance(grid, h0.cm()); - gridImp.addImportance(grid, h1.cm()); - gridImp.addImportance(grid, h2.cm()); - gridImp.addImportance(grid, h3.cm()); - - MyNode& start = (MyNode&)grid.getNodeFor(GridPoint(500,300,h0.cm())); - MyNode& end = (MyNode&)grid.getNodeFor(GridPoint(7000,5000,h3.cm())); + MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm())); + MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm())); Vis vis; - vis.addFloor(f0, h0); - vis.addFloor(f1, h1); - vis.addFloor(f2, h2); - vis.addFloor(f3, h3); + vis.addFloor(floors.f0, floors.h0); + vis.addFloor(floors.f1, floors.h1); + vis.addFloor(floors.f2, floors.h2); + vis.addFloor(floors.f3, floors.h3); + +// vis.gp << "set xrange [1000:4000]\n"; +// vis.gp << "set yrange [1000:4000]\n"; +// vis.gp << "set zrange [0:600]\n"; + + // switch between different grid-walkers + GridWalkRandomHeadingUpdate walk; + //GridWalkRandomHeadingUpdateAdv walk; + //GridWalkPushForward walk; + //GridWalkLightAtTheEndOfTheTunnel walk(grid, DijkstraMapper(grid), end); + std::vector> states; + for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState(&start, Heading::rnd())); } +// track the number-of-visits for each node to draw something like a particle-heat-map? - //GridWalkRandomHeadingUpdate walk; - //GridWalkRandomHeadingUpdateAdv walk; - //GridWalkPushForward walk; - GridWalkLightAtTheEndOfTheTunnel walk(grid, DijkstraMapper(grid), end); - - - std::vector> states; - for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState(&start, Heading::rnd())); } - + // show the importance factors // vis.addGrid(grid); // vis.show(); // sleep(100); // vis.removeGrid(); Distribution::Normal wDist(0.3, 0.3); - //std::minstd_rand gen(1234); - //std::normal_distribution dist(0.6, 0.3); while(true) { - for (GridWalkState& state : states) { + for (GridWalkState& state : states) { state = walk.getDestination(grid, state, std::abs(wDist.draw()) ); } usleep(1000*80); @@ -152,6 +68,20 @@ int main(void) { sleep(1000); +} + + + +int main(void) { + + //testModelWalk(); + + Eval eval; + eval.setEval1(); + eval.run(); + return 0; } + + diff --git a/code/particles/MyControl.h b/code/particles/MyControl.h new file mode 100755 index 0000000..3d8fde6 --- /dev/null +++ b/code/particles/MyControl.h @@ -0,0 +1,8 @@ +#ifndef MYCONTROL_H +#define MYCONTROL_H + +struct MyControl { + +}; + +#endif // MYCONTROL_H diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h new file mode 100755 index 0000000..4c011d6 --- /dev/null +++ b/code/particles/MyEvaluation.h @@ -0,0 +1,85 @@ +#ifndef MYEVALUATION_H +#define MYEVALUATION_H + +#include +#include "MyObservation.h" +#include "MyState.h" + +#include "../frank/WiFiEvaluation.h" +#include "../frank/BeaconEvaluation.h" +#include "../toni/BarometerEvaluation.h" +#include "../lukas/StepEvaluation.h" +#include "../lukas/TurnEvaluation.h" + +class MyEvaluation : public K::ParticleFilterEvaluation { + +private: + + WiFiEvaluation wifiEval; + BeaconEvaluation beaconEval; + BarometerEvaluation barometerEval; + StepEvaluation stepEval; + TurnEvaluation turnEval; + + bool useWifi = true; + bool useStep = true; + bool useTurn = true; + bool useBaro = true; + bool useIB = true; + +public: + + void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) { + this->useWifi = useWifi; + this->useStep = useStep; + this->useTurn = useTurn; + this->useBaro = useBaro; + this->useIB = useIB; + } + + virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { + + //if (observation.wifi) { + wifiEval.nextObservation(observation.wifi); + //} + + // evalulate each particle + double sum = 0; + for (K::Particle& p : particles) { + + double weight = 1.0; + + if (useWifi) { + weight *= wifiEval.getProbability(p.state, observation); + } + +// if (useBaro && observation.barometer) { +// weight *= barometerEval.getProbability(p.state, observation.barometer); +// } + +// if (useIB) { +// weight *= beaconEval.getProbability(p.state, observation); +// } + +// if (useStep) { +// weight *= stepEval.getProbability(p.state, observation.step); +// p.state.distanceWalkedCM = 0.0; +// } + +// if (useTurn) { +// weight *= turnEval.getProbability(p.state, observation.turn, true); +// } + + // set and accumulate + p.weight = weight; + sum += p.weight; + + } + + return sum; + + } + +}; + +#endif // MYEVALUATION_H diff --git a/code/particles/MyInitializer.h b/code/particles/MyInitializer.h new file mode 100755 index 0000000..858b036 --- /dev/null +++ b/code/particles/MyInitializer.h @@ -0,0 +1,59 @@ +#ifndef MYINITIALIZER3_H +#define MYINITIALIZER3_H + +#include +#include "MyState.h" + +#include + +class MyInitializer : public K::ParticleFilterInitializer { + +private: + int x_cm; + int y_cm; + int z_cm; + int heading; + + Grid& grid; + +public: + + /** q0 = random */ + MyInitializer(Grid& grid) : grid(grid), heading(0) { + + } + + /** q0 = given */ + MyInitializer(Grid& grid, int x_cm, int y_cm, int z_cm, int heading) : + grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) { + + } + + virtual void initialize(std::vector>& particles) override { + + std::minstd_rand gen; + std::uniform_int_distribution<> dist(0, grid.getNumNodes()); + + for (K::Particle& p : particles) { + + MyGridNode& n = grid[dist(gen)]; + + //p.state.pCur = Point3(x_cm, y_cm, z_cm); + //GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z); + //p.state.walkState.node = &grid.getNodeFor(gp); + + p.state.pCur = (Point3) n; + p.state.walkState.node = &n; + + p.state.pOld = p.state.pCur; + p.state.walkState.heading = Heading::rnd(); + p.state.distanceWalkedCM = 0; + p.state.hPa = 0; + + } + + } + +}; + +#endif // MYINITIALIZER_H diff --git a/code/particles/MyObservation.h b/code/particles/MyObservation.h new file mode 100755 index 0000000..2b045c1 --- /dev/null +++ b/code/particles/MyObservation.h @@ -0,0 +1,49 @@ +#ifndef MYOBSERVATION_H +#define MYOBSERVATION_H + +#include "../frank/WiFiObservation.h" +#include "../frank/BeaconObservation.h" +#include "../toni/BarometerObservation.h" +#include "../lukas/StepObservation.h" +#include "../lukas/TurnObservation.h" + +/** + * all available sensor readings + */ +struct MyObservation { + + /** wifi observation */ + WiFiObservation wifi; + + /** barometer observation data (if any) */ + BarometerObservation* barometer = nullptr; + + /** beacon observation data */ + BeaconObservation beacons; + + /** step observation data (if any) */ + StepObservation* step = nullptr; + + /** turn observation data (if any) */ + TurnObservation* turn = nullptr; + + /** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */ + uint64_t latestSensorDataTS = 0; + + /** ctor */ + MyObservation() { +// reset(); + } + +// /** set all observations to null */ +// void reset() { +// //delete wifi; wifi = nullptr; +// delete barometer; barometer = nullptr; +// delete beacons; beacons = nullptr; +// //delete step; step = nullptr; +// //delete turn; turn = nullptr; +// } + +}; + +#endif // MYOBSERVATION_H diff --git a/code/particles/MyState.h b/code/particles/MyState.h new file mode 100755 index 0000000..1fec717 --- /dev/null +++ b/code/particles/MyState.h @@ -0,0 +1,136 @@ +#ifndef MYSTATE_H +#define MYSTATE_H + +#include +#include + +#include + +#include "../MyGridNode.h" + +/** + * one possible state for the pedestrian + * 3D position (x, y, floor-nr) + */ +struct MyState { + + // current position + Point3 pCur; + + // previous position + Point3 pOld; + + // the grid-walk state + GridWalkState walkState; + + int distanceWalkedCM; + + +// double heading_old; + +// //double transHeading; +// float numZChanges; + +// // cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed! +// double distanceWalkedCM; + + double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h + +// double vertical_acc; //vertical acceleration + +// /** the pedestrian's current heading */ +// double heading; + + /** empty ctor */ + MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) { + ; + } + +// /** get the 2D distance between this state and the given x,y (in centimter) */ +// double getDistance2D(const double x_cm, const double y_cm) const { +// const double dx = (x_cm - this->x_cm); +// const double dy = (y_cm - this->y_cm); +// return std::sqrt( (dx*dx) + (dy*dy) ); +// } + +// /** get the 3D distance between this state and the given x,y,floor (in centimter) */ +// double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const { +// const double dx = (x_cm - this->x_cm); +// const double dy = (y_cm - this->y_cm); +// const double dz = (z_nr - this->z_nr) * floor_height_cm; +// return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) ); +// } + + /** -------- METHODS FOR THE PARTICLE FILTER -------- */ + + MyState& operator += (const MyState& o) { + pCur += o.pCur; + //hPa += o.hPa; + //distanceWalked += o.distanceWalked; + return *this; + } + + MyState& operator /= (const double d) { + pCur /= d; + //hPa /= d; + //distanceWalked /= d; + return *this; + } + + MyState operator * (const double d) const { + MyState s = MyState(*this); + s.pCur *= d; + //s.hPa *= d; + //distanceWalked *= d; + return s; + } + + + // use the default one +// MyState& operator = (const MyState& o) { +// x_cm = o.x_cm; +// y_cm = o.y_cm; +// z_nr = o.z_nr; +// x_cm_old = o.x_cm_old; +// y_cm_old = o.y_cm_old; +// z_nr_old = o.z_nr_old; +// hPa = o.hPa; +// heading_old = o.heading_old; +// heading = o.heading; +// distanceWalkedCM = o.distanceWalkedCM; +// return *this; +// } + +// /** rejection for the regional estimator. reject after 150cm distance */ +// bool belongsToRegion(const MyState& o) const { + +//// // do NOT group particles in distinct floors! +//// if (z_nr != o.z_nr) {return false;} + +//// // get the 2D distance +//// double d = (x_cm - o.x_cm)*(x_cm - o.x_cm) + +//// (y_cm - o.y_cm)*(y_cm - o.y_cm); +//// d = std::sqrt(d); + +//// // 2D distance below grouping threshold? +//// return d < 350.0; + +// const double dx = (x_cm - o.x_cm); +// const double dy = (y_cm - o.y_cm); +// const double dz = (z_nr - o.z_nr) * 3000; + +// // get the 2D distance +// double d = dx*dx + dy*dy + dz*dz; +// d = std::sqrt(d); +// return d < 350.0; + +// } + + + +// MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;} + + +}; + +#endif // MYSTATE_H diff --git a/code/particles/MyTransition.h b/code/particles/MyTransition.h new file mode 100755 index 0000000..29a6fc5 --- /dev/null +++ b/code/particles/MyTransition.h @@ -0,0 +1,189 @@ +#ifndef MYTRANSITION_H +#define MYTRANSITION_H + +#include +#include +#include + + +#include +#include + +#include "MyState.h" +#include "MyControl.h" +//#include "Helper.h" + +#include "../toni/barometric.h" + +#include "../MyGridNode.h" + +inline double sgn(double x){ + return ((x>0)?1 : ((x<0)?-1 : 1)); +} + +class MyTransition : public K::ParticleFilterTransition { + +private: + + Grid& grid; + GridWalk& walker; + + + /** a simple normal distribution */ + K::UniformDistribution distWalkStop; + K::NormalDistribution distWalk; + K::NormalDistribution distStop; + + + /** normal distribution for barometer */ + K::NormalDistribution distBaro; + + +public: + + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ + MyTransition(Grid& grid, GridWalk& walker) : + grid(grid), walker(walker), + distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) { + + distWalkStop.setSeed(1234); + distWalk.setSeed(1234); + distStop.setSeed(1234); + + distBaro.setSeed(5678); + + } + +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 = ts - this->ts; + this->ts = ts; + } + } + + virtual void transition(std::vector>& particles, const MyControl* control) override { + + for (K::Particle& p : particles) { + + // TODO: depending on the time since the last update + // random distance to move +// const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw()); +// double dist_m = distance * deltaMS / 1000.0; + +// if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;} + + // update the old heading and the other old values + //p.state.walkState.heading = p.state.heading; + 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); + + // find the node (square) the particle is within + // just to be safe, we round z to the nearest floor + //Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr)); + const MyGridNode* src = p.state.walkState.node; + + // might happen during initialization: + // the particle is nowhere near the grid.. replace it with a random on on the grid + // alternative: just ignore.. resampling will fix this issue quickly ;) +// if (!src) { +// auto it = graph->getNodes().begin(); +// std::advance(it, rand() % graph->getNodes().size()); +// src = it->second; +// } + + // get new destination + //const Node3* dst = choice->getTarget(src, p.state, dist_m); + p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m ); + + // randomly move the particle within its target grid (box) + // (z remains unchanged!) + 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 + p.state.pCur = (Point3) *p.state.walkState.node + noise; + + + // --- ATTENTION HORRIBLE CODE INCOMING. --- + +// //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!";} + + } + + } + + +}; + +#endif // MYTRANSITION_H diff --git a/code/particles/MyTransitionSimple.h b/code/particles/MyTransitionSimple.h new file mode 100755 index 0000000..35fcb26 --- /dev/null +++ b/code/particles/MyTransitionSimple.h @@ -0,0 +1,83 @@ +#ifndef MYTRANSITIONSIMPLE_H +#define MYTRANSITIONSIMPLE_H + +#include +#include +#include "MyState.h" +#include "MyControl.h" + +class MyTransitionSimple : public K::ParticleFilterTransition { + +private: + + /** a simple normal distribution */ + K::NormalDistribution distX; + K::NormalDistribution distY; + K::NormalDistribution distZ; + K::NormalDistribution distBaro; + +public: + + /** ctor */ + MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) { + distX.setSeed(1234); + distY.setSeed(1235); + distZ.setSeed(1236); + distBaro.setSeed(5678); + } + +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 = ts - this->ts; + this->ts = ts; + } + } + + virtual void transition(std::vector>& particles, const MyControl* control) override { + + for (K::Particle& p : particles) { + + p.state.heading_old = p.state.heading; + p.state.x_cm_old = p.state.x_cm; + p.state.y_cm_old = p.state.y_cm; + p.state.z_nr_old = p.state.z_nr; + + p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0; + p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0; + p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25; + p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old, p.state.x_cm, p.state.y_cm); + +// if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;} +// if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;} +// if (p.state.x_cm < 0) {p.state.x_cm = 0;} +// if (p.state.y_cm < 0) {p.state.y_cm = 0;} + + + + //update barometer + p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw(); + + // update walked distance (2D) + const double dx = p.state.x_cm_old - p.state.x_cm; + const double dy = p.state.y_cm_old - p.state.y_cm; + p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy)); + + + } + + } + + +}; + + +#endif // MYTRANSITIONSIMPLE_H diff --git a/code/particles/P3.h b/code/particles/P3.h new file mode 100755 index 0000000..cd46a23 --- /dev/null +++ b/code/particles/P3.h @@ -0,0 +1,31 @@ +#ifndef P3_H +#define P3_H + +struct P3 { + + double x; + double y; + double z; + + P3() : x(0), y(0), z(0) {;} + P3(const double x, const double y, const double z) : x(x), y(y), z(z) {;} + + P3 operator - (const P3& o) const { + return P3(x-o.x, y-o.y, z-o.z); + } + + P3 operator + (const P3& o) const { + return P3(x+o.x, y+o.y, z+o.z); + } + + P3 operator * (const double v) const { + return P3(x*v, y*v, z*v); + } + + double getLength(const double floorHeight_cm) const { + return std::sqrt(x*x + y*y + z*floorHeight_cm*z*floorHeight_cm); + } + +}; + +#endif // P3_H diff --git a/code/toni/BarometerEvaluation.h b/code/toni/BarometerEvaluation.h index c2597b0..27f86c8 100755 --- a/code/toni/BarometerEvaluation.h +++ b/code/toni/BarometerEvaluation.h @@ -2,10 +2,10 @@ #include "../particles/MyState.h" #include "BarometerObservation.h" -#include "barometric.h" -#include +//#include "barometric.h" +//#include -double g_BarometerObservation = 0.0; +static constexpr double g_BarometerObservation = 0.0; class BarometerEvaluation { @@ -13,46 +13,50 @@ public: double getProbability(const MyState& state, const BarometerObservation* obs) const { - //rho_z - double barometerSigma = 0.3; + return 1; - //The height of the single floor levels. - const static double floor_height[3] = {4.1, 3.4, 3.4}; +// //rho_z +// double barometerSigma = 0.3; - if(USE_BAROMETRIC_FORMULAR){ - //height the particle has climbed. - double h_1 = 0.0; - for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){ - h_1 += floor_height[i]; - } +// //The height of the single floor levels. +// const static double floor_height[3] = {4.1, 3.4, 3.4}; - if(h_1 != 0.0){ - // 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); - barometerSigma = std::abs(mslp - pressure); - } +// if(USE_BAROMETRIC_FORMULAR){ +// //height the particle has climbed. +// double h_1 = 0.0; +// for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){ +// h_1 += floor_height[i]; +// } - } - else { - // constant value for sigma if we assume all floors are same in height - barometerSigma = 0.30 / 1.0; //hPa - } +// if(h_1 != 0.0){ +// // 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); +// barometerSigma = std::abs(mslp - pressure); +// } - // evaluate the current particle with a normal distribution - const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa); +// } +// else { +// // constant value for sigma if we assume all floors are same in height +// barometerSigma = 0.30 / 1.0; //hPa +// } - //Just for the visualization. i'm a lazy bastard - g_BarometerObservation = obs->hpa; +// // evaluate the current particle with a normal distribution +// const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa); - assert(barometerProbability == barometerProbability); - assert(state.hPa == state.hPa); - assert(obs->hpa == obs->hpa); +// //Just for the visualization. i'm a lazy bastard +// g_BarometerObservation = obs->hpa; - //std::cout << barometerProbability << std::endl; +// assert(barometerProbability == barometerProbability); +// assert(state.hPa == state.hPa); +// assert(obs->hpa == obs->hpa); + +// //std::cout << barometerProbability << std::endl; + +// return pow(2.0, barometerProbability); +// //return barometerProbability; + + } - return pow(2.0, barometerProbability); - //return barometerProbability; - } }; diff --git a/code/toni/BarometerSensorReader.h b/code/toni/BarometerSensorReader.h index f9cbe2b..ee6ca28 100755 --- a/code/toni/BarometerSensorReader.h +++ b/code/toni/BarometerSensorReader.h @@ -2,7 +2,7 @@ #include "circular.h" #include "BarometerObservation.h" -#include "../SensorReader.h" +#include "../reader/SensorReader.h" #include //circular_buffer measurementHistory(1000); From 3419078453b292d5fb468ffe174b0d9fa2b0cc85 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 29 Jan 2016 15:26:16 +0100 Subject: [PATCH 3/5] title and form --- tex/bare_conf.tex | 6 +++--- tex/chapters/relatedwork.tex | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/tex/bare_conf.tex b/tex/bare_conf.tex index 4b58606..bf4e04d 100644 --- a/tex/bare_conf.tex +++ b/tex/bare_conf.tex @@ -131,7 +131,7 @@ % not capitalized unless they are the first or last word of the title. % Linebreaks \\ can be used within to get better formatting as desired. % Do not put math or special symbols in the title. -\title{Realistic Pathfinding for Multi Sensor 3D Indoor Localisation and Navigation} +\title{On Prior Navigation Knowledge in Multi Sensor Indoor Localisation} % author names and affiliations @@ -180,10 +180,10 @@ \input{chapters/relatedwork} -\input{chapters/grid} - \input{chapters/system} +\input{chapters/grid} + \input{chapters/experiments} \input{chapters/conclusion} diff --git a/tex/chapters/relatedwork.tex b/tex/chapters/relatedwork.tex index 7c37295..2db85c3 100644 --- a/tex/chapters/relatedwork.tex +++ b/tex/chapters/relatedwork.tex @@ -6,7 +6,9 @@ \subitem klarstellen was wir anders/besser machen \item graphen-basierte systeme \subitem probability graph / transition - \item pathfinding + \item pathfinding for humans \subitem computerspiele machen das schon ewig. robotor auch. \subitem auf menschliches verhalten anpassen. gibt es viele theoritsche ansätze und simulationen aber in noch keinem system. \end{itemize} + + From cc899d1c4679d4c8a237c0d2fd6f4f270f5d6139 Mon Sep 17 00:00:00 2001 From: FrankE Date: Sat, 30 Jan 2016 19:50:58 +0100 Subject: [PATCH 4/5] updates the visualisation removed obsolte parts fixed baromter stuff worked on eval added ground-truth --- code/Helper.h | 14 +++ code/OldGroundTruth.h | 138 ++++++++++++++++++++++++++++++ code/Settings.h | 2 +- code/Vis.h | 46 ++++++++++ code/eval/Eval.h | 12 ++- code/eval/EvalBase.h | 88 ++++++++++++------- code/eval/GroundTruthWay.h | 2 +- code/frank/BeaconEvaluation.h | 4 +- code/frank/PositionedBeacon.h | 2 +- code/frank/Settings.h | 24 +++--- code/frank/WiFiEvaluation.h | 13 ++- code/particles/MyEvaluation.h | 12 +-- code/particles/MyState.h | 10 ++- code/particles/MyTransition.h | 20 +---- code/particles/P3.h | 31 ------- code/plan.svg | 25 +++--- code/toni/BarometerEvaluation.h | 20 ++--- code/toni/BarometerSensorReader.h | 115 ++++++++++++++----------- 18 files changed, 388 insertions(+), 190 deletions(-) create mode 100644 code/OldGroundTruth.h delete mode 100755 code/particles/P3.h diff --git a/code/Helper.h b/code/Helper.h index dcf5fee..58eb290 100644 --- a/code/Helper.h +++ b/code/Helper.h @@ -9,6 +9,7 @@ #include "Settings.h" #include "MyGridNode.h" +#include "OldGroundTruth.h" class Helper { @@ -62,6 +63,9 @@ public: const LengthF h2 = LengthF::cm(align(getHeight(2))); const LengthF h3 = LengthF::cm(align(getHeight(3))); + // all ground-truth points + std::unordered_map gtwp; + FHWSFloors() {;} }; @@ -81,6 +85,16 @@ public: f.s12 = fpFac.getStairs("staircase_1_2"); f.s23 = fpFac.getStairs("staircase_2_3"); + OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222); + OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222); + OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 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));} + return f; } diff --git a/code/OldGroundTruth.h b/code/OldGroundTruth.h new file mode 100644 index 0000000..c3a4f91 --- /dev/null +++ b/code/OldGroundTruth.h @@ -0,0 +1,138 @@ +#ifndef OLDGROUNDTRUTH_H +#define OLDGROUNDTRUTH_H + +#include +#include +#include +#include + +/** + * TODO: REMOVE + */ +class OldGroundTruth { + + /** + * helper class for SVG floorplans. + * + * converts between the SVG's scale and real-world scale + */ + class SVGScaler { + + private: + + /** the scaling factor to apply to the svg data */ + double scalingFactor; + + public: + + /** ctor */ + SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) { + ; + } + + /** scale (x, y) into (_x, _y) */ + void scale(const double x, const double y, float& _x, float& _y) const { + _x = x * scalingFactor; + _y = y * scalingFactor; + } + + /** scale the given point into a new output point */ + Point2 scale(const K::Point p) const { + Point2 ret; + scale (p.x, p.y, ret.x, ret.y); + return ret; + } + + /** scale the given line into a new output line */ + Line2 scale(const K::Line l) const { + Line2 ret; + scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y); + scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y); + return ret; + } + + }; + +private: + + /** helper to scale the SVG into real-world-scale */ + SVGScaler scaler; + + /** all ground-truth waypoints within the floorplan */ + std::unordered_map points; + +public: + + OldGroundTruth() :scaler(0) {;} + + /** + * ctor + * @param file the svg's filename + * @param layerName the name of the layer (within the SVG) to load + * @param scalingFactor the scaling to apply to convert between SVG and real-world scale + */ + OldGroundTruth(const std::string& file, const std::string& layerName, const double scalingFactor) : scaler(scalingFactor) { + + K::SVGFile svg; + K::SVGLoader::load(K::File(file), &svg); + K::SVGComposite* sc = svg.getLayers(); + + K::SVGLayer* layer = sc->getContainedLayerNamed(layerName); + if (!layer) {throw "svg has no layer named '" + layerName + "'";} + load(layer); + + } + + /** get all waypoints */ + const std::unordered_map& getWaypoints() const { + return points; + } + + +private: + + + + /** recursive loading/parsing of nested SVG elements */ + void load(K::SVGElement* el) { + + switch (el->getType()) { + + case SVGElementType::COMPOSITE: { + for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) { + load(sub); + } + break; + } + + case SVGElementType::LAYER: { + K::SVGLayer* layer = (K::SVGLayer*) el; + for (K::SVGElement* sub : layer->getChilds()) { + load(sub); + } + break; + } + + case SVGElementType::TEXT: { + const K::SVGText* text = (K::SVGText*) el; + if (text->getText().empty()) {break;} + const int id = std::stoi(text->getText()); + points[id] = scaler.scale(K::Point(text->getPosition().x, text->getPosition().y)); + break; + } + + case SVGElementType::PATH: { + break; + } + + default: + throw "should not happen!"; + + } + + } + + +}; + +#endif // OLDGROUNDTRUTH_H diff --git a/code/Settings.h b/code/Settings.h index 1722a92..bbb1900 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -16,7 +16,7 @@ namespace MiscSettings { const int timeSteps = 500; - const int numParticles = 1000; + const int numParticles = 2500; } diff --git a/code/Vis.h b/code/Vis.h index d90fbeb..d7d4035 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -10,6 +10,8 @@ #include #include +#include "eval/GroundTruthWay.h" + class Vis { public: @@ -20,6 +22,8 @@ public: K::GnuplotSplotElementColorPoints gridNodes; K::GnuplotSplotElementLines gridEdges; K::GnuplotSplotElementPoints particles; + K::GnuplotSplotElementLines groundTruth; + K::GnuplotSplotElementLines estPath; public: @@ -35,11 +39,18 @@ public: gp << "unset ztics\n"; gp << "unset border\n"; + groundTruth.setLineWidth(2); + groundTruth.setColorHex("#666666"); + + estPath.setLineWidth(2); + // attach all layers splot.add(&floors); splot.add(&gridNodes); splot.add(&gridEdges); splot.add(&particles); + splot.add(&groundTruth); + splot.add(&estPath); } @@ -81,6 +92,27 @@ public: return *this; } + void addGroundTruth(GroundTruthWay& gtw) { + groundTruth.clear(); + for (auto it : gtw.getWay()) { + K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z); + groundTruth.add(gp); + } + } + + void addEstPath(std::vector& est) { + estPath.clear();; + for (const Point3& p : est) { + K::GnuplotPoint3 gp(p.x, p.y, p.z); + estPath.add(gp); + } + } + + void setTimestamp(uint64_t ts) { + static uint64_t firstTs = ts; + gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n"; + } + void removeGrid() { gridNodes.clear();; } @@ -89,6 +121,20 @@ public: particles.clear(); } + void addObject(const int idx, const Point3& p) { + 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 << "\n"; + } + + void setEstAndShould(const Point3& est, const Point3& should) { + addObject(2,est); + addObject(3,should); + } + template void addState(const GridWalkState& n) { particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm)); } diff --git a/code/eval/Eval.h b/code/eval/Eval.h index 45b8b8d..dd8a7bf 100644 --- a/code/eval/Eval.h +++ b/code/eval/Eval.h @@ -9,6 +9,8 @@ #include #include #include +#include +#include class Eval : public EvalBase { @@ -26,15 +28,16 @@ public: //GridWalkLightAtTheEndOfTheTunnel* walk = new GridWalkLightAtTheEndOfTheTunnel(grid, DijkstraMapper(grid), end); //GridWalkRandomHeadingUpdate* walk = new GridWalkRandomHeadingUpdate(); - //GridWalkRandomHeadingUpdateAdv* walk = new GridWalkRandomHeadingUpdateAdv(); - GridWalkPushForward* walk = new GridWalkPushForward(); + GridWalkRandomHeadingUpdateAdv* walk = new GridWalkRandomHeadingUpdateAdv(); + //GridWalkPushForward* walk = new GridWalkPushForward(); pf->setTransition( std::unique_ptr( 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"); - //gtw = getGroundTruthWay(*sr, gtwp, way2); + + gtw = getGroundTruthWay(*sr, floors.gtwp, way2); } @@ -47,7 +50,7 @@ public: // the particle filter's evaluation method std::unique_ptr eval = std::unique_ptr( new MyEvaluation() ); - eval.get()->setUsage(true, false, false, false, false); + eval.get()->setUsage(true, false, false, true, true); pf->setEvaluation( std::move(eval) ); // resampling step? @@ -56,6 +59,7 @@ public: // state estimation step pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); } diff --git a/code/eval/EvalBase.h b/code/eval/EvalBase.h index 41b0714..99a4c15 100644 --- a/code/eval/EvalBase.h +++ b/code/eval/EvalBase.h @@ -10,7 +10,6 @@ #include "GroundTruthWay.h" -#include "../particles/P3.h" #include "../particles/MyState.h" #include "../particles/MyObservation.h" #include "../particles/MyEvaluation.h" @@ -43,10 +42,15 @@ protected: SensorReaderTurn* srt; SensorReaderStep* srs; - GroundTruthWay gtw; + std::string runName; + GroundTruthWay gtw; + std::vector 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 way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector 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}; + public: EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) { @@ -62,11 +66,39 @@ public: } + GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map& waypoints, std::vector ids) { + + // construct the ground-truth-path by using all contained waypoint ids + std::vector path; + for (int id : ids) { + auto it = waypoints.find(id); + assert(it != waypoints.end()); + path.push_back(it->second); + } + + // new created the timed path + GroundTruthWay gtw; + int i = 0; + while (sr.hasNext()) { + const SensorEntry se = sr.getNext(); + if (se.data.empty()) {continue;} // why necessary?? + if (se.idx == 99) { + gtw.add(se.ts, path[i]); + ++i; + } + } + + // ensure the sensor-data contained usable timestamps for the ground-truth mapping + assert(i>0); + + sr.rewind(); + return gtw; + + } + void run() { - // read CSV input - // const int s_wifi = 0; - //SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv"); + // sensor numbers const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_linearAcceleration = 2; @@ -111,10 +143,10 @@ public: std::vector pathEst; uint64_t lastTransitionTS = 0; - bool firstReading = true; int64_t start_time = -1; K::Statistics stats; + int cnt = 0; // process each sensor reading while(sr->hasNext()) { @@ -127,12 +159,6 @@ public: if (start_time == -1) {start_time = se.ts;} int64_t current_time = se.ts - start_time; - // ensure the graph timestamp starts with the first reading - if (firstReading) { - //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); - firstReading = false; - } - switch(se.idx) { case s_wifi: { @@ -203,37 +229,33 @@ public: } - // let the transition know the current timestamp to determine the time since the last transition - //if (!useSimpleTrans) { - ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); - //} else { - // ((MyTransitionSimple*)pf->getTransition())->setCurrentTime(lastTransitionTS); - //} + // 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 Point3 curEst = est.pCur; - pathEst.push_back(curEst); - - // debug print current particle set. - //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); // error calculation. compare ground-truth to estimation -// const Point3 curGT = gtw.getPosAtTime(se.ts - 750); + const Point3 curGT = gtw.getPosAtTime(se.ts - 750); + const Point3 diff = curEst - curGT; -// // TODO -// const Point3 diff = curEst - curGT; - -// //if (std::abs(diff.z) < 0.1) { -// const float err = diff.length(); -// std::cout << err << std::endl; -// stats.add(err); -// std::cout << stats.asString() << std::endl; -// //} + // skip the first 8 scans due to uniform distribution start + if (++cnt > 8) { + pathEst.push_back(curEst); + const float err = diff.length(); + stats.add(err); + std::cout << stats.asString() << std::endl; + } + // plot vis.clearStates(); for (const K::Particle p : pf->getParticles()) {vis.addState(p.state.walkState);} - + vis.setTimestamp(se.ts); + vis.addGroundTruth(gtw); + vis.addEstPath(pathEst); + vis.setEstAndShould(curEst, curGT); vis.show();; } diff --git a/code/eval/GroundTruthWay.h b/code/eval/GroundTruthWay.h index b7631dc..2e1387c 100644 --- a/code/eval/GroundTruthWay.h +++ b/code/eval/GroundTruthWay.h @@ -16,7 +16,7 @@ public: } /** get the ground truth way */ - const std::vector& getWay() const {return entries;} + const std::vector& getWay() const {return entries;} }; diff --git a/code/frank/BeaconEvaluation.h b/code/frank/BeaconEvaluation.h index 3ac5eda..19f38b1 100755 --- a/code/frank/BeaconEvaluation.h +++ b/code/frank/BeaconEvaluation.h @@ -23,7 +23,7 @@ public: double prob = 1.0; // const double tx = -74; - const double waf = 20.0; + const double waf = 8.0; // // get the ap the client had the strongest measurement for // const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); @@ -40,7 +40,7 @@ public: // distance (in meter) between particle and AP //const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0; - const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0; + const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0; // floor difference? //const double floorDist = std::abs(beacon->zNr - state.getFloorNr()); diff --git a/code/frank/PositionedBeacon.h b/code/frank/PositionedBeacon.h index 534df8c..58f76e4 100755 --- a/code/frank/PositionedBeacon.h +++ b/code/frank/PositionedBeacon.h @@ -21,7 +21,7 @@ public: /** ctor */ PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) : - mac(mac), tx(tx), pl(pl), Point3(x_cm, y_cm, z_cm) { + Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) { ; } diff --git a/code/frank/Settings.h b/code/frank/Settings.h index cc82dbb..6818d7c 100755 --- a/code/frank/Settings.h +++ b/code/frank/Settings.h @@ -20,27 +20,27 @@ public: Settings() { - const double pl = 2.7; + 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-0.5); - addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl-0.5); - addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.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); + 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-0.5); + 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-0.5); - addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5); + 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-0.25); - addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl-0.25); + 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); @@ -63,9 +63,9 @@ public: 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("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); diff --git a/code/frank/WiFiEvaluation.h b/code/frank/WiFiEvaluation.h index 16ea667..c33ebf4 100755 --- a/code/frank/WiFiEvaluation.h +++ b/code/frank/WiFiEvaluation.h @@ -38,10 +38,9 @@ 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 int floor_height_cm = 350; // get the ap the client had the strongest measurement for - const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); + //const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); //const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; //const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); //const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); @@ -53,13 +52,13 @@ public: const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap); // distance (in meter) between particle and AP - //const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0; - const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0; + const float distToAP_m = state.pCur.getDistance(*ap) / 100.0; // floor difference? - const float floorDiff = std::round( - std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)) - ); + const float floorDiff = //std::ceil( + std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z)); + //); + //const float floorDiff = std::abs(ap->z - state.pCur.z) / 340; // estimate the rssi depending on above distance const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf); diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h index 4c011d6..09a02f8 100755 --- a/code/particles/MyEvaluation.h +++ b/code/particles/MyEvaluation.h @@ -53,13 +53,13 @@ public: weight *= wifiEval.getProbability(p.state, observation); } -// if (useBaro && observation.barometer) { -// weight *= barometerEval.getProbability(p.state, observation.barometer); -// } + if (useBaro && observation.barometer) { + weight *= barometerEval.getProbability(p.state, observation.barometer); + } -// if (useIB) { -// weight *= beaconEval.getProbability(p.state, observation); -// } + if (useIB) { + weight *= beaconEval.getProbability(p.state, observation); + } // if (useStep) { // weight *= stepEval.getProbability(p.state, observation.step); diff --git a/code/particles/MyState.h b/code/particles/MyState.h index 1fec717..48ffd3d 100755 --- a/code/particles/MyState.h +++ b/code/particles/MyState.h @@ -65,14 +65,14 @@ struct MyState { MyState& operator += (const MyState& o) { pCur += o.pCur; - //hPa += o.hPa; + hPa += o.hPa; //distanceWalked += o.distanceWalked; return *this; } MyState& operator /= (const double d) { pCur /= d; - //hPa /= d; + hPa /= d; //distanceWalked /= d; return *this; } @@ -80,7 +80,7 @@ struct MyState { MyState operator * (const double d) const { MyState s = MyState(*this); s.pCur *= d; - //s.hPa *= d; + s.hPa *= d; //distanceWalked *= d; return s; } @@ -101,6 +101,10 @@ struct MyState { // return *this; // } + bool belongsToRegion(const MyState& o) const { + return o.pCur.getDistance(pCur) < 700; + } + // /** rejection for the regional estimator. reject after 150cm distance */ // bool belongsToRegion(const MyState& o) const { diff --git a/code/particles/MyTransition.h b/code/particles/MyTransition.h index 29a6fc5..824d02e 100755 --- a/code/particles/MyTransition.h +++ b/code/particles/MyTransition.h @@ -48,7 +48,7 @@ public: */ MyTransition(Grid& grid, GridWalk& walker) : grid(grid), walker(walker), - distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) { + distWalkStop(0.0, 1.0), distWalk(1.5, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) { distWalkStop.setSeed(1234); distWalk.setSeed(1234); @@ -101,20 +101,6 @@ public: // update cumulative distance p.state.distanceWalkedCM += std::abs(dist_m * 100.0); - // find the node (square) the particle is within - // just to be safe, we round z to the nearest floor - //Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr)); - const MyGridNode* src = p.state.walkState.node; - - // might happen during initialization: - // the particle is nowhere near the grid.. replace it with a random on on the grid - // alternative: just ignore.. resampling will fix this issue quickly ;) -// if (!src) { -// auto it = graph->getNodes().begin(); -// std::advance(it, rand() % graph->getNodes().size()); -// src = it->second; -// } - // get new destination //const Node3* dst = choice->getTarget(src, p.state, dist_m); p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m ); @@ -129,7 +115,7 @@ public: // --- ATTENTION HORRIBLE CODE INCOMING. --- - + 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; @@ -167,7 +153,7 @@ public: // if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING) // p.state.hPa += numFloorChanged * distBaro.draw(); // else -// p.state.hPa = numFloorChanged * distBaro.draw(); + // p.state.hPa = numFloorChanged * distBaro.draw(); // } // // sanity check diff --git a/code/particles/P3.h b/code/particles/P3.h deleted file mode 100755 index cd46a23..0000000 --- a/code/particles/P3.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef P3_H -#define P3_H - -struct P3 { - - double x; - double y; - double z; - - P3() : x(0), y(0), z(0) {;} - P3(const double x, const double y, const double z) : x(x), y(y), z(z) {;} - - P3 operator - (const P3& o) const { - return P3(x-o.x, y-o.y, z-o.z); - } - - P3 operator + (const P3& o) const { - return P3(x+o.x, y+o.y, z+o.z); - } - - P3 operator * (const double v) const { - return P3(x*v, y*v, z*v); - } - - double getLength(const double floorHeight_cm) const { - return std::sqrt(x*x + y*y + z*floorHeight_cm*z*floorHeight_cm); - } - -}; - -#endif // P3_H diff --git a/code/plan.svg b/code/plan.svg index 8ec6368..5cb035b 100755 --- a/code/plan.svg +++ b/code/plan.svg @@ -43,16 +43,16 @@ inkscape:pageopacity="0" inkscape:pageshadow="2" inkscape:window-width="1600" - inkscape:window-height="782" + inkscape:window-height="845" id="namedview3234" showgrid="true" - inkscape:zoom="2.4027176" - inkscape:cx="555.925" - inkscape:cy="388.39069" + inkscape:zoom="0.84948895" + inkscape:cx="272.89941" + inkscape:cy="1411.2257" inkscape:window-x="0" inkscape:window-y="0" inkscape:window-maximized="1" - inkscape:current-layer="layer16" + inkscape:current-layer="layer8" inkscape:object-nodes="true" units="px" showborder="true" @@ -118,7 +118,7 @@ inkscape:groupmode="layer" id="layer1" inkscape:label="roomnames_3" - style="display:none"> + style="display:inline"> + style="display:none"> @@ -4398,7 +4398,7 @@ sodipodi:nodetypes="ccc" /> @@ -4423,9 +4423,10 @@ sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> hpa); + const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa); // //Just for the visualization. i'm a lazy bastard -// g_BarometerObservation = obs->hpa; + //g_BarometerObservation = obs->hpa; -// assert(barometerProbability == barometerProbability); -// assert(state.hPa == state.hPa); -// assert(obs->hpa == obs->hpa); + assert(barometerProbability == barometerProbability); + assert(state.hPa == state.hPa); + assert(obs->hpa == obs->hpa); // //std::cout << barometerProbability << std::endl; -// return pow(2.0, barometerProbability); -// //return barometerProbability; + //return pow(2.0, barometerProbability); + return barometerProbability; } diff --git a/code/toni/BarometerSensorReader.h b/code/toni/BarometerSensorReader.h index ee6ca28..77c2318 100755 --- a/code/toni/BarometerSensorReader.h +++ b/code/toni/BarometerSensorReader.h @@ -5,25 +5,29 @@ #include "../reader/SensorReader.h" #include +#include + //circular_buffer measurementHistory(1000); class BarometerSensorReader{ private: - circular_buffer measurementHistory; +// circular_buffer measurementHistory; + + MovingAVG avg; public: - BarometerSensorReader(){ - if(!USE_STATIC_CIRCULAR_BUFFERING){ - //8.33min - measurementHistory.reserve(10000); - } - else{ - //30 * 500ms = 1,5s - measurementHistory.reserve(30); - } + BarometerSensorReader(): avg(3) { +// if(!USE_STATIC_CIRCULAR_BUFFERING){ +// //8.33min +// measurementHistory.reserve(10000); +// } +// else{ +// //30 * 500ms = 1,5s +// measurementHistory.reserve(30); +// } } BarometerObservation* readBarometer(const SensorEntry& se) { @@ -31,52 +35,63 @@ public: std::string tmp = se.data; BarometerObservation* obs = new BarometerObservation(); - //Read the hPa - double hPa = stod(tmp); + // get the next hPa reading and average it + avg.add(stod(tmp)); + const double hPa = avg.get(); - // load the measurement at current time into the history - double currentMeasurement = hPa - measurementHistory[0]; + // everything realtive to the first measurement + static double first_hPa = 0; + if (avg.getNumUsed() < avg.getSize()) {first_hPa = avg.get();} - if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){ + obs->hpa = hPa - first_hPa; - //smoothing with alpha value - if(measurementHistory.size() > 1){ - double alpha = 0.1; - double lastMeasurement = measurementHistory[measurementHistory.size() - 1]; - currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement); - - obs->hpa = currentMeasurement; - }else{ - obs->hpa = 0; - } - - measurementHistory.push_back(currentMeasurement); - } - else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){ - - currentMeasurement = hPa; - measurementHistory.push_back(currentMeasurement); - - // calculate the relative air pressure by getting the mean of the first and last three entrys of the history - // and subtract them. - if (measurementHistory.size() > 5){ - double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0; - double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0; - - obs->hpa = meanHead - meanTail; - } - else{ - obs->hpa = 0; - } - } - else //no data smoothing - { - measurementHistory.push_back(currentMeasurement); - obs->hpa = currentMeasurement; - } + std::cout << obs->hpa << std::endl; + // done return obs; + + +// if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){ + +// //smoothing with alpha value +// if(measurementHistory.size() > 1){ +// double alpha = 0.1; +// double lastMeasurement = measurementHistory[measurementHistory.size() - 1]; +// currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement); + +// obs->hpa = currentMeasurement; +// }else{ +// obs->hpa = 0; +// } + +// measurementHistory.push_back(currentMeasurement); +// } +// else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){ + +// currentMeasurement = hPa; +// measurementHistory.push_back(currentMeasurement); + +// // calculate the relative air pressure by getting the mean of the first and last three entrys of the history +// // and subtract them. +// if (measurementHistory.size() > 5){ +// double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0; +// double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0; + +// obs->hpa = meanHead - meanTail; +// } +// else{ +// obs->hpa = 0; +// } +// } +// else //no data smoothing +// { +// measurementHistory.push_back(currentMeasurement); +// obs->hpa = currentMeasurement; +// } + +// return obs; + } //TODO From 8c3749c000d77fab81d126cfdb50a6c823b5691f Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 1 Feb 2016 19:15:17 +0100 Subject: [PATCH 5/5] added floorplan for new eval --- code/plan_new.svg | 4709 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 4709 insertions(+) create mode 100644 code/plan_new.svg diff --git a/code/plan_new.svg b/code/plan_new.svg new file mode 100644 index 0000000..b9a399c --- /dev/null +++ b/code/plan_new.svg @@ -0,0 +1,4709 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + 26 + 27 + + 28 + 29 + 5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +