From 07d739ebb7a7f018e675b99de1762fcf1f098cd5 Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 27 Jan 2016 21:39:21 +0100 Subject: [PATCH 1/2] 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/2] 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);