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

@@ -3,6 +3,7 @@
 
 #include "Exception.h"
 #include <sstream>
+#include <cmath>
 
 // 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 <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {
+		if (v < min || v > max) {
 			std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n";
-		if (v < min || v > max) {doThrow(err+ss.str());}
+			doThrow(err+ss.str());
+		}
 	}
 
 }

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 */

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 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;}
+		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);
 
+		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;
+		}
+
+		return false; // No collision
+
+
+
+//		const double delta = 0.0000001;
+
+//		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;

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 */
-	WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;}
+		_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
+
+	} heading;
+
+
+
+
+	/** ctor */
+	explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
 
 };
 

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

math/MovingAverageTS.h

@@ -0,0 +1,67 @@
+#ifndef MOVINGAVERAGETS_H
+#define MOVINGAVERAGETS_H
+
+#include <vector>
+#include "../data/Timestamp.h"
+#include <algorithm>
+
+template <typename T> 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<Entry> 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

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<Scalar> lst) {
+	float getNearestDistance(const std::initializer_list<Scalar> 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<Scalar> lst, const float maxRadius) const {
+		std::vector<std::pair<size_t, float>> 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

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

sensors/imu/StepDetection.h

@@ -16,6 +16,7 @@
 
 
 #include "../../Assertions.h"
+#include "../../math/MovingAverageTS.h"
 
 
 /**
@@ -27,83 +28,153 @@ class StepDetection {
 
 private:
 
-	/** low pass acc-magnitude */
-	float avg1 = 0;
+	MovingAverageTS<float> avgLong;
+	MovingAverageTS<float> avgShort;
 
-	/** 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.17;
-
-		/** block until the given timestamp before detecting additional steps */
 	Timestamp blockUntil;
+	bool waitForUp = false;
 
-
-	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 {
+		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;
 
-			return false;
 
 	}
 
-	}
+
+//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;
+
+//		}
+
+//	}
 
 
 };

sensors/imu/TurnDetection.h

@@ -4,6 +4,7 @@
 #include "GyroscopeData.h"
 #include "AccelerometerData.h"
 #include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
 
 #include <eigen3/Eigen/Dense>
 
@@ -23,147 +24,100 @@ class TurnDetection {
 
 private:
 
-	//std::vector<AccelerometerData> accData;
-	std::vector<GyroscopeData> gyroData;
 
-	Timestamp lastGyro;
-	Timestamp lastRotMatEst;
+	//std::vector<GyroscopeData> 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<Eigen::Vector3f> 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();
 		}
 
 

sensors/offline/OfflineAndroid.h

@@ -4,11 +4,14 @@
 #include <fstream>
 #include <string>
 
+#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 <typename SensorData> 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<OfflineEntry<WiFiMeasurements>> wifi;
 	std::vector<OfflineEntry<GroundTruthID>> groundTruth;
+	std::vector<OfflineEntry<GyroscopeData>> gyro;
+
+	std::vector<OfflineEntry<AccelerometerData>> accel;
+	std::vector<OfflineEntry<AccelerometerData>> 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<OfflineEntry<WiFiMeasurements>>& getWiFi() const {return wifi;}
 
+	/** get all gyroscope readings */
+	const std::vector<OfflineEntry<GyroscopeData>>& getGyroscope() const {return gyro;}
+
+	/** get all accelerometer readings */
+	const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}
+
+	/** get all gravity readings */
+	const std::vector<OfflineEntry<AccelerometerData>>& 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<AccelerometerData>(ts, data));
+				if (listener) {listener->onAccelerometer(ts, data);}
+				break;
+			}
+
+			case 1: {
+				const AccelerometerData data = parseAccelerometer(sensorData);
+				gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
+				if (listener) {listener->onGravity(ts, data);}
+				break;
+			}
+
+			case 3: {
+				const GyroscopeData data = parseGyroscope(sensorData);
+				gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
+				if (listener) {listener->onGyroscope(ts, data);}
+				break;
+			}
+
 			case 8: {
-				OfflineEntry<WiFiMeasurements> entry(ts, parseWiFi(sensorData));
-				wifi.push_back(entry);
+				const WiFiMeasurements data = parseWiFi(sensorData);
+				wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
+				if (listener) {listener->onWiFi(ts, data);}
 				break;
 			}
+
 			case 99: {
-				OfflineEntry<GroundTruthID> entry (ts, parseGroundTruthTick(sensorData));
-				groundTruth.push_back(entry);
+				const GroundTruthID data = parseGroundTruthTick(sensorData);
+				groundTruth.push_back(OfflineEntry<GroundTruthID>(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) {
 

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

tests/data/WalkHeadingMap.xml

@@ -0,0 +1,30 @@
+<map width="70" depth="50">
+    <floors>
+        <floor atHeight="0" height="4" name="0. Stock">
+            <outline>
+                <polygon name="new" method="0">
+                    <point x="0" y="0"/>
+                    <point x="20" y="0"/>
+                    <point x="20" y="20"/>
+                    <point x="-0" y="20"/>
+                </polygon>
+            </outline>
+            <obstacles>
+                <line material="3" type="1" x1="9" y1="15" x2="11" y2="15"/>
+                <line material="3" type="1" x1="15" y1="11" x2="15" y2="9"/>
+                <line material="3" type="1" x1="9" y1="5" x2="11" y2="5"/>
+                <line material="3" type="1" x1="5" y1="11" x2="5" y2="9"/>
+                <line material="3" type="1" x1="14" y1="15" x2="15" y2="14"/>
+                <line material="3" type="1" x1="15" y1="6" x2="14" y2="5"/>
+                <line material="3" type="1" x1="5" y1="6" x2="6" y2="5"/>
+                <line material="3" type="1" x1="5" y1="14" x2="6" y2="15"/>
+                <line material="3" type="1" x1="21" y1="-1" x2="21" y2="-5"/>
+            </obstacles>
+            <underlays/>
+            <pois/>
+            <accesspoints/>
+            <beacons/>
+            <stairs/>
+        </floor>
+    </floors>
+</map>

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 <typename T> Plot& addEdges(Grid<T>& g) {
 
 		// prevent adding edges twice
-		std::set<size_t> done;
+		std::set<std::string> 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);

tests/grid/TestGridWalk2HeadingControl.cpp

@@ -0,0 +1,177 @@
+#ifdef WITH_TESTS
+
+#include <fstream>
+
+#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 <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+
+
+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<MyNode12456012> grid(25);
+	GridFactory<MyNode12456012> 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<MyNode12456012, MyState0012345> walker;
+	WalkModuleHeadingControl<MyNode12456012, MyState0012345, MyControl> 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<float> dist(0.75, 0.10);
+
+	for (float rad = 0; rad < 6*M_PI; rad += M_PI*0.125) {
+
+		// setup particles
+		std::vector<Particle> 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

tests/grid/TestGridWalk2RelPressure.cpp

@@ -27,13 +27,13 @@
 #include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
 
 
-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<MyNode> grid(20);
+	Grid<MyNode124234> 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<MyNode, MyState> walker;
-	WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPres(&ctrl, 0.1);
+	MyState316123 ms(start, 0);
+	GridWalker<MyNode124234, MyState316123> walker;
+	WalkModuleRelativePressureControl<MyNode124234, MyState316123, MyControl> 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";

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<MyNode1239, MyState23452> walker;
-	WalkModuleHeadingControl<MyNode1239, MyState23452, Control> modHead(&ctrl);
+	WalkModuleHeadingControl<MyNode1239, MyState23452, Control> 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));
 		}