From 51c0945e125e5401cfd04cc18f5a9ca282c9f224 Mon Sep 17 00:00:00 2001 From: kazu Date: Sat, 1 Oct 2016 13:17:14 +0200 Subject: [PATCH] statistic helper classes test-cases modified grid importance for better trap-detection --- grid/factory/v2/GridNodeImportance.h | 6 ++ grid/factory/v2/Importance.h | 26 +++--- grid/walk/v2/GridWalker.h | 26 ++++-- math/DrawList.h | 9 +++ math/Median.h | 43 ---------- math/Stats.h | 9 +++ math/stats/Average.h | 43 ++++++++++ math/stats/Maximum.h | 42 ++++++++++ math/stats/Median.h | 52 ++++++++++++ math/stats/Minimum.h | 40 ++++++++++ sensors/pressure/PressureTendence.h | 2 +- sensors/radio/VAPGrouper.h | 15 ++-- tests/grid/TestGridWalk2HeadingControl.cpp | 1 + tests/grid/TestGridWalk2RelPressure.cpp | 1 + tests/math/TestStats.cpp | 92 ++++++++++++++++++++++ 15 files changed, 343 insertions(+), 64 deletions(-) delete mode 100644 math/Median.h create mode 100644 math/Stats.h create mode 100644 math/stats/Average.h create mode 100644 math/stats/Maximum.h create mode 100644 math/stats/Median.h create mode 100644 math/stats/Minimum.h create mode 100644 tests/math/TestStats.cpp diff --git a/grid/factory/v2/GridNodeImportance.h b/grid/factory/v2/GridNodeImportance.h index 88b09c3..44142b9 100644 --- a/grid/factory/v2/GridNodeImportance.h +++ b/grid/factory/v2/GridNodeImportance.h @@ -10,6 +10,12 @@ struct GridNodeImportance { /** get the node's nav importance */ float getNavImportance() const {return navImportance;} + /** importance-weight for random walks */ + float walkImportance; + + /** get the node's random-walk importance */ + float getWalkImportance() const {return walkImportance;} + /** ctor */ GridNodeImportance() : navImportance(1.0f) {;} diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 6ee6f4c..a024781 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -86,7 +86,7 @@ public: KNN>, 3> knnStairs(knnArrStairs); // probability adjustments - Distribution::Triangle avoidWalls(0.0, 0.45f); + Distribution::Triangle avoidWalls(0.0, 0.35f); Distribution::Normal favorDoors(0.0f, 0.4f); Distribution::Normal favorStairs(0.0f, 1.5f); @@ -111,8 +111,8 @@ public: // 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} )); + // get the distance to the nearest outline node [an outline node is directly adjacent to a wall] + const float distToOutline_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} )); // get the distance to the nearest door const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} )); @@ -120,7 +120,13 @@ public: // get the distance to the nearest stair const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} )); - const bool useNormal = (distToWall_m*3.5 < distToDoor_m && distToWall_m*3.5 < distToStair_m); + // use wall-avoidance? + //const bool useWallAvoidance = (distToWall_m*6.0 < distToDoor_m && distToWall_m*6.0 < distToStair_m); + //const bool useWallAvoidance = (distToDoor_m > 0.4f) && (distToStair_m > 0.4f); + const bool useWallAvoidance = + (distToOutline_m < 0.001f) && // node is an outline node [outline-nodes are adjacent to a wall] + (distToDoor_m > 0.3f) && // doors are 30cm away + (distToStair_m > 0.6f); // stairs are 60cm away; // final probability n1.walkImportance = 1.0f; @@ -128,18 +134,20 @@ public: n1.walkImportance += favorStairs.getProbability(distToStair_m) * 1.0f; // use wall avoidance - if (useNormal) { - n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.4f; + if (useWallAvoidance) { + //n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.3f; + n1.walkImportance = 0.20f; // only addresses direct outline nodes } // navigation importance is calculated using other formulae n1.navImportance = 1.0f; - if (useNormal) { - n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.4; + if (useWallAvoidance) { + n1.navImportance -= avoidWalls.getProbability(distToOutline_m) * 0.3f; + } //n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5; - n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0; + n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0f; diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index f25bc68..1fe88b7 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -6,6 +6,7 @@ #include "../../../math/DrawList.h" #include "modules/WalkModule.h" #include "../../../math/Distributions.h" +#include "../../../math/Stats.h" /** * modular grid-walker that takes various sub-components to determine @@ -28,6 +29,16 @@ public: } /** perform the walk based on the configured setup */ + WalkState getDestination(Grid& grid, const WalkState& _startState, const float _dist_m) { + double tmp; // ignored + return getDestination(grid, _startState, _dist_m, tmp); + } + + + /** + * perform the walk based on the configured setup + * returns the walks overall probability using the out-parameter + */ WalkState getDestination(Grid& grid, const WalkState& _startState, const float _dist_m, double& probability) { Assert::isTrue(_dist_m >= 0, "walk distance must not be negative!"); @@ -53,8 +64,7 @@ public: // add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced) float dist_m = _dist_m + _startState.distance.error_m; - probability = 1.0; - int cnt = 0; + Stats::Maximum maxEdgeProb; // until distance is reached while (dist_m > 0) { @@ -71,12 +81,13 @@ public: for (const Node& neighbor : grid.neighbors(*curNode)) { const double prob = getProbability(currentState, *startNode, *curNode, neighbor); drawer.add(&neighbor, prob); + maxEdgeProb.add(prob); } // pick a neighbor and get its probability - double nodeProbability; - const Node* nextNode = drawer.get(nodeProbability); - if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one + //double nodeProbability; + const Node* nextNode = drawer.get(); + //if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one //probability += nodeProbability; //++cnt; @@ -91,7 +102,10 @@ public: } //if (cnt != 0) {probability /= cnt;} else {probability = 1.0;} - probability *= curNode->getWalkImportance() < 0.4f ? (1e-5) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) + probability = 1.0; + //probability = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0); // dist_m might be zero -> no edges -> no maximum + probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) + //probability = std::pow(probability, 5); // update after updateAfter(currentState, *startNode, *curNode); diff --git a/math/DrawList.h b/math/DrawList.h index 2c2b994..639205c 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -91,6 +91,15 @@ public: } /** get a random element based on its probability */ + T get() { + double tmp; // ignored + return get(tmp); + } + + /** + * get a random element based on its probability. + * the probability of the picked element is returned using the out parameter + */ T get(double& elemProbability) { // generate random number between [0:cumProbability] diff --git a/math/Median.h b/math/Median.h deleted file mode 100644 index d7a0d70..0000000 --- a/math/Median.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef MEDIAN_H -#define MEDIAN_H - -#include -#include - -template class Median { - -private: - - /** all scalar values in a sorted order (ascending) */ - std::vector sorted; - -public: - - /** add the given scalar value to the median calculation */ - void add(const Scalar value) { - - const auto idx = std::upper_bound( sorted.begin(), sorted.end(), value ); - sorted.insert( idx, value ); - - } - - /** get the median of all added values */ - float get() const { - - if (sorted.size() % 2 == 1) { // odd - - const int idx = sorted.size()/2; - return sorted[idx]; - - } else { // even - - const int idx0 = sorted.size()/2 - 1; - return (sorted[idx0] + sorted[idx0+1]) / 2; - - } - - } - -}; - -#endif // MEDIAN_H diff --git a/math/Stats.h b/math/Stats.h new file mode 100644 index 0000000..b65437b --- /dev/null +++ b/math/Stats.h @@ -0,0 +1,9 @@ +#ifndef MATH_STATS_H +#define MATH_STATS_H + +#include "stats/Average.h" +#include "stats/Median.h" +#include "stats/Minimum.h" +#include "stats/Maximum.h" + +#endif // MATH_STATS_H diff --git a/math/stats/Average.h b/math/stats/Average.h new file mode 100644 index 0000000..5db5149 --- /dev/null +++ b/math/stats/Average.h @@ -0,0 +1,43 @@ +#ifndef AVERAGE_H +#define AVERAGE_H + +#include "../../Assertions.h" + +namespace Stats { + + template class Average { + + private: + + int cnt; + Scalar sum; + + public: + + /** ctor */ + Average() : cnt(0), sum(0) { + ; + } + + /** contains a valid average? */ + bool isValid() const { + return cnt > 0; + } + + /** add a new value */ + void add(const Scalar val) { + sum += val; + ++cnt; + } + + /** get the current value */ + Scalar get() const { + Assert::isNot0(cnt, "add() values first!"); + return sum / (Scalar)cnt; + } + + }; + +} + +#endif // AVERAGE_H diff --git a/math/stats/Maximum.h b/math/stats/Maximum.h new file mode 100644 index 0000000..f7235e2 --- /dev/null +++ b/math/stats/Maximum.h @@ -0,0 +1,42 @@ +#ifndef STATS_MAXIMUM_H +#define STATS_MAXIMUM_H + +#include "../../Assertions.h" + +namespace Stats { + + template class Maximum { + + private: + + const Scalar START = -99999999; + Scalar curMax; + + public: + + /** ctor */ + Maximum() : curMax(START) { + ; + } + + /** is a valid maximum available? */ + inline bool isValid() const { + return curMax != START; + } + + /** add a new value */ + inline void add(const Scalar val) { + if (val > curMax) {curMax = val;} + } + + /** get the current value */ + inline Scalar get() const { + Assert::notEqual(curMax, START, "add() values first!"); + return curMax; + } + + }; + +} + +#endif // STATS_MAXIMUM_H diff --git a/math/stats/Median.h b/math/stats/Median.h new file mode 100644 index 0000000..a74d57e --- /dev/null +++ b/math/stats/Median.h @@ -0,0 +1,52 @@ +#ifndef STATS_MEDIAN_H +#define STATS_MEDIAN_H + +#include +#include + +#include "../../Assertions.h" + +namespace Stats { + + template class Median { + + private: + + /** all scalar values in a sorted order (ascending) */ + std::vector sorted; + + public: + + /** add the given scalar value to the median calculation */ + void add(const Scalar value) { + + const auto idx = std::upper_bound( sorted.begin(), sorted.end(), value ); + sorted.insert( idx, value ); + + } + + /** get the median of all added values */ + float get() const { + + // sanity check + Assert::isNot0(sorted.size(), "add elements first!"); + + if (sorted.size() % 2 == 1) { // odd + + const int idx = sorted.size()/2; + return sorted[idx]; + + } else { // even + + const int idx0 = sorted.size()/2 - 1; + return (sorted[idx0] + sorted[idx0+1]) / 2; + + } + + } + + }; + +} + +#endif // STATS_MEDIAN_H diff --git a/math/stats/Minimum.h b/math/stats/Minimum.h new file mode 100644 index 0000000..4567586 --- /dev/null +++ b/math/stats/Minimum.h @@ -0,0 +1,40 @@ +#ifndef STATS_MINIMUM_H +#define STATS_MINIMUM_H + +namespace Stats { + + template class Minimum { + + private: + + const Scalar START = +999999999; + Scalar curMin; + + public: + + /** ctor */ + Minimum() : curMin(START) { + ; + } + + /** is a valid minimum available? */ + inline bool isValid() const { + return curMin != START; + } + + /** add a new value */ + void add(const Scalar val) { + if (val < curMin) {curMin = val;} + } + + /** get the current value */ + Scalar get() const { + Assert::notEqual(curMin, START, "add() values first!"); + return curMin; + } + + }; + +} + +#endif // STATS_MINIMUM_H diff --git a/sensors/pressure/PressureTendence.h b/sensors/pressure/PressureTendence.h index a847580..5a6f22a 100644 --- a/sensors/pressure/PressureTendence.h +++ b/sensors/pressure/PressureTendence.h @@ -4,7 +4,7 @@ #include "../../data/Timestamp.h" #include "../../math/MovingAVG.h" #include "../../math/MovingMedian.h" -#include "../../math/Median.h" +#include "../../math/Stats.h" #include "BarometerData.h" diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index 2493da8..bdf2292 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -5,7 +5,7 @@ #include #include -#include "../../math/Median.h" +#include "../../math/Stats.h" #include "WiFiMeasurements.h" @@ -142,7 +142,7 @@ private: /** get the median signal strength */ inline float getMedian(const std::vector& vaps) const { - Median median; + Stats::Median median; for (const WiFiMeasurement& vap : vaps) { median.add(vap.getRSSI()); } @@ -153,11 +153,16 @@ private: /** get the maximum signal strength */ inline float getMax(const std::vector& vaps) const { - float max = -9999999; + Stats::Maximum max; for (const WiFiMeasurement& vap : vaps) { - if (vap.getRSSI() > max) {max = vap.getRSSI();} + max.add(vap.getRSSI()); } - return max; + return max.get(); +// float max = -9999999; +// for (const WiFiMeasurement& vap : vaps) { +// if (vap.getRSSI() > max) {max = vap.getRSSI();} +// } +// return max; } diff --git a/tests/grid/TestGridWalk2HeadingControl.cpp b/tests/grid/TestGridWalk2HeadingControl.cpp index 57242d5..123f565 100644 --- a/tests/grid/TestGridWalk2HeadingControl.cpp +++ b/tests/grid/TestGridWalk2HeadingControl.cpp @@ -30,6 +30,7 @@ struct MyNode12456012 : public GridPoint, public GridNode { + float getWalkImportance() const {return 1;} MyNode12456012() {;} MyNode12456012(const int x, const int y, const int z) : GridPoint(x,y,z) {;} }; diff --git a/tests/grid/TestGridWalk2RelPressure.cpp b/tests/grid/TestGridWalk2RelPressure.cpp index 72f1cf5..22e9960 100644 --- a/tests/grid/TestGridWalk2RelPressure.cpp +++ b/tests/grid/TestGridWalk2RelPressure.cpp @@ -28,6 +28,7 @@ struct MyNode124234 : public GridPoint, public GridNode { + float getWalkImportance() const {return 1;} MyNode124234() {;} MyNode124234(const int x, const int y, const int z) : GridPoint(x,y,z) {;} }; diff --git a/tests/math/TestStats.cpp b/tests/math/TestStats.cpp new file mode 100644 index 0000000..9fb4b2f --- /dev/null +++ b/tests/math/TestStats.cpp @@ -0,0 +1,92 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../math/Stats.h" + +using namespace Stats; + +TEST(Statistics, minimum) { + + Minimum min; + + min.add(+999); + ASSERT_EQ(999, min.get()); + + min.add(1); + ASSERT_EQ(1, min.get()); + + min.add(2); + ASSERT_EQ(1, min.get()); + + min.add(0); + ASSERT_EQ(0, min.get()); + + min.add(-99); + ASSERT_EQ(-99, min.get()); + +} + +TEST(Statistics, maximum) { + + Maximum max; + + max.add(-999); + ASSERT_EQ(-999, max.get()); + + max.add(1); + ASSERT_EQ(1, max.get()); + + max.add(2); + ASSERT_EQ(2, max.get()); + + max.add(1); + ASSERT_EQ(2, max.get()); + + max.add(99); + ASSERT_EQ(99, max.get()); + +} + +TEST(Statistics, median) { + + Median med; + + ASSERT_THROW(med.get(), std::exception); + + med.add(1); + ASSERT_EQ(1, med.get()); + + med.add(2); + ASSERT_EQ(1.5, med.get()); + + med.add(3); + ASSERT_EQ(2, med.get()); + + med.add(99); + ASSERT_EQ(2.5, med.get()); + + +} + +TEST(Statistics, average) { + + Average avg; + + ASSERT_THROW(avg.get(), std::exception); + + avg.add(1); + ASSERT_EQ(1, avg.get()); + + avg.add(2); + ASSERT_EQ(1.5, avg.get()); + + avg.add(3); + ASSERT_EQ(2, avg.get()); + + avg.add(99); + ASSERT_EQ(26.25, avg.get()); + +} + + +#endif