huge commit

- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases

grid/walk/v2/modules/WalkModule.h

@@ -0,0 +1,39 @@
+#ifndef WALKMODULE_H
+#define WALKMODULE_H
+
+#include "../../../Grid.h"
+
+/** base-class for all WalkStates */
+struct WalkState {
+
+	/** position where the walk starts */
+	GridPoint startPos;
+
+	/** ctor */
+	WalkState(const GridPoint& startPos) : startPos(startPos) {;}
+
+};
+
+
+/**
+ * base-class for all walk-modules that influence p(e)
+ */
+template <typename Node, typename WalkState> class WalkModule {
+
+public:
+
+	/** update the given WalkState before starting the walk. e.g. based on sensor readings */
+	virtual void updateBefore(WalkState& state) = 0;
+
+	/** get the probability p(e) from curNode to potentialNode */
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) = 0;
+
+	/** update the walk state based on the given transition (if any update is necssary) */
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) = 0;
+
+};
+
+#endif // WALKMODULE_H

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -0,0 +1,68 @@
+#ifndef WALKMODULEFOLLOWDESTINATION_H
+#define WALKMODULEFOLLOWDESTINATION_H
+
+#include "WalkModule.h"
+#include "../../../../nav/dijkstra/Dijkstra.h"
+
+/**
+ * favour edges p(e) that approach the destination
+ */
+template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
+
+private:
+
+	Dijkstra<Node> dijkstra;
+
+	struct DijkstraMapper {
+		const Grid<Node>& grid;
+		DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
+		int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
+		const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
+		float getWeightBetween(const Node& n1, const Node& n2) const {
+			return n1.getDistanceInCM(n2) * n2.navImportance;
+		}
+	};
+
+public:
+
+	/** ctor */
+	WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
+
+		// shortest path calculation
+		dijkstra.build(&destination, DijkstraMapper(grid));
+
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+
+		const float kappa = 0.8;
+		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
+		const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
+
+		// probability
+		return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+};
+
+#endif // WALKMODULEFOLLOWDESTINATION_H

grid/walk/v2/modules/WalkModuleHeading.h

@@ -0,0 +1,76 @@
+#ifndef WALKMODULEHEADING_H
+#define WALKMODULEHEADING_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+public:
+
+	/** ctor */
+	WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+
+		(void) state;
+		state.startHeading += draw.get();
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		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;
+//		}
+
+	}
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// TODO
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		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;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
+
+		// determine probability
+		return dist.getProbability(angularDiff);
+
+	}
+
+
+};
+
+#endif // WALKMODULEHEADING_H

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -0,0 +1,78 @@
+#ifndef WALKMODULEHEADINGCONTROL_H
+#define WALKMODULEHEADINGCONTROL_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+	Control* ctrl;
+
+	//std::unordered_map<WalkState*, float> errorTracker;
+
+public:
+
+	/** ctor 3.0 should be OK! */
+	WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 1.5).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+
+		const float var = draw.get() * 0.15;//0.05;
+		//const float var = 0;
+		state.startHeading += ctrl->turnAngle + var;
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		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;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+		// determine probability
+		const float prob = dist.getProbability(angularDiff);
+		return prob;
+
+	}
+
+
+
+};
+
+#endif // WALKMODULEHEADINGCONTROL_H

grid/walk/v2/modules/WalkModuleNodeImportance.h

@@ -0,0 +1,55 @@
+#ifndef WALKMODULENODEIMPORTANCE_H
+#define WALKMODULENODEIMPORTANCE_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * favor edges based on the importance-factor of the next node.
+ * @see struct GridNodeImportance
+ */
+template <typename Node, typename WalkState> class WalkModuleNodeImportance : public WalkModule<Node, WalkState> {
+
+private:
+
+public:
+
+	/** ctor */
+	WalkModuleNodeImportance() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const double prob = potentialNode.getNavImportance();
+		return std::pow(prob, 10);
+		//return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULENODEIMPORTANCE_H

grid/walk/v2/modules/WalkModuleRelativePressureControl.h

@@ -0,0 +1,162 @@
+#ifndef WALKMODULERELATIVEPRESSURE_H
+#define WALKMODULERELATIVEPRESSURE_H
+
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+#include "../../../../Assertions.h"
+
+
+/**
+ * a walk-state the contains the pressure relative to time t0
+ */
+struct WalkStateRelativePressure {
+
+	/** the pressure level [relative to time t0] one SHOULD messure at this state */
+	float pressureRelToT0;
+
+	int dirLock = 0;
+
+	/** ctor */
+	WalkStateRelativePressure(const float pressureRelToT0) : pressureRelToT0(pressureRelToT0) {;}
+
+};
+
+
+/**
+ * uses the pressure relative to t=0 within the control-data
+ * to estimate the likelyhood for any z-changes during the transition
+ */
+template <typename Node, typename WalkState, typename Control> class WalkModuleRelativePressureControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	Control* ctrl;
+
+	/** pressure-change (hPa) per meter */
+	const float hPaPerMeter = 0.126f;		// given an average hPa of 938
+
+public:
+
+	/** ctor */
+	WalkModuleRelativePressureControl(Control* ctrl, const float hPaPerMeter) : ctrl(ctrl), hPaPerMeter(hPaPerMeter) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		// e.g. walking down from the 3rd to the second floor
+//		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+//		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+//		const int deltaZ_cm = startNode.z_cm - endNode.z_cm;
+//		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+
+//		// update the states pressure
+//		state.pressureRelToT0 += deltaPressure;
+
+//		// sanity checks
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+//		static int xx = 0;
+//		if (++xx % 1024 == 0) {
+//			++xx;
+//		}
+
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - nextNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		// update the states pressure
+		state.pressureRelToT0 = expectedPressure;
+
+		// sanity checks
+		Assert::isNotNaN(deltaPressure, "detected NaN!");
+		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+		if(std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {--state.dirLock;}
+			if (state.dirLock < 0) {++state.dirLock;}
+		} else {
+			if (deltaZ_cm > 0) {state.dirLock = +25;}
+			if (deltaZ_cm < 0) {state.dirLock = -25;}
+		}
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the values from control-data
+		const float curRelPressure = ctrl->barometer.hPaRelativeToT0;
+		const float pressureSigma = ctrl->barometer.estimatedSigma * 1.50f;
+
+		// not yet available/calibrated/possible -> skip evaluation!
+		if (pressureSigma == 0) {return 1;}
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = +3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		const float oldErr = std::abs(state.pressureRelToT0 - curRelPressure);
+		const float newErr = std::abs(expectedPressure - curRelPressure);
+
+		if (std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {
+				if (deltaZ_cm < 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			} else {
+				if (deltaZ_cm > 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			}
+		}
+
+
+		if (oldErr > pressureSigma) {
+			if (deltaZ_cm == 0) {return 0.001;}
+		}
+
+		if (newErr > oldErr) {return 0.002;}
+		if (newErr == oldErr) {return 0.005;}
+		if (newErr < oldErr) {return 0.99;}
+		throw 1;
+
+
+
+//		// compare control-data with potential transition
+//		double prob = Distribution::Normal<double>::getProbability(curRelPressure, pressureSigma, expectedPressure);
+////		prob = std::pow(prob, 20);
+
+//		// sanity checks
+//		Assert::isNotNaN(prob, "detected NaN!");
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(expectedPressure, "detected NaN!");
+
+//		return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULERELATIVEPRESSURE_H

grid/walk/v2/modules/WalkModuleSpread.h

@@ -0,0 +1,62 @@
+#ifndef WALKMODULESPREAD_H
+#define WALKMODULESPREAD_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * simply try to move away from the starting node as much as possible
+ */
+template <typename Node, typename WalkState> class WalkModuleSpread : public WalkModule<Node, WalkState> {
+
+private:
+
+	Point3 avg;
+
+public:
+
+	/** ctor */
+	WalkModuleSpread() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+		avg = avg * 0.999 + state.startPos.inMeter() * 0.001;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const float dOld = avg.getDistance(curNode.inMeter());
+		const float dNew = avg.getDistance(potentialNode.inMeter());
+
+		if (dNew > dOld) {return 0.8;}
+		if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
+		if (dNew == dOld) {return 0.2;}
+		return 0;
+
+	}
+
+
+};
+
+
+#endif // WALKMODULESPREAD_H

grid/walk/v2/modules/WalkStateHeading.h

@@ -0,0 +1,16 @@
+#ifndef WALKSTATEHEADING_H
+#define WALKSTATEHEADING_H
+
+#include "../../../../geo/Heading.h"
+
+struct WalkStateHeading {
+
+	Heading startHeading;
+
+	/** ctor */
+	WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;}
+
+};
+
+
+#endif // WALKSTATEHEADING_H