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

grid/factory/v2/GridFactory.h

@@ -18,18 +18,7 @@
 #include <functional>
 
 
-/** 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 <typename T> 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<Floorplan::FloorObstacleLine*>(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<Floorplan::FloorObstacleCircle*>(fo)) {
+				throw Exception("should not happen");
+
+			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(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? */

grid/factory/v2/GridFactoryListener.h

@@ -0,0 +1,19 @@
+#ifndef GRIDFACTORYLISTENER_H
+#define GRIDFACTORYLISTENER_H
+
+#include <string>
+
+/** 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

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 <typename T> static void addImportance(Grid<T>& g) {
+	template <typename T> static void addImportance(Grid<T>& 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<T> inv(g.getGridSize_cm());
@@ -51,15 +59,13 @@ public:
 		// construct KNN search
 		KNN<Grid<T>, 3> knn(inv);
 
-		// the number of neighbors to use
-		static constexpr int numNeighbors = 12;
-
 		// create list of all door-nodes
 		std::vector<T> doors;
 
 		// create list of all stair-nodes
 		std::vector<T> stairs;
 
+
 		// process each node
 		for (T& n1 : g) {
 
@@ -83,9 +89,26 @@ public:
 		Distribution::Normal<float> favorDoors(0.0f, 0.5f);
 		Distribution::Normal<float> 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!");
+
 		}
 
 	}

grid/factory/v2/Stairs.h

@@ -22,6 +22,9 @@ private:
 	/** calculation helper */
 	Helper<T> helper;
 
+	// keep a list of all vertices below stairwells and remove them hereafter
+	std::vector<T*> 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<T*> 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<XYZ>& points) {
 		auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
 		auto it = std::min_element(points.begin(), points.end(), comp);

grid/walk/v2/GridWalker.h

@@ -28,13 +28,15 @@ public:
 	/** perform the walk based on the configured setup */
 	WalkState getDestination(Grid<Node>& 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<Node, WalkState>* 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);
 
 	}
 

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

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

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();

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -13,11 +13,11 @@ template <typename Node, typename WalkState, typename Control> class WalkModuleH
 
 private:
 
-	/** van-Mises distribution */
+	/** CURRENTLY NOT USED van-Mises distribution */
 	Distribution::LUT<double> dist;
 
-	/** van-Mises draw list */
-	DrawList<double> draw;
+	/** random noise */
+	Distribution::Normal<float> distNoise;
 
 	Control* ctrl;
 
@@ -26,16 +26,25 @@ private:
 public:
 
 	/** ctor 3.0 should be OK! */
-	WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
+	WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
+		dist(Distribution::VonMises<double>(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;

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 {

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