From d283d9b326c0a8bd8652587a17130f2b8f2e04ee Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 7 Sep 2016 10:16:51 +0200 Subject: [PATCH] geometry changes/fixes/new features new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping --- Assertions.h | 8 +- geo/Angle.h | 11 +- geo/Line2.h | 60 +++-- grid/factory/v2/GridFactory.h | 73 ++++-- grid/factory/v2/GridFactoryListener.h | 19 ++ grid/factory/v2/Importance.h | 43 +++- grid/factory/v2/Stairs.h | 24 +- grid/walk/v2/GridWalker.h | 42 ++-- grid/walk/v2/modules/WalkModule.h | 6 +- grid/walk/v2/modules/WalkModuleFavorZ.h | 2 +- grid/walk/v2/modules/WalkModuleHeading.h | 9 +- .../v2/modules/WalkModuleHeadingControl.h | 51 +++- grid/walk/v2/modules/WalkModuleSpread.h | 2 +- grid/walk/v2/modules/WalkStateHeading.h | 31 ++- math/DrawList.h | 1 + math/MovingAverageTS.h | 67 +++++ misc/KNN.h | 10 +- sensors/imu/AccelerometerData.h | 22 ++ sensors/imu/StepDetection.h | 195 +++++++++----- sensors/imu/TurnDetection.h | 237 ++++++++---------- sensors/offline/OfflineAndroid.h | 135 ++++++++-- sensors/radio/VAPGrouper.h | 4 + tests/data/WalkHeadingMap.xml | 30 +++ tests/grid/Plot.h | 8 +- tests/grid/TestGridWalk2HeadingControl.cpp | 177 +++++++++++++ ...ssure.cpp => TestGridWalk2RelPressure.cpp} | 32 +-- tests/grid/TestGridWalkV2.cpp | 10 +- 27 files changed, 976 insertions(+), 333 deletions(-) create mode 100644 grid/factory/v2/GridFactoryListener.h create mode 100644 math/MovingAverageTS.h create mode 100644 tests/data/WalkHeadingMap.xml create mode 100644 tests/grid/TestGridWalk2HeadingControl.cpp rename tests/grid/{GridWalk2RelPressure.cpp => TestGridWalk2RelPressure.cpp} (70%) diff --git a/Assertions.h b/Assertions.h index f4b5d1c..567ef4d 100644 --- a/Assertions.h +++ b/Assertions.h @@ -3,6 +3,7 @@ #include "Exception.h" #include +#include // for GTEST Testcases #define FRIEND_TEST(test_case_name, test_name)\ @@ -66,9 +67,12 @@ namespace Assert { if (std::abs(v1-v2) > delta) {doThrow(err);} } + template static inline void isBetween(const T v, const T min, const T max, const STR err) { - std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n"; - if (v < min || v > max) {doThrow(err+ss.str());} + if (v < min || v > max) { + std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n"; + doThrow(err+ss.str()); + } } } diff --git a/geo/Angle.h b/geo/Angle.h index 5cfd6b6..e278bcf 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -46,10 +46,13 @@ public: * - as a change-in-direction between [-PI:+PI] */ static float getSignedDiffRAD_2PI(const float r1, const float r2) { - Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); - Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); - const float a1 = (r2-r1); - if (std::abs(a1) < M_PI) {return a1;} else {return (M_PI-a1);} + Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); // [0:360] deg + Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); // [0:360] deg + float diff = r1-r2; + if (diff > +M_PI) {diff = -(2*M_PI - diff);} + else if (diff < -M_PI) {diff = +(2*M_PI + diff);} + Assert::isBetween(diff, (float)-M_PI, (float)(+M_PI), "result out of bounds"); // [-180:+180] deg + return diff; } /** convert degrees to radians */ diff --git a/geo/Line2.h b/geo/Line2.h index 9350697..b8ee038 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -55,28 +55,54 @@ public: bool getSegmentIntersection(const Line2& other) const { - const double delta = 0.0000001; + const float p0_x = p1.x, p1_x = p2.x, p2_x = other.p1.x, p3_x = other.p2.x; + const float p0_y = p1.y, p1_y = p2.y, p2_y = other.p1.y, p3_y = other.p2.y; - const double bx = p2.x - p1.x; - const double by = p2.y - p1.y; + const float s1_x = p1_x - p0_x; + const float s1_y = p1_y - p0_y; + const float s2_x = p3_x - p2_x; + const float s2_y = p3_y - p2_y; - const double dx = other.p2.x - other.p1.x; - const double dy = other.p2.y - other.p1.y; + const float s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y); + const float t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y); - const double b_dot_d_perp = bx*dy - by*dx; + if (s >= 0 && s <= 1 && t >= 0 && t <= 1) { + // Collision detected +// if (i_x != NULL) +// *i_x = p0_x + (t * s1_x); +// if (i_y != NULL) +// *i_y = p0_y + (t * s1_y); + return true; + } - if (std::abs(b_dot_d_perp) == 0) {return false;} + return false; // No collision - const double cx = other.p1.x - p1.x; - const double cy = other.p1.y - p1.y; - const double t = (cx * dy - cy * dx) / b_dot_d_perp; - if(t < 0+delta || t > 1-delta) {return false;} - const double u = (cx * by - cy * bx) / b_dot_d_perp; - if(u < 0+delta || u > 1-delta) {return false;} +// const double delta = 0.0000001; - return true; +// const double bx = p2.x - p1.x; +// const double by = p2.y - p1.y; + +// const double dx = other.p2.x - other.p1.x; +// const double dy = other.p2.y - other.p1.y; + +// const double b_dot_d_perp = bx*dy - by*dx; + +// if (std::abs(b_dot_d_perp) == 0) {return false;} + +// const double cx = other.p1.x - p1.x; +// const double cy = other.p1.y - p1.y; + +// const double t = (cx * dy - cy * dx) / b_dot_d_perp; +// if(t < 0+delta || t > 1-delta) { +// return false;} + +// const double u = (cx * by - cy * bx) / b_dot_d_perp; +// if(u < 0+delta || u > 1-delta) { +// return false;} + +// return true; } @@ -98,10 +124,12 @@ public: const float cy = other.p1.y - p1.y; const float t = (cx * dy - cy * dx) / b_dot_d_perp; - if(t < 0+delta || t > 1-delta) {return false;} + if(t < 0+delta || t > 1-delta) { + return false;} const float u = (cx * by - cy * bx) / b_dot_d_perp; - if(u < 0+delta || u > 1-delta) {return false;} + if(u < 0+delta || u > 1-delta) { + return false;} result.x = p1.x + t * bx; result.y = p1.y + t * by; diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 4e8b118..8568656 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -18,18 +18,7 @@ #include -/** listen for events during the build process */ -class GridFactoryListener { - -public: - - virtual void onGridBuildUpdateMajor(const std::string& what) = 0; - virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0; - - virtual void onGridBuildUpdateMinor(const std::string& what) = 0; - virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0; - -}; +#include "GridFactoryListener.h" template class GridFactory { @@ -156,7 +145,7 @@ public: GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); // slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit - bbox.grow(0.012345); + bbox.grow(0.42345); if (intersects(bbox, floor)) {continue;} // add to the grid @@ -170,7 +159,7 @@ public: } // connect the g - connectAdjacent(z_cm); + connectAdjacent(floor, z_cm); } @@ -186,6 +175,9 @@ public: if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);} } + // cleanup + stairs.finalize(); + } @@ -273,7 +265,7 @@ public: } /** connect all neighboring nodes located on the given height-plane */ - void connectAdjacent(const float z_cm) { + void connectAdjacent(const Floorplan::Floor* floor, const float z_cm) { Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false); Log::tick(); @@ -285,7 +277,7 @@ public: if (n1.z_cm != z_cm) {continue;} // connect the node with its neighbors - connectAdjacent(n1); + connectAdjacent(floor, n1); } @@ -293,8 +285,13 @@ public: } - /** connect the given node with its neighbors */ - void connectAdjacent(T& n1) { + /** + * connect the given node with its neighbors. + * even though a node has a neighbor, it might still be blocked by an obstacle: + * e.g. a 45° wall directly between nodes. a neighbor exists, but is unreachable due to the wall. + * we thus perform an additional intersection check with all obstacles within the floor the node n1 belongs to + */ + void connectAdjacent(const Floorplan::Floor* floor, T& n1) { const int gridSize_cm = grid.getGridSize_cm(); @@ -313,6 +310,7 @@ public: // does the grid contain the potential neighbor? const T* n2 = grid.getNodePtrFor(p); if (n2 != nullptr) { + if (isBlocked(floor, n1, *n2)) {continue;} // is there a (e.g. small) obstacle between the two? grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed! } @@ -479,7 +477,46 @@ private: } } + return false; + + } + + /** + * normally, the algorithm will not try to connect two nodes that are neighbors but have an obstacle between them. + * however, especially for 45° obstacles it might happen that a neighbor exists but is blocked by an obstacle. + * we thus need this additional check to ensure everything is fine... even though it needs performance... + */ + static inline bool isBlocked(const Floorplan::Floor* floor, const GridPoint& n1, const GridPoint& n2) { + + // (obstacles use meter, while the nodes are in centimeter! + const Point2 p1_m = n1.inMeter().xy(); + const Point2 p2_m = n2.inMeter().xy(); + const Line2 lineNodes(p1_m, p2_m); + + // process each obstacle + for (Floorplan::FloorObstacle* fo : floor->obstacles) { + + // depends on the type of obstacle + if (dynamic_cast(fo)) { + const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo; + const Line2 lineObstacle(line->from, line->to); + if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;} + + } else if (dynamic_cast(fo)) { + throw Exception("should not happen"); + + } else if (dynamic_cast(fo)) { + // DOORS ARE NOT AN OBSTACLE + + } else { + throw Exception("TODO: not yet implemented obstacle type"); + + } + + } + return false; + } /** does the bbox intersect with any of the floor's walls? */ diff --git a/grid/factory/v2/GridFactoryListener.h b/grid/factory/v2/GridFactoryListener.h new file mode 100644 index 0000000..0709c0a --- /dev/null +++ b/grid/factory/v2/GridFactoryListener.h @@ -0,0 +1,19 @@ +#ifndef GRIDFACTORYLISTENER_H +#define GRIDFACTORYLISTENER_H + +#include + +/** listen for events during the build process */ +class GridFactoryListener { + +public: + + virtual void onGridBuildUpdateMajor(const std::string& what) = 0; + virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0; + + virtual void onGridBuildUpdateMinor(const std::string& what) = 0; + virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0; + +}; + +#endif // GRIDFACTORYLISTENER_H diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 24d0704..3e706ef 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -1,8 +1,11 @@ #ifndef IMPORTANCE_H #define IMPORTANCE_H + + #include "../../../geo/Units.h" #include "../../Grid.h" +#include "GridFactoryListener.h" #include "../../../misc/KNN.h" #include "../../../misc/KNNArray.h" @@ -35,9 +38,14 @@ public: } /** attach importance-factors to the grid */ - template static void addImportance(Grid& g) { + template static void addImportance(Grid& g, GridFactoryListener* l = nullptr) { - Log::add(name, "adding importance information to all nodes");// at height " + std::to_string(z_cm)); + Log::add(name, "adding importance information to all nodes"); + if (l) { + l->onGridBuildUpdateMajor(2, 0); + l->onGridBuildUpdateMajor("adding importance information"); + l->onGridBuildUpdateMinor("performing initial setups"); + } // get an inverted version of the grid Grid inv(g.getGridSize_cm()); @@ -51,15 +59,13 @@ public: // construct KNN search KNN, 3> knn(inv); - // the number of neighbors to use - static constexpr int numNeighbors = 12; - // create list of all door-nodes std::vector doors; // create list of all stair-nodes std::vector stairs; + // process each node for (T& n1 : g) { @@ -83,9 +89,26 @@ public: Distribution::Normal favorDoors(0.0f, 0.5f); Distribution::Normal favorStairs(0.0f, 3.5f); + if (l) { + l->onGridBuildUpdateMajor(2, 1); + l->onGridBuildUpdateMinor("calculating importance for each node"); + } + + std::cout << "dunno why, but the KNN for stairs searches extremely slow!" << std::endl; // process each node again - for (T& n1 : g) { + for (int i = 0; i < g.getNumNodes(); ++i) { + + + // log + if (i % (g.getNumNodes() / 20) == 0) { + if (l) { + l->onGridBuildUpdateMinor(g.getNumNodes(), i); + } + } + + // get the node + T& n1 = g[i]; // get the distance to the nearest wall const float distToWall_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} )); @@ -101,12 +124,16 @@ public: // final probability n1.navImportance = 1.0f; n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f; - n1.navImportance += favorStairs.getProbability(distToStair_m) * 3.5f; + n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f; + // use wall avoidance if (useNormal) { - n1.navImportance -= avoidWalls.getProbability(distToWall_m); + n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f; } + // sanity check + Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!"); + } } diff --git a/grid/factory/v2/Stairs.h b/grid/factory/v2/Stairs.h index 9e139ae..882eb16 100644 --- a/grid/factory/v2/Stairs.h +++ b/grid/factory/v2/Stairs.h @@ -22,6 +22,9 @@ private: /** calculation helper */ Helper helper; + // keep a list of all vertices below stairwells and remove them hereafter + std::vector toDelete; + std::ofstream outStairs; std::ofstream outDelete; @@ -77,6 +80,7 @@ public: } ~Stairs() { + finalize(); outStairs.close(); outDelete.close(); } @@ -201,10 +205,6 @@ public: } - // keep a list of all vertices below stairwells and remove them hereafter - std::vector toDelete; - - // add all stair nodes or replace them with already existing ones for (Intermediate& iNode : stairNodes) { @@ -288,13 +288,23 @@ public: } } - // delete all pending nodes and perform a cleanup - for (T* n : toDelete) {grid.remove(*n);} - grid.cleanup(); + // finalize after ALL stairs were added. much faster and should be safe! + //finalize(); } + void finalize() { + + // delete all pending nodes and perform a cleanup + if (!toDelete.empty()) { + for (T* n : toDelete) {grid.remove(*n);} + toDelete.clear(); + grid.cleanup(); + } + + } + static float minZ(const std::vector& points) { auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;}; auto it = std::min_element(points.begin(), points.end(), comp); diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 2100abe..322f1ef 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -28,13 +28,15 @@ public: /** perform the walk based on the configured setup */ WalkState getDestination(Grid& grid, const WalkState& _startState, float dist_m) { - WalkState startState = _startState; - updateBefore(startState); - + // keep the starting state for reference + //const WalkState startState = _startState; + // the current state that is modified for each step + WalkState currentState = _startState; + updateBefore(currentState); // get the node that corresponds to start; - const Node* startNode = grid.getNodePtrFor(startState.startPos); + const Node* startNode = grid.getNodePtrFor(currentState.position); Assert::isNotNull(startNode, "failed to termine start-node for grid-walk"); // currently examined node @@ -47,7 +49,7 @@ public: // evaluate each neighbor for (const Node& neighbor : grid.neighbors(*curNode)) { - const double prob = getProbability(startState, *startNode, *curNode, neighbor); + const double prob = getProbability(currentState, *startNode, *curNode, neighbor); drawer.add(&neighbor, prob); } @@ -55,23 +57,21 @@ public: const Node* nextNode = drawer.get(); // inform - step(startState, *curNode, *nextNode); + step(currentState, *curNode, *nextNode); - // update + // update distance-to-walk and current position dist_m -= nextNode->getDistanceInMeter(*curNode); curNode = nextNode; + currentState.position = *curNode; + } - // output state - WalkState nextState = startState; - nextState.startPos = *curNode; - - // update - updateAfter(nextState, *startNode, *curNode); + // update after + updateAfter(currentState, *startNode, *curNode); // done - return nextState; + return currentState; } @@ -107,16 +107,18 @@ private: /** get the probability for the given random walk (one edge) */ inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const { - //double prob = 1.0; - double prob = 0; + double prob = 1.0; + //double prob = 0; for (const WalkModule* mdl : modules) { - //prob *= mdl->getProbability(state, start, cur, next); - prob += std::log( mdl->getProbability(state, start, cur, next) ); + const double subProb = mdl->getProbability(state, start, cur, next); + Assert::isTrue(subProb >= 0, "probability must not be negative!"); + prob *= subProb; + //prob += std::log( mdl->getProbability(state, start, cur, next) ); } - //return prob; - return std::exp(prob); + return prob; + //return std::exp(prob); } diff --git a/grid/walk/v2/modules/WalkModule.h b/grid/walk/v2/modules/WalkModule.h index e620da6..09403db 100644 --- a/grid/walk/v2/modules/WalkModule.h +++ b/grid/walk/v2/modules/WalkModule.h @@ -6,11 +6,11 @@ /** base-class for all WalkStates */ struct WalkState { - /** position where the walk starts */ - GridPoint startPos; + /** current position within the grid (-> in cm!) */ + GridPoint position; /** ctor */ - WalkState(const GridPoint& startPos) : startPos(startPos) {;} + explicit WalkState(const GridPoint& position) : position(position) {;} }; diff --git a/grid/walk/v2/modules/WalkModuleFavorZ.h b/grid/walk/v2/modules/WalkModuleFavorZ.h index 87e319a..e6207a3 100644 --- a/grid/walk/v2/modules/WalkModuleFavorZ.h +++ b/grid/walk/v2/modules/WalkModuleFavorZ.h @@ -41,7 +41,7 @@ public: (void) startNode; if (curNode.z_cm != potentialNode.z_cm) { - return 8; + return 40; } else { return 1; } diff --git a/grid/walk/v2/modules/WalkModuleHeading.h b/grid/walk/v2/modules/WalkModuleHeading.h index b0eeb38..b96bbc3 100644 --- a/grid/walk/v2/modules/WalkModuleHeading.h +++ b/grid/walk/v2/modules/WalkModuleHeading.h @@ -28,13 +28,16 @@ public: virtual void updateBefore(WalkState& state) override { - (void) state; - state.startHeading += draw.get(); + // add noise + state.heading.direction += draw.get(); } virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { + (void) state; + (void) startNode; + (void) endNode; // if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) { // Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm); // state.startHeading = head; @@ -60,7 +63,7 @@ public: const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm); // compare the heading against the state's heading - const Heading stateHead = state.startHeading; + const Heading stateHead = state.heading.direction; // get the difference const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD(); diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 8642121..772f267 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -13,11 +13,11 @@ template class WalkModuleH private: - /** van-Mises distribution */ + /** CURRENTLY NOT USED van-Mises distribution */ Distribution::LUT dist; - /** van-Mises draw list */ - DrawList draw; + /** random noise */ + Distribution::Normal distNoise; Control* ctrl; @@ -26,16 +26,25 @@ private: public: /** ctor 3.0 should be OK! */ - WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) { + WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) : + dist(Distribution::VonMises(0.0f, 2.0).getLUT()), + distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)), + ctrl(ctrl) { ; } virtual void updateBefore(WalkState& state) override { - const float var = draw.get() * 0.20;//0.05; - //const float var = 0; - state.startHeading += ctrl->turnAngle + var; + // NOTE: ctrl->turnAngle is cumulative SINCE the last transition! + // reset this one after every transition! + Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?"); + + // sensor noise + const float var = distNoise.draw(); + + // adjust the state's heading using the control-data + state.heading.direction += ctrl->turnAngle + var; } @@ -46,9 +55,24 @@ public: } virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { + (void) state; - (void) curNode; - (void) nextNode; + + // get the heading denoted by the way from curNode to nextNode + const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm); + + // get the heading requested by the state + const Heading stateHead = state.heading.direction; + + // get the error (signed difference) between both + const float angularDiff = stateHead.getSignedDiff(head); + + // adjust the error. + // note: the error may get > +/- 2PI but this is not an issue! + // when the error is added to the current heading within getProbability(), + // it is ensured their sum is within [0:2pi] + state.heading.error += angularDiff; + } @@ -60,11 +84,18 @@ public: const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm); // compare the heading against the state's heading - the last error - const Heading stateHead = state.startHeading; + const Heading stateHead = state.heading.direction + state.heading.error; // get the difference const float angularDiff = head.getDiffHalfRAD(stateHead); + if (angularDiff > Angle::degToRad(135)) {return 0.01;} + if (angularDiff > Angle::degToRad(90)) {return 0.02;} + if (angularDiff > Angle::degToRad(45)) {return 0.07;} + {return 0.90;} + +// add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE" + // determine probability const float prob = dist.getProbability(angularDiff); return prob; diff --git a/grid/walk/v2/modules/WalkModuleSpread.h b/grid/walk/v2/modules/WalkModuleSpread.h index 64dcd71..17450b4 100644 --- a/grid/walk/v2/modules/WalkModuleSpread.h +++ b/grid/walk/v2/modules/WalkModuleSpread.h @@ -24,7 +24,7 @@ public: virtual void updateBefore(WalkState& state) override { (void) state; - avg = avg * 0.999 + state.startPos.inMeter() * 0.001; + avg = avg * 0.999 + state.position.inMeter() * 0.001; } virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { diff --git a/grid/walk/v2/modules/WalkStateHeading.h b/grid/walk/v2/modules/WalkStateHeading.h index 0af5a94..bade121 100644 --- a/grid/walk/v2/modules/WalkStateHeading.h +++ b/grid/walk/v2/modules/WalkStateHeading.h @@ -3,12 +3,39 @@ #include "../../../../geo/Heading.h" +/** + * base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work + */ struct WalkStateHeading { - Heading startHeading; + /** used for better naming: heading.error instead of headingError */ + struct _Heading { + + /** + * the direction [0:2pi] the walk should move to + * e.g. indiciated by: + * compass + * integration over gyroscope values + */ + Heading direction; + + /** + * (cumulative) error between walked edges and requested direction (above). + * is used to ensure that (even though the grid contains only 45° edges) we + * approximately walk into the requested direction. + */ + float error = 0; + + /** ctor */ + _Heading(const Heading direction, const float error) : direction(direction), error(error) {;} + + } heading; + + + /** ctor */ - WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;} + explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;} }; diff --git a/math/DrawList.h b/math/DrawList.h index 9412dd2..67119c5 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -60,6 +60,7 @@ public: /** add a new user-element and its probability */ void add(T element, const double probability) { + Assert::isTrue(probability >= 0, "probability must not be negative!"); cumProbability += probability; elements.push_back(Entry(element, cumProbability)); } diff --git a/math/MovingAverageTS.h b/math/MovingAverageTS.h new file mode 100644 index 0000000..dd8b7bd --- /dev/null +++ b/math/MovingAverageTS.h @@ -0,0 +1,67 @@ +#ifndef MOVINGAVERAGETS_H +#define MOVINGAVERAGETS_H + +#include +#include "../data/Timestamp.h" +#include + +template class MovingAverageTS { + +private: + + /** timestamp -> value combination */ + struct Entry { + Timestamp ts; + T value; + Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;} + }; + + /** the regional window to use */ + Timestamp window; + + /** the value history for the window-size */ + std::vector history; + + /** current sum */ + T sum; + +public: + + + /** ctor with the window-size to use */ + MovingAverageTS(const Timestamp window, const T zeroElement) : window(window), sum(zeroElement) { + + } + + /** add a new entry */ + void add(const Timestamp ts, const T& data) { + + // append to history + history.push_back(Entry(ts, data)); + + // adjust sum + sum += data; + + // remove too-old history entries + const Timestamp oldest = ts - window; + while(history.front().ts < oldest) { + + // adjust sum + sum -= history.front().value; + + // remove from history + history.erase(history.begin()); + + } + + } + + /** get the current average */ + T get() const { + return sum / history.size(); + } + + +}; + +#endif // MOVINGAVERAGETS_H diff --git a/misc/KNN.h b/misc/KNN.h index 1d21af8..46aa88b 100644 --- a/misc/KNN.h +++ b/misc/KNN.h @@ -94,13 +94,21 @@ public: } /** get the distance to the element nearest to the given point */ - float getNearestDistance(const std::initializer_list lst) { + float getNearestDistance(const std::initializer_list lst) const { size_t idx; float distSquared; tree.knnSearch(lst.begin(), 1, &idx, &distSquared); return std::sqrt(distSquared); } + /** get the distance to the element nearest to the given point */ + float getNearestDistanceWithinRadius(const std::initializer_list lst, const float maxRadius) const { + std::vector> res; + tree.radiusSearch(lst.begin(), maxRadius*maxRadius, res, nanoflann::SearchParams()); + if (res.empty()) {return NAN;} + return std::sqrt(res[0].second); + } + void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) { // find k-nearest-neighbors diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index 08b8a79..5fedce6 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -18,6 +18,28 @@ struct AccelerometerData { return std::sqrt( x*x + y*y + z*z ); } + AccelerometerData& operator += (const AccelerometerData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + AccelerometerData& operator -= (const AccelerometerData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + AccelerometerData operator - (const AccelerometerData& o) const { + return AccelerometerData(x-o.x, y-o.y, z-o.z); + } + + AccelerometerData operator / (const float val) const { + return AccelerometerData(x/val, y/val, z/val); + } + }; #endif // ACCELEROMETERDATA_H diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 6202a4e..426d446 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -16,6 +16,7 @@ #include "../../Assertions.h" +#include "../../math/MovingAverageTS.h" /** @@ -27,85 +28,155 @@ class StepDetection { private: - /** low pass acc-magnitude */ - float avg1 = 0; + MovingAverageTS avgLong; + MovingAverageTS avgShort; - /** even-more low-pass acc-magnitude */ - float avg2 = 0; + Timestamp blockUntil; + bool waitForUp = false; -private: - - class Stepper { - - private: - - /** block for 300 ms after every step */ - const Timestamp blockTime = Timestamp::fromMS(300); - - /** the threshold for detecting a spike as step */ - const float threshold = 0.17; - - /** block until the given timestamp before detecting additional steps */ - Timestamp blockUntil; - - - public: - - /** is the given (relative!) magnitude (mag - ~9.81) a step? */ - bool isStep(const Timestamp ts, const float mag) { - - // still blocking - if (ts < blockUntil) { - return false; - } - - // threshold reached? -> step! - if (mag > threshold) { - - // block x milliseconds until detecting the next step - blockUntil = ts + blockTime; - - // we have a step - return true; - - } - - // no step - return false; - - } - - - }; - - Stepper stepper; + const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good + const float upperThreshold = +0.4f; // + is usually smaller than down (look at graphs) + const float lowerThreshold = -0.8f; public: + /** ctor */ + StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { + ; + } + /** does the given data indicate a step? */ - bool isStep(const Timestamp ts, const AccelerometerData& acc) { + bool add(const Timestamp ts, const AccelerometerData& acc) { - avg1 = avg1 * 0.91 + acc.magnitude() * 0.09; // short-time average [filtered steps] - avg2 = avg2 * 0.97 + acc.magnitude() * 0.03; // long-time average [gravity] + // update averages + avgLong.add(ts, acc.magnitude()); + avgShort.add(ts, acc.magnitude()); - // average maginitude must be > 9.0 to be stable enough to proceed - if (avg2 > 9) { + // difference between long-term-average (gravity) and very-short-time average + const float delta = avgShort.get() - avgLong.get(); - // gravity-free magnitude - const float avg = avg1 - avg2; + bool step = false; - // detect steps - return stepper.isStep(ts, avg); - } else { - - return false; + if (blockUntil > ts) {return false;} + // wait for a rising edge + if (waitForUp && delta > upperThreshold) { + blockUntil = ts + blockTime; // block some time + waitForUp = false; } + // wait for a falling edge + if (!waitForUp && delta < lowerThreshold) { + blockUntil = ts + blockTime; // block some time + waitForUp = true; + step = true; + } + +// static K::Gnuplot gp; +// static K::GnuplotPlot plot; +// static K::GnuplotPlotElementLines lines1; plot.add(&lines1); +// static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff"); +// static Timestamp ref = ts; + +// static int i = 0; + +// //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); +// lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) ); + +// if (++i % 100 == 0) { +// gp.draw(plot); +// gp.flush(); +// usleep(1000*25); +// } + + return step; + + } +//private: + +// /** low pass acc-magnitude */ +// float avg1 = 0; + +// /** even-more low-pass acc-magnitude */ +// float avg2 = 0; + +//private: + +// class Stepper { + +// private: + +// /** block for 300 ms after every step */ +// const Timestamp blockTime = Timestamp::fromMS(300); + +// /** the threshold for detecting a spike as step */ +// const float threshold = 0.30; + +// /** block until the given timestamp before detecting additional steps */ +// Timestamp blockUntil; + + +// public: + +// /** is the given (relative!) magnitude (mag - ~9.81) a step? */ +// bool isStep(const Timestamp ts, const float mag) { + +// // still blocking +// if (ts < blockUntil) { +// return false; +// } + +// // threshold reached? -> step! +// if (mag > threshold) { + +// // block x milliseconds until detecting the next step +// blockUntil = ts + blockTime; + +// // we have a step +// return true; + +// } + +// // no step +// return false; + +// } + + +// }; + +// Stepper stepper; + +//public: + +// /** does the given data indicate a step? */ +// bool add(const Timestamp ts, const AccelerometerData& acc) { + +// avg1 = avg1 * 0.91 + acc.magnitude() * 0.09; // short-time average [filtered steps] +// avg2 = avg2 * 0.97 + acc.magnitude() * 0.03; // long-time average [gravity] + +// // average maginitude must be > 9.0 to be stable enough to proceed +// if (avg2 > 9) { + +// // gravity-free magnitude +// const float avg = avg1 - avg2; + +// // detect steps +// return stepper.isStep(ts, avg); + +// } else { + +// return false; + +// } + +// } + + }; diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 5adf703..67473ff 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.h @@ -4,6 +4,7 @@ #include "GyroscopeData.h" #include "AccelerometerData.h" #include "../../data/Timestamp.h" +#include "../../math/MovingAverageTS.h" #include @@ -23,147 +24,100 @@ class TurnDetection { private: - //std::vector accData; - std::vector gyroData; - Timestamp lastGyro; - Timestamp lastRotMatEst; + //std::vector gyroData; + Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); + + Timestamp lastGyroReading; + + + struct { + Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); + bool isKnown = false; + Timestamp lastEstimation; + } orientation; + - Eigen::Matrix3f rotMat; - Eigen::Vector3f avgAcc; - Eigen::Vector3f leGyro; public: /** ctor */ - TurnDetection() : rotMat(Eigen::Matrix3f::Identity()) { - + TurnDetection() { + ; } + + // does not seem to help... +// struct DriftEstimator { + + +// MovingAverageTS avg; + +// DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) { +// ; +// } + +// void removeDrift(const Timestamp ts, Eigen::Vector3f& gyro) { + +// if (gyro.norm() < 0.15) { +// avg.add(ts, gyro); +// gyro -= avg.get(); +// } + +// } + +// } driftEst; + + + + + float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { - if (lastGyro.isZero()) {lastGyro = ts;} + + // ignore the first reading completely, just remember its timestamp + if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;} + + // time-difference between previous and current reading + const Timestamp curDiff = ts - lastGyroReading; + lastGyroReading = ts; + + // fast sensors might lead to delay = 0 ms. filter those values + if (curDiff.isZero()) {return 0.0f;} + + // ignore readings until the first orientation-estimation is available + // otherwise we would use a wrong rotation matrix which yields wrong results! + if (!orientation.isKnown) {return 0.0f;} - // TESTING! - gyroData.push_back(gyro); - static Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity(); - if (gyroData.size() > 25) { - Eigen::Vector3f sum = Eigen::Vector3f::Zero(); - int cnt = 0; - for (GyroscopeData gd : gyroData) { - //if (gd.z > gd.x && gd.z > gd.y) { - Eigen::Vector3f vec; vec << (gd.x), (gd.y), (gd.z); - sum += vec; - ++cnt; - //} - } - gyroData.clear(); - if (cnt > 10) { - Eigen::Vector3f z; z << 0,0, sum(2) < 0 ? -1 : +1; - rotMat = getRotationMatrix(sum.normalized(), z.normalized()); - } - } - - // current gyro-reading as vector + // get the current gyro-reading as vector Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z; - leGyro = vec; - // current value, rotated into the new coordinate system - Eigen::Vector3f curVec = rotMat * vec; - - // previous value - static Eigen::Vector3f oldVec = curVec; - - // time-difference between previous and current value - const Timestamp diff = ts - lastGyro; - lastGyro = ts; + // rotate it into our desired coordinate system, where the smartphone lies flat on the ground + Eigen::Vector3f curGyro = orientation.rotationMatrix * vec; + //driftEst.removeDrift(ts, curGyro); // area - Eigen::Vector3f area = Eigen::Vector3f::Zero(); - if (!diff.isZero()) { - area = (oldVec * diff.sec()) + // squared region - ((curVec - oldVec) * 0.5 * diff.sec()); // triangle region to the next (enhances the quality) - } + const Eigen::Vector3f area = + + // Trapezoid rule (should be more accurate but does not always help?!) + //(prevGyro * curDiff.sec()) + // squared region + //((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality) + + // just the rectangular region + (prevGyro * curDiff.sec()); // BEST?! + + + //} // update the old value - oldVec = curVec; + prevGyro = curGyro; - const float delta = area(2);// * 0.8; - - - - - static int i = 0; ++i; - if (i % 50 == 0) { - - static K::Gnuplot gp; - - gp << "set view equal xyz\n"; - gp << "set xrange[-1:+1]\n"; - gp << "set yrange[-1:+1]\n"; - gp << "set zrange[-1:+1]\n"; - - K::GnuplotSplot plot; - K::GnuplotSplotElementLines lines; plot.add(&lines); - - K::GnuplotPoint3 p0(0,0,0); - //K::GnuplotPoint3 pO(vec(0), vec(1), vec(2)); - - K::GnuplotPoint3 px(rotMat(0,0), rotMat(1,0), rotMat(2,0)); //px = px * eval(0); - K::GnuplotPoint3 py(rotMat(0,1), rotMat(1,1), rotMat(2,1)); //py = py * eval(1); - K::GnuplotPoint3 pz(rotMat(0,2), rotMat(1,2), rotMat(2,2)); //pz = pz * eval(2); - - lines.addSegment(p0, px*0.15); - lines.addSegment(p0, py*0.4); - lines.addSegment(p0, pz*1.0); - - Eigen::Vector3f ori = leGyro; - Eigen::Vector3f re = rotMat * leGyro; - Eigen::Vector3f avg = est.lastAvg * 0.3; - - gp << "set arrow 1 from 0,0,0 to " << avg(0) << "," << avg(1) << "," << avg(2) << " lw 2\n"; - - gp << "set arrow 2 from 0,0,0 to " << ori(0) << "," << ori(1) << "," << ori(2) << " lw 1 dashtype 2 \n"; - gp << "set arrow 3 from 0,0,0 to " << re(0) << "," << re(1) << "," << re(2) << " lw 1\n"; - -// gp << "set arrow 2 from 0,0,0 to " << vec(0) << "," << vec(1) << "," << vec(2) << "\n"; -// gp << "set arrow 3 from 0,0,0 to " << nVec(0) << "," << nVec(1) << "," << nVec(2) << "\n"; - - gp.draw(plot); - //gp.flush(); - - } - - - - static Eigen::Vector3f sum = Eigen::Vector3f::Zero(); - sum += area; - - if (i % 30 == 0) { - - static int idx = 0; - static K::Gnuplot gp2; - gp2 << "set arrow 1 from 0,0 to 10000,0\n"; - gp2 << "set arrow 2 from 0,5 to 10000,5\n"; - gp2 << "set arrow 3 from 0,10 to 10000,10\n"; - - K::GnuplotPlot plot2; - static K::GnuplotPlotElementLines linesX; plot2.add(&linesX); - static K::GnuplotPlotElementLines linesY; plot2.add(&linesY); - static K::GnuplotPlotElementLines linesZ; plot2.add(&linesZ); - - //linesX.add(K::GnuplotPoint2(idx, sum(0) + 0)); - //linesY.add(K::GnuplotPoint2(idx, sum(1) + 5)); - linesZ.add(K::GnuplotPoint2(idx, sum(2) + 10)); - ++idx; - - gp2.draw(plot2); - //gp2.flush(); - - } + // rotation = z-axis only! + const float delta = area(2); + // done return delta; } @@ -174,15 +128,17 @@ public: // add accelerometer data //pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z)); - est.add((acc.x), (acc.y), (acc.z)); + est.addAcc(acc); // start with the first available timestamp - if (lastRotMatEst.isZero()) {lastRotMatEst = ts;} + if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;} - // if we have at-least 1 sec of acc-data, re-calculate the current smartphone holding - if (ts - lastRotMatEst > Timestamp::fromMS(500)) { - rotMat = est.get(); - lastRotMatEst = ts; + // if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding + if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) { + orientation.rotationMatrix = est.get(); + orientation.isKnown = true; + orientation.lastEstimation = ts; + est.reset(); } } @@ -243,44 +199,49 @@ public: struct XYZ { - Eigen::Vector3f lastAvg; - Eigen::Vector3f avg; + Eigen::Vector3f sum; int cnt; XYZ() { reset(); } - void add(const float x, const float y, const float z) { + /** add the given accelerometer reading */ + void addAcc(const AccelerometerData& acc) { - Eigen::Vector3f vec; vec << x,y,z; - avg += vec; + // did NOT improve the result for every smartphone (only some) + //const float deltaMag = std::abs(acc.magnitude() - 9.81); + //if (deltaMag > 5.0) {return;} + + // adjust sum and count (for average calculation) + Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z; + sum += vec; ++cnt; } - Eigen::Matrix3f get() { + /** get the current rotation matrix estimation */ + Eigen::Matrix3f get() const { - avg/= cnt; - lastAvg = avg; + // get the current acceleromter average + const Eigen::Vector3f avg = sum / cnt; // rotate average accelerometer into (0,0,1) Eigen::Vector3f zAxis; zAxis << 0, 0, 1; const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); - // sanity check + // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) Eigen::Vector3f aligned = (rotMat * avg).normalized(); Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); - reset(); - return rotMat; } + /** reset the current sum etc. */ void reset() { cnt = 0; - avg = Eigen::Vector3f::Zero(); + sum = Eigen::Vector3f::Zero(); } diff --git a/sensors/offline/OfflineAndroid.h b/sensors/offline/OfflineAndroid.h index a4faa3e..5aa0b30 100644 --- a/sensors/offline/OfflineAndroid.h +++ b/sensors/offline/OfflineAndroid.h @@ -4,11 +4,14 @@ #include #include +#include "../../misc/Debug.h" #include "../../Assertions.h" #include "../../math/Interpolator.h" #include "../../geo/Point3.h" #include "../../data/Timestamp.h" #include "../radio/WiFiMeasurements.h" +#include "../imu/AccelerometerData.h" +#include "../imu/GyroscopeData.h" template struct OfflineEntry { @@ -31,6 +34,15 @@ struct WalkedPath { }; +/** listener for event callbacks */ +class OfflineAndroidListener { +public: + virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0; + virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0; + virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0; + virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0; +}; + /** read recorded android sensor data files */ class OfflineAndroid { @@ -39,13 +51,20 @@ private: std::vector> wifi; std::vector> groundTruth; + std::vector> gyro; + + std::vector> accel; + std::vector> gravity; + WalkedPath walkedPath; + const char* name = "OfflineData"; + public: /** ctor */ - OfflineAndroid(const std::string& file) { - parse(file); + OfflineAndroid() { + ; } /** get all ground truth readings */ @@ -54,6 +73,16 @@ public: /** get all WiFi readings */ const std::vector>& getWiFi() const {return wifi;} + /** get all gyroscope readings */ + const std::vector>& getGyroscope() const {return gyro;} + + /** get all accelerometer readings */ + const std::vector>& getAccelerometer() const {return accel;} + + /** get all gravity readings */ + const std::vector>& getGravity() const {return gravity;} + + /** get the walked path */ const WalkedPath& getWalkedPath() const {return walkedPath;} @@ -67,10 +96,12 @@ public: return interpol; } +public: -private: + void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) { - void parse(const std::string& file) { + Log::add(name, "parsing data file: " + file , false); + Log::tick(); // open the stream std::ifstream inp(file); @@ -96,28 +127,64 @@ private: if (delim == 0) {break;} - parse(Timestamp::fromMS(ts), sensorID, sensorData); - + parse(Timestamp::fromMS(ts), sensorID, sensorData, listener); } + Log::tock(); + + Log::add(name, + "gyro(" + std::to_string(gyro.size()) + ") " + + "accel(" + std::to_string(accel.size()) + ") " + "wifi(" + std::to_string(wifi.size()) + ") " + + "gt(" + std::to_string(groundTruth.size()) + ") " + ); + } +private: + /** parse the given data */ - void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData) { + void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) { // how to parse switch(sensorID) { + + case 0: { + const AccelerometerData data = parseAccelerometer(sensorData); + accel.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onAccelerometer(ts, data);} + break; + } + + case 1: { + const AccelerometerData data = parseAccelerometer(sensorData); + gravity.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onGravity(ts, data);} + break; + } + + case 3: { + const GyroscopeData data = parseGyroscope(sensorData); + gyro.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onGyroscope(ts, data);} + break; + } + case 8: { - OfflineEntry entry(ts, parseWiFi(sensorData)); - wifi.push_back(entry); + const WiFiMeasurements data = parseWiFi(sensorData); + wifi.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onWiFi(ts, data);} break; } + case 99: { - OfflineEntry entry (ts, parseGroundTruthTick(sensorData)); - groundTruth.push_back(entry); + const GroundTruthID data = parseGroundTruthTick(sensorData); + groundTruth.push_back(OfflineEntry(ts, data)); + // TODO listener break; } + case 100: { walkedPath = parseWalkedPath(sensorData); break; @@ -138,16 +205,19 @@ private: const std::string mac = data.substr(0, 17); data = data.substr(17); - assert(data[0] == ';'); data = data.substr(1); + Assert::isTrue(data[0] == ';', "unexpected character"); + data = data.substr(1); const std::string freq = data.substr(0, 4); data = data.substr(4); - assert(data[0] == ';'); data = data.substr(1); + Assert::isTrue(data[0] == ';', "unexpected character"); + data = data.substr(1); const int pos = data.find(';'); const std::string rssi = data.substr(0, pos); data = data.substr(pos); - assert(data[0] == ';'); data = data.substr(1); + Assert::isTrue(data[0] == ';', "unexpected character"); + data = data.substr(1); const WiFiMeasurement e(mac, std::stof(rssi)); obs.entries.push_back(e); @@ -158,6 +228,43 @@ private: } + static inline GyroscopeData parseGyroscope(const std::string& data) { + + const size_t pos1 = data.find(';', 0); + const size_t pos2 = data.find(';', pos1+1); + const size_t pos3 = data.find(';', pos2+1); + + Assert::isTrue(pos1 != std::string::npos, "format error"); + Assert::isTrue(pos2 != std::string::npos, "format error"); + Assert::isTrue(pos3 != std::string::npos, "format error"); + + const std::string sx = data.substr(0, pos1); + const std::string sy = data.substr(pos1+1, pos2-pos1-1); + const std::string sz = data.substr(pos2+1, pos3-pos2-1); + + return GyroscopeData(std::stof(sx), std::stof(sy), std::stof(sz)); + + } + + + static inline AccelerometerData parseAccelerometer(const std::string& data) { + + const size_t pos1 = data.find(';', 0); + const size_t pos2 = data.find(';', pos1+1); + const size_t pos3 = data.find(';', pos2+1); + + Assert::isTrue(pos1 != std::string::npos, "format error"); + Assert::isTrue(pos2 != std::string::npos, "format error"); + Assert::isTrue(pos3 != std::string::npos, "format error"); + + const std::string sx = data.substr(0, pos1); + const std::string sy = data.substr(pos1+1, pos2-pos1-1); + const std::string sz = data.substr(pos2+1, pos3-pos2-1); + + return AccelerometerData(std::stof(sx), std::stof(sy), std::stof(sz)); + + } + /** parse the given GroundTruth entry */ static inline GroundTruthID parseGroundTruthTick(const std::string& data) { diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index 5d31761..2493da8 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -16,6 +16,9 @@ public: /** the mode denotes the algorithm that is used for grouping VAPs together */ enum class Mode { + /** do NOT group */ + DISABLED, + /** group VAPs by setting the MAC's last digit to zero */ LAST_MAC_DIGIT_TO_ZERO, @@ -91,6 +94,7 @@ public: MACAddress getBaseMAC(const MACAddress& mac) const { switch(mode) { + case Mode::DISABLED: return mac; case Mode::LAST_MAC_DIGIT_TO_ZERO: return lastMacDigitToZero(mac); default: throw Exception("unsupported vap-grouping mode given"); } diff --git a/tests/data/WalkHeadingMap.xml b/tests/data/WalkHeadingMap.xml new file mode 100644 index 0000000..bd8c9cc --- /dev/null +++ b/tests/data/WalkHeadingMap.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/grid/Plot.h b/tests/grid/Plot.h index 7bb9cbe..02877ca 100644 --- a/tests/grid/Plot.h +++ b/tests/grid/Plot.h @@ -43,6 +43,9 @@ public: gp << "set cbrange[0.5:1.5]\n"; gp << "set palette gray negative\n"; + gp << "set xlabel 'x'\n"; + gp << "set ylabel 'y'\n"; + //gp << "set hidden3d front\n"; splot.add(&nodes); nodes.setPointSize(0.5); @@ -72,13 +75,14 @@ public: template Plot& addEdges(Grid& g) { // prevent adding edges twice - std::set done; + std::set done; for (T& n1 : g) { for (const T& n2 : g.neighbors(n1)) { const size_t uid1 = g.getUID(n1); const size_t uid2 = g.getUID(n2); - size_t edge = uid1+uid2;//std::max(uid1,uid2) << 32 | std::min(uid1,uid2); + //size_t edge = uid1+uid2; + std::string edge = std::to_string(std::max(uid1,uid2)) + std::to_string(std::min(uid1,uid2)); if (done.find(edge) == done.end()) { K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); diff --git a/tests/grid/TestGridWalk2HeadingControl.cpp b/tests/grid/TestGridWalk2HeadingControl.cpp new file mode 100644 index 0000000..4f58ffe --- /dev/null +++ b/tests/grid/TestGridWalk2HeadingControl.cpp @@ -0,0 +1,177 @@ +#ifdef WITH_TESTS + +#include + +#include "../Tests.h" + +#include "Plot.h" + +#include "../../floorplan/v2/FloorplanReader.h" + +#include "../../grid/factory/v2/GridFactory.h" +#include "../../grid/factory/v2/GridNodeImportance.h" + +#include "../../grid/walk/v2/GridWalker.h" +#include "../../grid/walk/v2/modules/WalkModuleHeading.h" +#include "../../grid/walk/v2/modules/WalkModuleHeadingControl.h" +#include "../../grid/walk/v2/modules/WalkModuleFollowDestination.h" +#include "../../grid/walk/v2/modules/WalkModuleRelativePressureControl.h" + +#include "../../sensors/radio/WiFiGridNode.h" + + +#include +#include +#include +#include + +#include +#include + + +struct MyNode12456012 : public GridPoint, public GridNode { + MyNode12456012() {;} + MyNode12456012(const int x, const int y, const int z) : GridPoint(x,y,z) {;} +}; + +struct MyState0012345 : public WalkState, public WalkStateHeading { + MyState0012345(const GridPoint& pos, const Heading& heading, const float headingError) : WalkState(pos), WalkStateHeading(heading, headingError) {;} +}; + + +TEST(GridWalk2HeadingControl, Heading) { + + + const Heading head0(0, 0, 0, +20); + const Heading head1(0, 0, 20, +20); + const Heading head2(0, 0, 20, 0); + const Heading head3(0, 0, 20, -20); + const Heading head4(0, 0, 0, -20); + + const Heading baseHead = 0; + + const float diff0 = head0.getDiffHalfRAD(baseHead); + const float diff1 = head1.getDiffHalfRAD(baseHead); + const float diff2 = head2.getDiffHalfRAD(baseHead); + const float diff3 = head3.getDiffHalfRAD(baseHead); + const float diff4 = head4.getDiffHalfRAD(baseHead); + + const float d = 0.0001; + + ASSERT_NEAR(0, diff2, d); + ASSERT_NEAR(diff1, diff3, d); + ASSERT_NEAR(diff0, diff4, d); + +} + +TEST(GridWalk2HeadingControl, LIVE_walkHeading) { + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("WalkHeadingMap.xml")); + + Grid grid(25); + GridFactory fac(grid); + fac.build(map); + + const int sxy = 2000; + +// int sxy = 2000; + +// for (int x = 0; x < sxy; x += 20) { +// for (int y = 0; y < sxy; y+= 20) { +// grid.add(MyNode12456012(x,y,0)); +// } +// } + +// for (int x = 0; x < sxy; x += 20) { +// for (int y = 0; y < sxy; y+= 20) { + +// MyNode12456012* n1 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x,y,0)); + +// // connect horizontally +// for (int dx = -20; dx <= +20; dx += 20) { +// for (int dy = -20; dy <= +20; dy += 20) { + +// if (dx == 0 && dy == 0) {continue;} +// MyNode12456012* n2 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,0)); +// if (n2) { grid.connectUniDir(*n1, *n2); } + +// } + +// } + +// } +// } + + + struct MyControl { + float turnAngle = 0; + } ctrl; + + + // one particle + struct Particle { + GridPoint pos; + Heading head; + float headErr = 0; + Particle(const GridPoint pos, const Heading head) : pos(pos), head(head) {;} + }; + + GridWalker walker; + WalkModuleHeadingControl modPres(&ctrl, 0.0); + walker.addModule(&modPres); + + Plot p; + //p.addEdges(grid); + p.addNodes(grid); + + // noisy step size + std::minstd_rand gen; + std::normal_distribution dist(0.75, 0.10); + + for (float rad = 0; rad < 6*M_PI; rad += M_PI*0.125) { + + // setup particles + std::vector particles; + for (int i = 0; i < 75; ++i) { + particles.push_back( Particle(GridPoint(sxy/2, sxy/2, 0), 0.0f) ); + } + + // run + for (int i = 0; i < 30; ++i) { + + ctrl.turnAngle = (i == 0) ? (rad) : (0); + + p.clearParticles(); + + for (Particle& particle : particles) { + + // particle -> state + MyState0012345 state(particle.pos, particle.head, particle.headErr); + + // process state + const float dist_m = dist(gen); + state = walker.getDestination(grid, state, dist_m); + + // state -> particle + particle.head = state.heading.direction; + particle.pos = state.position; + particle.headErr = state.heading.error; + + p.addParticle(Point3(particle.pos.x_cm, particle.pos.y_cm, particle.pos.z_cm)); + + } + + Point2 p1(sxy/2, sxy/2); + Point2 p2 = p1 + (Point2(1,0).rotated(rad) * sxy/2); + p.gp << "set arrow 1 from " << p1.x << "," << p1.y << "," << 0 << " to " << p2.x << "," << p2.y << "," << 0 << " front \n"; + + p.fire(); + usleep(1000*33); + + } + + } + +} + +#endif diff --git a/tests/grid/GridWalk2RelPressure.cpp b/tests/grid/TestGridWalk2RelPressure.cpp similarity index 70% rename from tests/grid/GridWalk2RelPressure.cpp rename to tests/grid/TestGridWalk2RelPressure.cpp index 6e6808a..72f1cf5 100644 --- a/tests/grid/GridWalk2RelPressure.cpp +++ b/tests/grid/TestGridWalk2RelPressure.cpp @@ -27,13 +27,13 @@ #include -struct MyNode : public GridPoint, public GridNode { - MyNode() {;} - MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;} +struct MyNode124234 : public GridPoint, public GridNode { + MyNode124234() {;} + MyNode124234(const int x, const int y, const int z) : GridPoint(x,y,z) {;} }; -struct MyState : public WalkState, public WalkStateRelativePressure { - MyState(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;} +struct MyState316123 : public WalkState, public WalkStateRelativePressure { + MyState316123(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;} }; @@ -41,7 +41,7 @@ struct MyState : public WalkState, public WalkStateRelativePressure { TEST(GridWalk2RelPressure, LIVE_walkHeading) { - Grid grid(20); + Grid grid(20); int sxy = 300; int sz = 600; @@ -49,7 +49,7 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) { for (int x = 0; x < sxy; x += 40) { for (int y = 0; y < sxy; y+= 40) { for (int z = 0; z < sz; z += 20) { - grid.add(MyNode(x,y,z)); + grid.add(MyNode124234(x,y,z)); } } } @@ -60,8 +60,8 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) { // connect vertically for (int dz = -20; dz <= +20; dz += 40) { - MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z)); - MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z+dz)); + MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z)); + MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z+dz)); if (n1&&n2) { grid.connectUniDir(*n1, *n2); } @@ -71,8 +71,8 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) { for (int dx = -40; dx <= +40; dx += 40) { for (int dy = -40; dy <= +40; dy += 40) { if (dx == 0 && dy == 0) {continue;} - MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z)); - MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z)); + MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z)); + MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z)); if (n1&&n2) { grid.connectUniDir(*n1, *n2); } @@ -92,11 +92,11 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) { } ctrl; GridPoint start(120,120,sz/2); - const MyNode* n = grid.getNodePtrFor(start); + const MyNode124234* n = grid.getNodePtrFor(start); - MyState ms(start, 0); - GridWalker walker; - WalkModuleRelativePressureControl modPres(&ctrl, 0.1); + MyState316123 ms(start, 0); + GridWalker walker; + WalkModuleRelativePressureControl modPres(&ctrl, 0.1); walker.addModule(&modPres); Plot p; @@ -109,7 +109,7 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) { ctrl.barometer.hPaRelativeToT0 = (std::sin(i/25.0)) * 0.3; ms = walker.getDestination(grid, ms, 0.3); - p.addParticle(ms.startPos.inCentimeter()); + p.addParticle(ms.position.inCentimeter()); usleep(1000*50); p.gp << "set label 97 at screen 0.05, 0.95 ' baro: " << ctrl.barometer.hPaRelativeToT0 << "'\n"; diff --git a/tests/grid/TestGridWalkV2.cpp b/tests/grid/TestGridWalkV2.cpp index bcbbb28..822e368 100644 --- a/tests/grid/TestGridWalkV2.cpp +++ b/tests/grid/TestGridWalkV2.cpp @@ -39,7 +39,7 @@ struct MyNode1239 : public GridPoint, public GridNode, public GridNodeImportance // ENSURE UNIQUE CLASS NAME struct MyState23452 : public WalkState, public WalkStateHeading { - MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head) {;} + MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head, 0) {;} }; @@ -64,7 +64,7 @@ TEST(GridWalk2, LIVE_error) { } ctrl; GridWalker walker; - WalkModuleHeadingControl modHead(&ctrl); + WalkModuleHeadingControl modHead(&ctrl, 3.0f); walker.addModule(&modHead); @@ -81,9 +81,9 @@ TEST(GridWalk2, LIVE_error) { for (int j = 0; j < 100; ++j) { state = walker.getDestination(grid, state, 0.4); - gp << "set label 1 at screen 0.5,0.5 '" << state.startHeading.getRAD() << "'\n"; + gp << "set label 1 at screen 0.5,0.5 '" << state.heading.direction.getRAD() << "'\n"; - lines.add(K::GnuplotPoint2(state.startPos.x_cm, state.startPos.y_cm)); + lines.add(K::GnuplotPoint2(state.position.x_cm, state.position.y_cm)); gp.draw(plot); gp.flush(); @@ -147,7 +147,7 @@ TEST(GgridWalk2, LIVE_walkHeading) { for (int i = 0; i < points.size(); ++i) { MyState23452 start(points[i], Heading(0.0)); MyState23452 next = walker.getDestination(grid, start, 2.0); - points[i] = next.startPos; + points[i] = next.position; states.add(K::GnuplotPoint3(points[i].x_cm, points[i].y_cm, points[i].z_cm)); }