Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

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) {;}
 

grid/factory/v2/Importance.h

@@ -86,7 +86,7 @@ public:
 		KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
 
 		// probability adjustments
-		Distribution::Triangle<float> avoidWalls(0.0, 0.45f);
+		Distribution::Triangle<float> avoidWalls(0.0, 0.35f);
 		Distribution::Normal<float> favorDoors(0.0f, 0.4f);
 		Distribution::Normal<float> favorStairs(0.0f, 1.5f);
 
@@ -111,8 +111,8 @@ public:
 			// get the node
 			T& n1 = g[i];
 
-			// get the distance to the nearest wall
+			// get the distance to the nearest outline node [an outline node is directly adjacent to a wall]
-			const float distToWall_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
+			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) {
+			if (useWallAvoidance) {
-				n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.4f;
+				//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) {
+			if (useWallAvoidance) {
-				n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.4;
+				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;
 
 
 

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<Node>& 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<Node>& 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;
+		Stats::Maximum<double> maxEdgeProb;
-		int cnt = 0;
 
 		// 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;
+			//double nodeProbability;
-			const Node* nextNode = drawer.get(nodeProbability);
+			const Node* nextNode = drawer.get();
-			if ( nodeProbability < probability ) {probability = nodeProbability;}	// keep the smallest one
+			//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);

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]

math/Median.h

@@ -1,43 +0,0 @@
-#ifndef MEDIAN_H
-#define MEDIAN_H
-
-#include <vector>
-#include <algorithm>
-
-template <typename Scalar> class Median {
-
-private:
-
-	/** all scalar values in a sorted order (ascending) */
-	std::vector<Scalar> 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

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

math/stats/Average.h

@@ -0,0 +1,43 @@
+#ifndef AVERAGE_H
+#define AVERAGE_H
+
+#include "../../Assertions.h"
+
+namespace Stats {
+
+	template <typename Scalar> 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

math/stats/Maximum.h

@@ -0,0 +1,42 @@
+#ifndef STATS_MAXIMUM_H
+#define STATS_MAXIMUM_H
+
+#include "../../Assertions.h"
+
+namespace Stats {
+
+	template <typename Scalar> 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

math/stats/Median.h

@@ -0,0 +1,52 @@
+#ifndef STATS_MEDIAN_H
+#define STATS_MEDIAN_H
+
+#include <vector>
+#include <algorithm>
+
+#include "../../Assertions.h"
+
+namespace Stats {
+
+	template <typename Scalar> class Median {
+
+	private:
+
+		/** all scalar values in a sorted order (ascending) */
+		std::vector<Scalar> 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

math/stats/Minimum.h

@@ -0,0 +1,40 @@
+#ifndef STATS_MINIMUM_H
+#define STATS_MINIMUM_H
+
+namespace Stats {
+
+	template <typename Scalar> 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

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"
 

sensors/radio/VAPGrouper.h

@@ -5,7 +5,7 @@
 #include <unordered_map>
 #include <cmath>
 
-#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<WiFiMeasurement>& vaps) const {
 
-		Median<float> median;
+		Stats::Median<float> 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<WiFiMeasurement>& vaps) const {
 
-		float max = -9999999;
+		Stats::Maximum<float> 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;
 
 	}
 

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) {;}
 };

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) {;}
 };

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<float> 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<float> 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<float> 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<float> 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