current revision

nav/NavController.h

@@ -10,7 +10,8 @@
 #include "../sensors/TurnSensor.h"
 
 #include "../ui/debug/SensorDataWidget.h"
-#include "../ui/map/MapView.h"
+#include "../ui/map/3D/MapView3D.h"
+#include "../ui/debug/InfoWidget.h"
 
 #include <Indoor/Assertions.h>
 #include <thread>
@@ -18,24 +19,25 @@
 #include "State.h"
 #include "Filter.h"
 #include "Controller.h"
+#include "NavControllerListener.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>
 
+#ifndef ANDROID
+#include <valgrind/callgrind.h>
+#endif
+
 #include "Settings.h"
 #include "RegionalResampling.h"
+#include "NodeResampling.h"
 
 Q_DECLARE_METATYPE(const void*)
 
 
 
-
-
-
-
-
 class NavController :
 		public SensorListener<AccelerometerData>,
 		public SensorListener<GyroscopeData>,
@@ -43,7 +45,9 @@ class NavController :
 		public SensorListener<WiFiMeasurements>,
 		public SensorListener<GPSData>,
 		public SensorListener<StepData>,
-		public SensorListener<TurnData> {
+		public SensorListener<TurnData>,
+		public SensorListener<ActivityData> {
+
 
 private:
 
@@ -56,106 +60,164 @@ private:
 	MyControl curCtrl;
 
 	bool running = false;
-	std::thread tUpdate;
+	std::thread tFilter;
 	std::thread tDisplay;
 
 	std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
 
+	/** the estimated path */
+	std::vector<Point3> estPath;
+
+	/** all listeners */
+	std::vector<NavControllerListener*> listeners;
+
 public:
 
 	virtual ~NavController() {
 		if (running) {stop();}
 	}
 
+	/** ctor */
 	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);
-
+		// filter init
 		std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
 
+		// estimation
 		//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::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
 
+		// resampling
+		std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
 		//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::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
+		//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
 
+		// eval and transition
+		wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
+		std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
+		std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
 
-		std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(wifiModel));
-		std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid, &curCtrl));
-
+		// setup the filter
 		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);
+		pf->setNEffThreshold(0.75);
+		//pf->setNEffThreshold(0.65);		// still too low?
+		//pf->setNEffThreshold(0.25);		// too low
+
+		// attach as listener to all sensors
+		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);
+		SensorFactory::get().getActivity().addListener(this);
+
+		// hacky.. but we need to call this one from the main thread!
+		//mainController->getMapView()->showParticles(pf->getParticles());
+		qRegisterMetaType<const void*>();
 
 	}
 
+	/** attach a new event listener */
+	void addListener(NavControllerListener* l) {
+		listeners.push_back(l);
+	}
+
 	void start() {
+
 		Assert::isFalse(running, "already started!");
 		running = true;
-		tUpdate = std::thread(&NavController::update, this);
-		tDisplay = std::thread(&NavController::display, this);
+		curCtrl.resetAfterTransition();	// ensure we start empty ;)
+		tFilter = std::thread(&NavController::filterUpdateLoop, this);
+		tDisplay = std::thread(&NavController::updateMapViewLoop, this);
+
+		// start all sensors
+		SensorFactory::get().getAccelerometer().start();
+		SensorFactory::get().getGyroscope().start();
+		SensorFactory::get().getBarometer().start();
+		SensorFactory::get().getWiFi().start();
+
+#ifndef ANDROID
+		// #include <valgrind/callgrind.h>
+		// run with
+		// valgrind --tool=callgrind --quiet  --instr-atstart=no  ./yasmin
+		// show with
+		// kcachegrind callgrind.out.xxxx
+		CALLGRIND_START_INSTRUMENTATION;
+#endif
+
 	}
 
 	void stop() {
 		Assert::isTrue(running, "not started!");
 		running = false;
-		tUpdate.join();
+		tFilter.join();
 		tDisplay.join();
 	}
 
+
 	void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
 		(void) sensor;
-		curObs.currentTime = ts;
+		(void) data;
+		(void) ts;
+		gotSensorData(ts);
 	}
 
 	void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
 		(void) sensor;
-		curObs.currentTime = ts;
+		(void) ts;
+		(void) data;
+		gotSensorData(ts);
 	}
 
 	void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
 		(void) sensor;
-		curObs.currentTime = ts;
+		(void) ts;
+		(void) data;
+		gotSensorData(ts);
 	}
 
 	void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
 		(void) sensor;
 		(void) ts;
-		curObs.currentTime = ts;
 		curObs.wifi = data;
+		gotSensorData(ts);
 	}
 
 	void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
 		(void) sensor;
 		(void) ts;
-		curObs.currentTime = ts;
 		curObs.gps = data;
+		gotSensorData(ts);
 	}
 
 	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
+		gotSensorData(ts);
 	}
 
 	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
+		gotSensorData(ts);
+	}
+
+	void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override {
+		(void) sensor;
+		(void) ts;
+		curCtrl.activity = data.curActivity;
+		curObs.activity = data.curActivity;
+		debugActivity(data.curActivity);
+		gotSensorData(ts);
 	}
 
 	int cameraMode = 0;
@@ -165,12 +227,29 @@ public:
 
 private:
 
+	/** called when any sensor has received new data */
+	void gotSensorData(const Timestamp ts) {
+		curObs.currentTime = ts;
+		if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
+	}
+
+
+	void debugActivity(const ActivityData& activity)  {
+		QString act;
+		switch(activity.curActivity) {
+			case ActivityButterPressure::Activity::STAY:	act = "STAY"; break;
+			case ActivityButterPressure::Activity::DOWN:	act = "DOWN"; break;
+			case ActivityButterPressure::Activity::UP:		act = "UP"; break;
+			default:										act = "???"; break;
+		}
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
+	}
+
 	/** particle-filter update loop */
-	void update() {
+	void filterUpdateLoop() {
 
-		Timestamp lastTransition;
 
-		while(running) {
+		while(running && !Settings::Filter::useMainThread) {
 
 //			// fixed update rate based on the systems time -> LIVE! even for offline data
 //			const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -180,82 +259,94 @@ private:
 //			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));
-			}
+			const bool wasUpdated = filterUpdateIfNeeded();
+			if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
 
 		}
 	}
 
+	Timestamp lastTransition;
+
+	/** check whether its time for a filter update, and if so, execute the update and return true */
+	bool filterUpdateIfNeeded() {
+
+		//static float avgSum = 0;
+		//static int avgCount = 0;
+
+		// 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 >= Settings::Filter::updateEvery) {
+
+			// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
+			const Timestamp err = diff - Settings::Filter::updateEvery;
+			lastTransition = curObs.currentTime - err;
+
+			const Timestamp ts1 = Timestamp::fromUnixTime();
+			filterUpdate();
+			const Timestamp ts2 = Timestamp::fromUnixTime();
+			const Timestamp tsDiff = ts2-ts1;
+			const QString filterTime = QString::number(tsDiff.ms());
+			//avgSum += tsDiff.ms(); ++avgCount; std::cout << "ts:" << curObs.currentTime << " avg:" << (avgSum/avgCount) << std::endl;
+			QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
+			return true;
+
+		} else {
+
+			return false;
+
+		}
+
+	}
+
 
 	MyState curEst;
-	//MyState lastEst;
+	DijkstraPath<MyGridNode> pathToDest;
 
-	void doUpdate() {
+	/** perform a filter-update (called from a background-loop) */
+	void filterUpdate() {
 
 		//lastEst = curEst;
 		curEst = pf->update(&curCtrl, curObs);
+		Log::add("Nav", "cur est: " + curEst.position.asString());
 
-		// 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");
+		// inform listeners about the new estimation
+		for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}
 
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+
+		// update estimated path
+		estPath.push_back(curEst.position.inMeter());
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
 
 		PFTrans* trans = (PFTrans*)pf->getTransition();
 		const MyGridNode* node = grid->getNodePtrFor(curEst.position);
 		if (node) {
-			const DijkstraPath<MyGridNode> path = trans->modDestination.getShortestPath(*node);
+			try {
+				pathToDest = trans->modDestination.getShortestPath(*node);
+				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+			} catch (...) {;}
+		}
 		//	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;
+
+	const int display_ms = Settings::MapView::msPerFrame.ms();
 
 	/** UI update loop */
-	void display() {
+	void updateMapViewLoop() {
+
 		while(running) {
-			doDisplay();
+			const Timestamp ts1 = Timestamp::fromUnixTime();
+			updateMapView();
+			const Timestamp ts2 = Timestamp::fromUnixTime();
+			const Timestamp tsDiff = ts2-ts1;
+			const QString mapViewTime = QString::number(tsDiff.ms());
+			//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
 			std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
 		}
 	}
@@ -264,8 +355,8 @@ private:
 	Point3 curPosSlow;
 
 
-
-	void doDisplay() {
+	/** update the map-view (called from within a background-loop) */
+	void updateMapView() {
 
 		const float kappa1 = display_ms / 1000.0f;
 		const float kappa2 = kappa1 * 0.7;
@@ -278,19 +369,21 @@ private:
 		const Point3 dir = (curPosFast - curPosSlow).normalized();
 		const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized();
 
+		// how to update the camera
 		if (cameraMode == 0) {
-			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
+			mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
 		} else if (cameraMode == 1) {
-			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
+			mainController->getMapView3D()->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 spectator = curPosFast + Point3(0,0,25) - dir*15;
 			const Point3 spectatorDir = (curPosFast - spectator).normalized();
-			mainController->getMapView()->setLookEye(spectator);
-			mainController->getMapView()->setLookDir(spectatorDir);
+			mainController->getMapView3D()->setLookEye(spectator);
+			mainController->getMapView3D()->setLookDir(spectatorDir);
 		}
 
-		mainController->getMapView()->setCurrentEstimation(curPosFast, dir);
-
+		mainController->getMapView3D()->setClipAbove(curEst.position.inMeter().z + 2);
+		mainController->getMapView3D()->setCurrentEstimation(curEst.position.inMeter(), dir);
+		mainController->getMapView2D()->setCurrentEstimation(curEst.position.inMeter(), dir);
 
 	}