a lot!!! of changes

added main menu
added debug display
many debug widgets for plotting live data
worked on android live sensors
added offline-data sensor feeding
some dummy data sensors
worked on the map display
added ui debug for grid-points, particles and weights
added a cool dude to display the estimation
added real filtering based on the Indoor components
c++11 fixes for android compilation
online and offline filtering support
new resampling technique for testing
map loading via dialog

nav/Filter.h

@@ -0,0 +1,149 @@
+#ifndef FILTER_H
+#define FILTER_H
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
+
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
+
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+
+#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleButterActivity.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
+
+#include "State.h"
+#include "Node.h"
+
+#include "../Settings.h"
+
+class PFInit : public K::ParticleFilterInitializer<MyState> {
+
+private:
+
+	Grid<MyGridNode>* grid;
+
+public:
+
+	PFInit(Grid<MyGridNode>* grid) : grid(grid) {
+
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		std::minstd_rand gen;
+		std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
+		std::uniform_real_distribution<float> distHead(0, 2*M_PI);
+
+		for (K::Particle<MyState>& p : particles) {
+			const int idx = distIdx(gen);
+			const MyGridNode& node = (*grid)[idx];
+			p.state.position = node;								// random position
+			p.state.heading.direction = Heading(distHead(gen));		// random heading
+		}
+
+//		// fix position + heading
+//		for (K::Particle<MyState>& p : particles) {
+//			const int idx = 9000;
+//			const MyGridNode& node = (*grid)[idx];
+//			p.state.position = node;
+//			p.state.heading.direction = Heading(0);
+//		}
+
+	}
+
+};
+
+class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+
+public:
+
+	Grid<MyGridNode>* grid;
+	GridWalker<MyGridNode, MyState> walker;
+
+	WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
+	WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
+	WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
+	WalkModuleButterActivity<MyGridNode, MyState> modBarometer;
+	WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
+
+	std::minstd_rand gen;
+
+public:
+
+	PFTrans(Grid<MyGridNode>* grid, MyControl* ctrl) : grid(grid), modHeading(ctrl, Settings::turnSigma), modDestination(*grid) {
+
+		walker.addModule(&modFavorZ);
+		walker.addModule(&modHeading);
+		walker.addModule(&modImportance);
+		walker.addModule(&modBarometer);
+		walker.addModule(&modDestination);
+
+		if (Settings::destination != GridPoint(0,0,0)) {
+			modDestination.setDestination(grid->getNodeFor(Settings::destination));
+		}
+
+	}
+
+
+
+	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		std::normal_distribution<float> noise(0, Settings::stepSigma);
+
+		for (K::Particle<MyState>& p : particles) {
+			const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::stepLength + noise(gen));
+			p.state = walker.getDestination(*grid, p.state, dist_m);
+		}
+
+		((MyControl*)control)->resetAfterTransition();
+
+	}
+
+};
+
+class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+	WiFiModelLogDistCeiling& wifiModel;
+	WiFiObserverFree wiFiProbability;
+
+public:
+
+	PFEval(WiFiModelLogDistCeiling& wifiModel) : wifiModel(wifiModel), wiFiProbability(Settings::wifiSigma, wifiModel) {
+
+	}
+
+	double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
+
+		double sum = 0;
+
+		// smartphone is 1.3 meter above ground
+		const Point3 person(0,0,Settings::smartphoneAboveGround);
+
+		// local copy!! observation might be changed async outside!! (will really produces crashes!)
+		const MyObservation observation = _observation;
+
+		for (K::Particle<MyState>& p : particles) {
+			const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, observation.wifi);
+			const double pGPS = 1;
+			const double prob = pWiFi * pGPS;
+			p.weight = prob;
+			sum += prob;
+		}
+
+		return sum;
+
+	}
+
+};
+
+
+
+#endif // FILTER_H

nav/NavController.h

@@ -0,0 +1,299 @@
+#ifndef NAVCONTROLLER_H
+#define NAVCONTROLLER_H
+
+#include "../sensors/AccelerometerSensor.h"
+#include "../sensors/GyroscopeSensor.h"
+#include "../sensors/BarometerSensor.h"
+#include "../sensors/WiFiSensor.h"
+#include "../sensors/SensorFactory.h"
+#include "../sensors/StepSensor.h"
+#include "../sensors/TurnSensor.h"
+
+#include "../ui/debug/SensorDataWidget.h"
+#include "../ui/map/MapView.h"
+
+#include <Indoor/Assertions.h>
+#include <thread>
+
+#include "State.h"
+#include "Filter.h"
+#include "Controller.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+
+#include "Settings.h"
+#include "RegionalResampling.h"
+
+Q_DECLARE_METATYPE(const void*)
+
+
+
+
+
+
+
+
+class NavController :
+		public SensorListener<AccelerometerData>,
+		public SensorListener<GyroscopeData>,
+		public SensorListener<BarometerData>,
+		public SensorListener<WiFiMeasurements>,
+		public SensorListener<GPSData>,
+		public SensorListener<StepData>,
+		public SensorListener<TurnData> {
+
+private:
+
+	Controller* mainController;
+	Grid<MyGridNode>* grid;
+	WiFiModelLogDistCeiling wifiModel;
+	Floorplan::IndoorMap* im;
+
+	MyObservation curObs;
+	MyControl curCtrl;
+
+	bool running = false;
+	std::thread tUpdate;
+	std::thread tDisplay;
+
+	std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
+
+public:
+
+	virtual ~NavController() {
+		if (running) {stop();}
+	}
+
+	NavController(Controller* mainController, Grid<MyGridNode>* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) {
+
+		wifiModel.loadAPs(im, Settings::wifiTXP, Settings::wifiEXP, Settings::wifiWAF);
+
+		SensorFactory::get().getAccelerometer().addListener(this);
+		SensorFactory::get().getGyroscope().addListener(this);
+		SensorFactory::get().getBarometer().addListener(this);
+		SensorFactory::get().getWiFi().addListener(this);
+		SensorFactory::get().getSteps().addListener(this);
+		SensorFactory::get().getTurns().addListener(this);
+
+		std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
+
+		//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
+		std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.1));
+
+		//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
+		//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.10));
+		std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
+
+
+		std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(wifiModel));
+		std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid, &curCtrl));
+
+		pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
+		pf->setTransition(std::move(transition));
+		pf->setEvaluation(std::move(eval));
+		pf->setEstimation(std::move(estimation));
+		pf->setResampling(std::move(resample));
+
+		pf->setNEffThreshold(1.0);
+
+	}
+
+	void start() {
+		Assert::isFalse(running, "already started!");
+		running = true;
+		tUpdate = std::thread(&NavController::update, this);
+		tDisplay = std::thread(&NavController::display, this);
+	}
+
+	void stop() {
+		Assert::isTrue(running, "not started!");
+		running = false;
+		tUpdate.join();
+		tDisplay.join();
+	}
+
+	void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
+		(void) sensor;
+		curObs.currentTime = ts;
+	}
+
+	void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
+		(void) sensor;
+		curObs.currentTime = ts;
+	}
+
+	void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
+		(void) sensor;
+		curObs.currentTime = ts;
+	}
+
+	void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
+		(void) sensor;
+		(void) ts;
+		curObs.currentTime = ts;
+		curObs.wifi = data;
+	}
+
+	void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
+		(void) sensor;
+		(void) ts;
+		curObs.currentTime = ts;
+		curObs.gps = data;
+	}
+
+	void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override {
+		(void) sensor;
+		(void) ts;
+		curObs.currentTime = ts;
+		curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent;		// set to zero after each transition
+	}
+
+	void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override {
+		(void) sensor;
+		(void) ts;
+		curObs.currentTime = ts;
+		curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent;			// set to zero after each transition
+	}
+
+	int cameraMode = 0;
+	void toggleCamera() {
+		cameraMode = (cameraMode + 1) % 3;
+	}
+
+private:
+
+	/** particle-filter update loop */
+	void update() {
+
+		Timestamp lastTransition;
+
+		while(running) {
+
+//			// fixed update rate based on the systems time -> LIVE! even for offline data
+//			const Timestamp ts1 = Timestamp::fromUnixTime();
+//			doUpdate();
+//			const Timestamp ts2 = Timestamp::fromUnixTime();
+//			const Timestamp needed = ts2-ts1;
+//			const Timestamp sleep = Timestamp::fromMS(500) - needed;
+//			std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
+
+			// fixed update rate based on incoming sensor data
+			// allows working with live data and faster for offline data
+			const Timestamp diff = curObs.currentTime - lastTransition;
+			if (diff > Timestamp::fromMS(500)) {
+				doUpdate();
+				lastTransition = curObs.currentTime;
+			} else {
+				std::this_thread::sleep_for(std::chrono::milliseconds(10));
+			}
+
+		}
+	}
+
+
+	MyState curEst;
+	//MyState lastEst;
+
+	void doUpdate() {
+
+		//lastEst = curEst;
+		curEst = pf->update(&curCtrl, curObs);
+
+		// hacky.. but we need to call this one from the main thread!
+		//mainController->getMapView()->showParticles(pf->getParticles());
+		qRegisterMetaType<const void*>();
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+
+
+		PFTrans* trans = (PFTrans*)pf->getTransition();
+		const MyGridNode* node = grid->getNodePtrFor(curEst.position);
+		if (node) {
+			const DijkstraPath<MyGridNode> path = trans->modDestination.getShortestPath(*node);
+		//	mainController->getMapView()->showGridImportance();
+			Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "setPath", Qt::QueuedConnection, Q_ARG(const void*, &path)), "call failed");
+		}
+
+		/*
+		static K::Gnuplot gp;
+		K::GnuplotSplot plot;
+		K::GnuplotSplotElementLines lines; plot.add(&lines);
+		K::GnuplotSplotElementPoints points; plot.add(&points);
+		K::GnuplotSplotElementPoints best; plot.add(&best);			best.setPointSize(2); best.setColorHex("#0000ff");
+
+		for (const K::Particle<MyState>& p : pf->getParticles()) {
+			const Point3 pos = p.state.position.inMeter();
+			points.add(K::GnuplotPoint3(pos.x, pos.y, pos.z));
+		}
+
+		for (const Floorplan::Floor* f : im->floors) {
+			for (const Floorplan::FloorOutlinePolygon* polygon : f->outline) {
+				for (int i = 0; i < polygon->poly.points.size(); ++i) {
+					const Point2 p1 = polygon->poly.points[i];
+					const Point2 p2 = polygon->poly.points[(i+1)%polygon->poly.points.size()];
+					K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
+					K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
+					lines.addSegment(gp1, gp2);
+				}
+			}
+		}
+
+		K::GnuplotPoint3 gpBest(curEst.position.x_cm/100.0f, curEst.position.y_cm/100.0f, curEst.position.z_cm/100.0f);
+		best.add(gpBest);
+
+		gp.draw(plot);
+		gp.flush();
+		*/
+
+
+	}
+
+	const int display_ms = 50;
+
+	/** UI update loop */
+	void display() {
+		while(running) {
+			doDisplay();
+			std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
+		}
+	}
+
+	Point3 curPosFast;
+	Point3 curPosSlow;
+
+
+
+	void doDisplay() {
+
+		const float kappa1 = display_ms / 1000.0f;
+		const float kappa2 = kappa1 * 0.7;
+
+		const float myHeight_m = 1.80;
+
+		curPosFast = curPosFast * (1-kappa1) + curEst.position.inMeter() * (kappa1);
+		curPosSlow = curPosSlow * (1-kappa2) + curEst.position.inMeter() * (kappa2);
+
+		const Point3 dir = (curPosFast - curPosSlow).normalized();
+		const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized();
+
+		if (cameraMode == 0) {
+			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
+		} else if (cameraMode == 1) {
+			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
+		} else if (cameraMode == 2) {
+			const Point3 spectator = curPosFast + Point3(0,0,20) - dir*15;
+			const Point3 spectatorDir = (curPosFast - spectator).normalized();
+			mainController->getMapView()->setLookEye(spectator);
+			mainController->getMapView()->setLookDir(spectatorDir);
+		}
+
+		mainController->getMapView()->setCurrentEstimation(curPosFast, dir);
+
+
+	}
+
+};
+
+#endif // NAVCONTROLLER_H

nav/Node.h

@@ -0,0 +1,29 @@
+#ifndef NODE_H
+#define NODE_H
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/sensors/radio/WiFiGridNode.h>
+
+struct MyGridNode : public GridNode, public GridPoint {//, public WiFiGridNode<10> {
+
+	float navImportance;
+	float getNavImportance() const { return navImportance; }
+
+	/** empty ctor */
+	MyGridNode() : GridPoint(-1, -1, -1) {;}
+
+	/** ctor */
+	MyGridNode(const int x_cm, const int y_cm, const int z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
+
+
+	static void staticDeserialize(std::istream& inp) {
+		//WiFiGridNode::staticDeserialize(inp);
+	}
+
+	static void staticSerialize(std::ostream& out) {
+		//WiFiGridNode::staticSerialize(out);
+	}
+
+};
+
+#endif // NODE_H

nav/RegionalResampling.h

@@ -0,0 +1,68 @@
+#ifndef REGIONALRESAMPLING_H
+#define REGIONALRESAMPLING_H
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include "State.h"
+
+class RegionalResampling : public K::ParticleFilterResampling<MyState> {
+
+public:
+
+	float maxDist = 12.5;
+
+	RegionalResampling() {;}
+
+	void resample(std::vector<K::Particle<MyState>>& particles) override {
+
+		Point3 sum;
+		for (const K::Particle<MyState>& p : particles) {
+			sum += p.state.position.inMeter();
+		}
+		const Point3 avg = sum / particles.size();
+
+		std::vector<K::Particle<MyState>> next;
+		for (const K::Particle<MyState>& p : particles) {
+			const float dist = p.state.position.inMeter().getDistance(avg);
+			if (rand() % 6 != 0) {continue;}
+			if (dist < maxDist) {next.push_back(p);}
+		}
+
+		// cumulate
+		std::vector<K::Particle<MyState>> copy = particles;
+		double cumWeight = 0;
+		for ( K::Particle<MyState>& p : copy) {
+			cumWeight += p.weight;
+			p.weight = cumWeight;
+		}
+
+		// draw missing particles
+		const int missing = particles.size() - next.size();
+		for (int i = 0; i < missing; ++i) {
+			next.push_back(draw(copy, cumWeight));
+		}
+
+		std::swap(next, particles);
+
+	}
+
+	std::minstd_rand gen;
+
+	/** draw one particle according to its weight from the copy vector */
+	const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
+
+		// generate random values between [0:cumWeight]
+		std::uniform_real_distribution<float> dist(0, cumWeight);
+
+		// draw a random value between [0:cumWeight]
+		const float rand = dist(gen);
+
+		// search comparator (cumWeight is ordered -> use binary search)
+		auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;};
+		auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
+		return *it;
+
+	}
+
+};
+
+#endif // REGIONALRESAMPLING_H

nav/State.h

@@ -0,0 +1,77 @@
+#ifndef STATE_H
+#define STATE_H
+
+#include <Indoor/grid/walk/v2/GridWalker.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleButterActivity.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
+
+#include <Indoor/sensors/radio/WiFiMeasurements.h>
+#include <Indoor/sensors/gps/GPSData.h>
+
+struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading, public WalkStateBarometerActivity {
+
+
+	/** ctor */
+	MyState(const int x_cm, const int y_cm, const int z_cm) : WalkState(GridPoint(x_cm, y_cm, z_cm)), WalkStateHeading(Heading(0), 0) {
+		;
+	}
+
+	MyState() : WalkState(GridPoint()), WalkStateHeading(Heading(0), 0) {
+		;
+	}
+
+	MyState& operator += (const MyState& o) {
+		position += o.position;
+		return *this;
+	}
+
+	MyState& operator /= (const float val) {
+		position /= val;
+		return *this;
+	}
+
+	MyState operator * (const float val) const {
+		MyState copy = *this;
+		copy.position = copy.position * val;
+		return copy;
+	}
+
+
+
+
+};
+
+/** observed sensor data */
+struct MyObservation {
+
+	/** wifi measurements */
+	WiFiMeasurements wifi;
+
+	/** gps measurements */
+	GPSData gps;
+
+	/** time of evaluation */
+	Timestamp currentTime;
+
+};
+
+/** (observed) control data */
+struct MyControl {
+
+	/** turn angle (in radians) since the last transition */
+	float turnSinceLastTransition_rad = 0;
+
+	/** number of steps since the last transition */
+	int numStepsSinceLastTransition = 0;
+
+	/** reset the control-data after each transition */
+	void resetAfterTransition() {
+		turnSinceLastTransition_rad = 0;
+		numStepsSinceLastTransition = 0;
+	}
+
+};
+
+#endif // STATE_H