added support for XML reading/writing

new serialization interfaces
new helper methods
new wifi models

sensors/activity/Activity.h

@@ -0,0 +1,20 @@
+#ifndef ACTIVITY_H
+#define ACTIVITY_H
+
+enum class Activity {
+
+	STANDING,
+
+	WALKING,
+
+	WALKING_UP,
+
+	WALKING_DOWN,
+
+	ELEVATOR_UP,
+
+	ELEVATOR_DOWN,
+
+};
+
+#endif // ACTIVITY_H

sensors/activity/ActivityDetector.h

@@ -0,0 +1,161 @@
+#ifndef ACTIVITYDETECTOR_H
+#define ACTIVITYDETECTOR_H
+
+
+#include "../imu/AccelerometerData.h"
+#include "../pressure/BarometerData.h"
+
+#include "../../data/Timestamp.h"
+
+#include "../../Assertions.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../math/MovingStdDevTS.h"
+#include "../../data/HistoryTS.h"
+
+#include "../activity/Activity.h"
+
+//#define ACT_DET_DEBUG_PLOT
+
+#ifdef ACT_DET_DEBUG_PLOT
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#endif
+
+
+
+/**
+ * simple step detection based on accelerometer magnitude.
+ * magnitude > threshold? -> step!
+ * block for several msec until detecting the next one
+ */
+class ActivityDetector {
+
+private:
+
+	MovingAverageTS<float> avgLong;
+	MovingAverageTS<float> avgShort;
+
+	MovingStdDevTS<float> stdDev;
+	MovingStdDevTS<float> stdDev2;
+
+	MovingAverageTS<float> baroAvg;
+	HistoryTS<float> baroHistory;
+
+	Activity current;
+
+
+
+public:
+
+#ifdef ACT_DET_DEBUG_PLOT
+	K::Gnuplot gp;
+	K::GnuplotPlot gplot;
+	K::GnuplotPlotElementLines l1;
+	K::GnuplotPlotElementLines l2;
+#endif
+
+	/** ctor */
+	ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0),
+		stdDev(Timestamp::fromMS(150), 0), stdDev2(Timestamp::fromMS(2000), 0),
+		baroAvg(Timestamp::fromMS(500), 0), baroHistory(Timestamp::fromMS(4000)) {
+		;
+
+		#ifdef ACT_DET_DEBUG_PLOT
+			gplot.add(&l1);
+			gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000");
+		#endif
+
+	}
+
+	//int xx = 0;
+
+	/** add barometer data */
+	void add(const Timestamp ts, const BarometerData& baro) {
+		if (baro.isValid()) {
+			baroAvg.add(ts, baro.hPa);
+			const float avg = baroAvg.get();
+			baroHistory.add(ts, avg);
+			//l1.add(K::GnuplotPoint2(xx, avg));
+			update();
+		}
+	}
+
+	/** get the currently detected activity */
+	Activity get() const {
+		return current;
+	}
+
+	/** does the given data indicate a step? */
+	void add(const Timestamp ts, const AccelerometerData& acc) {
+
+		// update averages
+		avgLong.add(ts, acc.magnitude());
+		avgShort.add(ts, acc.magnitude());
+		stdDev.add(ts, acc.magnitude());
+		stdDev2.add(ts, acc.magnitude());
+
+//		const float delta = std::abs(avgLong.get() - avgShort.get());
+
+//		static int x = 0; ++x;
+
+
+//		if (delta < 0.3) {
+//			return Activity::STANDING;
+//		}
+
+//		if (avgLong.get() > 9.81+0.5) {
+//			return Activity::WALKING_UP;
+//		} else if (avgLong.get() < 9.81-0.5) {
+//			return Activity::WALKING_DOWN;
+//		}
+
+//		return Activity::WALKING;
+
+	}
+
+
+private:
+
+	/** estimate the current activity based on the sensor data */
+	void update() {
+
+		// delta in acceleration
+		const float delta_acc = std::abs(avgLong.get() - avgShort.get());
+
+		if (delta_acc < 0.015) {
+			current = Activity::STANDING;
+			return;
+		}
+
+		// delta in pressure
+		const float delta_hPa = baroHistory.getMostRecent() - baroHistory.getOldest();
+
+		#ifdef ACT_DET_DEBUG_PLOT
+			l2.add(K::GnuplotPoint2(xx, delta_hPa));
+			gp.draw(gplot);
+			gp.flush();
+			++xx;
+		#endif
+
+		if (std::abs(delta_hPa) < 0.042) {
+			current = Activity::WALKING;
+			return;
+		} else if (delta_hPa > 0) {
+			current = Activity::WALKING_DOWN;
+			return;
+		} else {
+			current = Activity::WALKING_UP;
+			return;
+		}
+
+	}
+
+
+
+
+};
+
+#endif // ACTIVITYDETECTOR_H

sensors/gps/GPSProbability.h

@@ -0,0 +1,65 @@
+#ifndef GPSPROBABILITY_H
+#define GPSPROBABILITY_H
+
+#include "GPSData.h"
+#include "../../geo/Point3.h"
+#include "../../math/distribution/Region.h"
+#include "../../geo/EarthMapping.h"
+
+class GPSProbability {
+
+private:
+
+	/** convert between map and earth */
+	const EarthMapping& em;
+
+public:
+
+	/** ctor with the map<->earth translator */
+	GPSProbability(const EarthMapping& em) : em(em) {
+		;
+	}
+
+	/** get the probability for residing at pos_m [in meter, map-coordinates] given some GPS measurement */
+	double getProbability(const Point3 pos_m, const GPSData& d) const {
+
+		// pad GPS? -> no gps eval
+		if (isBad(d)) {return 1.0;}
+
+		// adjust accuracy [sometimes strange values are provided here!]
+		float accuracy = d.accuracy;
+		if (accuracy < 3.0) {
+			std::cout << "note: adjusting gps accuracy as '" << accuracy << "'' seems invalid" << std::endl;
+			accuracy = 3.0;
+		}
+
+		// convert GPS to map coordinats
+		const Point3 gpsToMap_m = em.worldToMap(d.toEarthPos());
+
+		// distance between given point and GPS's estimation
+		const float dist = pos_m.getDistance(gpsToMap_m);
+
+		// calculate probability
+		//const double prob = Distribution::Region<double>::getProbability(0, d.accuracy, dist);
+		const double prob = Distribution::Normal<double>::getProbability(0, accuracy, dist);
+
+		// sanity checks
+		Assert::isNot0(prob, "gps probability is 0.0");
+		Assert::isNotNaN(prob, "gps probability is NaN");
+
+		// done
+		return prob;
+
+	}
+
+private:
+
+	/** returns true if the given GPS reading is bad [inaccurate, invalid, ...] */
+	static inline bool isBad(const GPSData& d) {
+		return (!d.isValid()) || (d.accuracy == 0) || (d.accuracy > 25);
+	}
+
+
+};
+
+#endif // GPSPROBABILITY_H

sensors/offline/FilePlayer.h

@@ -0,0 +1,123 @@
+#ifndef FILEPLAYER_H
+#define FILEPLAYER_H
+
+#include "FileReader.h"
+#include <thread>
+
+namespace Offline {
+
+	/**
+	 * this class can be used to "play" previously recorded [so-called "offline"] files.
+	 * one can attach itself as listener and is informed whenever new sensor data is available.
+	 * one may choose whether to use full-speed playback [as many events as possible] or
+	 * live-speed playback with the same timing the values were recorded with
+	 */
+	class FilePlayer {
+
+	private:
+
+		FileReader* reader;
+
+		bool realtime = false;
+		bool enabled;
+		std::thread thread;
+
+		/** the listener to inform */
+		Listener* listener = nullptr;
+
+	public:
+
+		/** empty ctor */
+		FilePlayer() : reader(nullptr), listener(nullptr) {
+			;
+		}
+
+		/** ctor */
+		FilePlayer(FileReader* reader, Listener* l) : reader(reader), listener(l) {
+			;
+		}
+
+		/** whether to use realtime playback or "as fast as possible" */
+		void setRealtime(const bool rt) {
+			this->realtime = rt;
+		}
+
+		/** set the offline-file-reader to use as data source */
+		void setReader(FileReader* r) {
+			this->reader = r;
+		}
+
+		/** set the event listener to inform */
+		void setListener(Listener* l) {
+			this->listener = l;
+		}
+
+		/** start playback */
+		void start() {
+
+			// sanity check
+			Assert::isNotNull(reader, "call FilePlayer::setReader() first");
+			Assert::isNotNull(listener, "call FilePlayer::setListener() first");
+			Assert::isFalse(reader->getEntries().empty(), "FileReader has no loaded entries for playback within the FilePlayer!");
+
+			enabled = true;
+			thread = std::thread(&FilePlayer::loop, this);
+
+		}
+
+		/** stop playback */
+		void stop() {
+			enabled = false;
+		}
+
+		/** wait for termination */
+		void join() {
+			thread.join();
+		}
+
+	private:
+
+		/** background loop */
+		void loop() {
+
+			// get all sensor events from the offline file
+			const std::vector<Entry> events = reader->getEntries();
+
+			// process every event
+			for (const Entry& e : events) {
+
+				// aborted?
+				if (!enabled) {break;}
+
+				// timestamp
+				const Timestamp ts = Timestamp::fromMS(e.ts);
+
+				// event index
+				const size_t idx = e.idx;
+
+#warning "some sensors todo:"
+				switch(e.type) {
+				    case Sensor::ACC:		listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
+				    case Sensor::BARO:		listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
+				    case Sensor::BEACON:	break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
+				    case Sensor::COMPASS:	listener->onCompass(ts, reader->getCompass()[idx].data); break;
+				    case Sensor::GPS:		listener->onGPS(ts, reader->getGPS()[idx].data); break;
+				    case Sensor::GRAVITY:	listener->onGravity(ts, reader->getGravity()[idx].data); break;
+				    case Sensor::GYRO:		listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
+				    case Sensor::LIN_ACC:	break;//listener->on(ts, reader->getBarometer()[idx].data); break;
+				    case Sensor::WIFI:		listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
+				    default:				throw Exception("code error. found not-yet-implemented sensor");
+				}
+
+			}
+
+			// done
+			enabled = false;
+
+		}
+
+	};
+
+}
+
+#endif // FILEPLAYER_H

sensors/radio/model/WiFiModelFactory.h

@@ -0,0 +1,32 @@
+#ifndef WIFIMODELFACTORY_H
+#define WIFIMODELFACTORY_H
+
+
+#include "WiFiModel.h"
+#include "../../../floorplan/v2/Floorplan.h"
+
+/**
+ * this class can instantiate WiFiModels based on serialized XML files
+ */
+class WiFiModelFactory {
+
+private:
+
+	Floorplan::IndoorMap* map;
+
+public:
+
+	/** ctor. needs the floorplan for some models */
+	WiFiModelFactory(Floorplan::IndoorMap* map) : map(map) {
+		;
+	}
+
+	/** parse model from XML file */
+	WiFiModel* loadXML(const std::string& file);
+
+	/** parse model from XML node */
+	WiFiModel* readFromXML(XMLDoc* doc, XMLElem* src);
+
+};
+
+#endif // WIFIMODELFACTORY_H

sensors/radio/model/WiFiModelFactoryImpl.h

@@ -0,0 +1,42 @@
+#ifndef WIFIMODELFACTORYIMPL_H
+#define WIFIMODELFACTORYIMPL_H
+
+#include "WiFiModelFactory.h"
+#include "WiFiModelLogDist.h"
+#include "WiFiModelLogDistCeiling.h"
+#include "WiFiModelPerFloor.h"
+#include "WiFiModelPerBBox.h"
+
+WiFiModel* WiFiModelFactory::loadXML(const std::string& file) {
+
+	XMLDoc doc;
+	XMLserialize::assertOK(doc.LoadFile(file.c_str()), doc, "error while loading file");
+	XMLElem* root = doc.FirstChildElement("root");
+
+	return readFromXML(&doc, root);
+
+}
+
+WiFiModel* WiFiModelFactory::readFromXML(XMLDoc* doc, XMLElem* src) {
+
+	// each model attaches its "type" during serialization
+	const std::string type = src->Attribute("type");
+
+	WiFiModel* mdl = nullptr;
+
+	// create an instance for the model
+	if		(type == "WiFiModelLogDist")		{mdl = new WiFiModelLogDist();}
+	else if	(type == "WiFiModelLogDistCeiling")	{mdl = new WiFiModelLogDistCeiling(map);}
+	else if	(type == "WiFiModelPerFloor")		{mdl = new WiFiModelPerFloor(map);}
+	else if	(type == "WiFiModelPerBBox")		{mdl = new WiFiModelPerBBox(map);}
+	else										{throw Exception("invalid model type given: " + type);}
+
+	// load the model from XML
+	mdl->readFromXML(doc, src);
+
+	// done
+	return  mdl;
+
+}
+
+#endif // WIFIMODELFACTORYIMPL_H

sensors/radio/model/WiFiModelPerBBox.h

@@ -0,0 +1,167 @@
+#ifndef WIFIMODELPERBBOX_H
+#define WIFIMODELPERBBOX_H
+
+
+#include "../AccessPoint.h"
+#include "../../../geo/Point3.h"
+#include "../../../geo/BBoxes3.h"
+#include <vector>
+
+#include "WiFiModelFactory.h"
+
+/**
+ * FOR TESTING
+ *
+ * this model allows using a different sub-models
+ * identified by a bound-box to reduce the error
+ */
+class WiFiModelPerBBox : public WiFiModel {
+
+	struct ModelForBBoxes {
+
+		WiFiModel* mdl;
+		BBoxes3 bboxes;
+
+		/** ctor */
+		ModelForBBoxes(WiFiModel* mdl, BBoxes3 bboxes) : mdl(mdl), bboxes(bboxes) {;}
+
+		/** does this entry apply to the given position? */
+		bool matches(const Point3 pt) const {
+			if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");}
+			return bboxes.contains(pt);
+		}
+
+	};
+
+	Floorplan::IndoorMap* map;
+
+	/** all contained models [one per bbox] */
+	std::vector<ModelForBBoxes> models;
+
+public:
+
+	WiFiModelPerBBox(Floorplan::IndoorMap* map) : map(map) {
+		;
+	}
+
+	/** dtor */
+	virtual ~WiFiModelPerBBox() {
+
+	}
+
+
+	/** get a list of all APs known to the model */
+	std::vector<AccessPoint> getAllAPs() const override {
+
+		// combine all submodels
+		std::vector<AccessPoint> res;
+		for (const ModelForBBoxes& sub : models) {
+			for (const AccessPoint& ap : sub.mdl->getAllAPs()) {
+				if (std::find(res.begin(), res.end(), ap) == res.end()) {	// TODO use map instead?
+					res.push_back(ap);
+				}
+			}
+		}
+		return res;
+
+	}
+
+	void add(WiFiModel* mdl, const BBoxes3 bboxes) {
+		if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");}
+		ModelForBBoxes mfb(mdl, bboxes);
+		models.push_back(mfb);
+	}
+
+	float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
+
+		for (const ModelForBBoxes& mfb : models) {
+			if (mfb.matches(position_m)) {return mfb.mdl->getRSSI(accessPoint, position_m);}
+		}
+
+		return -120;
+
+	}
+
+	void readFromXML(XMLDoc* doc, XMLElem* src) override {
+
+		// check type
+		if (std::string("WiFiModelPerBBox") != src->Attribute("type")) {throw Exception("invalid model type");}
+
+		models.clear();
+
+		// model factory [create models based on XMl content]
+		WiFiModelFactory fac(map);
+
+		// parse all contained models [one per floor]
+		XML_FOREACH_ELEM_NAMED("entry", xentry, src) {
+
+			// all bboxes
+			BBoxes3 bboxes;
+			XML_FOREACH_ELEM_NAMED("bbox", xbbox, xentry) {
+
+				const float x1 = xbbox->FloatAttribute("x1");
+				const float y1 = xbbox->FloatAttribute("y1");
+				const float z1 = xbbox->FloatAttribute("z1");
+
+				const float x2 = xbbox->FloatAttribute("x2");
+				const float y2 = xbbox->FloatAttribute("y2");
+				const float z2 = xbbox->FloatAttribute("z2");
+
+				const BBox3 bbox(Point3(x1,y1,z1), Point3(x2,y2,z2));
+				bboxes.add(bbox);
+
+			}
+
+			// node for the model
+			XMLElem* xmodel = xentry->FirstChildElement("model");
+
+			// let the factory instantiate the model
+			WiFiModel* mdl = fac.readFromXML(doc, xmodel);
+
+			// add
+			models.push_back(ModelForBBoxes(mdl, bboxes));
+
+		}
+
+	}
+
+	void writeToXML(XMLDoc* doc, XMLElem* dst) override {
+
+		// set my type
+		dst->SetAttribute("type", "WiFiModelPerBBox");
+
+		for (const ModelForBBoxes& mfb : models) {
+
+			// all models
+			XMLElem* xentry = doc->NewElement("entry"); {
+
+				// each bbox
+				for (const BBox3& bbox : mfb.bboxes.get()) {
+					XMLElem* xbbox = doc->NewElement("bbox"); {
+
+						xbbox->SetAttribute("x1", bbox.getMin().x);
+						xbbox->SetAttribute("y1", bbox.getMin().y);
+						xbbox->SetAttribute("z1", bbox.getMin().z);
+
+						xbbox->SetAttribute("x2", bbox.getMax().x);
+						xbbox->SetAttribute("y2", bbox.getMax().y);
+						xbbox->SetAttribute("z2", bbox.getMax().z);
+
+					}; xentry->InsertFirstChild(xbbox);
+				}
+
+				// the corresponding model
+				XMLElem* xmodel = doc->NewElement("model"); {
+					mfb.mdl->writeToXML(doc, xmodel);
+				}; xentry->InsertEndChild(xmodel);
+
+			}; dst->InsertEndChild(xentry);
+
+		}
+
+	}
+
+};
+
+
+#endif // WIFIMODELPERBBOX_H

sensors/radio/model/WiFiModelPerFloor.h

@@ -0,0 +1,133 @@
+#ifndef WIFIMODELPERFLOOR_H
+#define WIFIMODELPERFLOOR_H
+
+#include "../AccessPoint.h"
+#include "../../../geo/Point3.h"
+#include <vector>
+
+#include "WiFiModelFactory.h"
+
+/**
+ * FOR TESTING
+ *
+ * this model allows using a different sub-models for each floor to reduce the error
+ */
+class WiFiModelPerFloor : public WiFiModel {
+
+	struct ModelForFloor {
+
+		float fromZ;
+		float toZ;
+		WiFiModel* mdl;
+
+		/** ctor */
+		ModelForFloor(const float fromZ, const float toZ, WiFiModel* mdl) : fromZ(fromZ), toZ(toZ), mdl(mdl) {;}
+
+		/** does this entry apply to the given z-position? */
+		bool matches(const float z) const {
+			return (fromZ <= z && z < toZ);
+		}
+
+	};
+
+	Floorplan::IndoorMap* map;
+
+	/** all contained models [one per floor] */
+	std::vector<ModelForFloor> models;
+
+public:
+
+	WiFiModelPerFloor(Floorplan::IndoorMap* map) : map(map) {
+		;
+	}
+
+	/** dtor */
+	virtual ~WiFiModelPerFloor() {
+
+	}
+
+
+	/** get a list of all APs known to the model */
+	std::vector<AccessPoint> getAllAPs() const override {
+
+		// combine all submodels
+		std::vector<AccessPoint> res;
+		for (const ModelForFloor& sub : models) {
+			for (const AccessPoint& ap : sub.mdl->getAllAPs()) {
+				if (std::find(res.begin(), res.end(), ap) == res.end()) {	// TODO use map instead?
+					res.push_back(ap);
+				}
+			}
+		}
+		return res;
+
+	}
+
+	void add(WiFiModel* mdl, const Floorplan::Floor* floor) {
+		ModelForFloor mff(floor->atHeight, floor->atHeight+floor->height, mdl);
+		models.push_back(mff);
+	}
+
+	float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
+
+		for (const ModelForFloor& mff : models) {
+			if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);}
+		}
+
+		return -120;
+
+	}
+
+	void readFromXML(XMLDoc* doc, XMLElem* src) override {
+
+		// check type
+		if (std::string("WiFiModelPerFloor") != src->Attribute("type")) {throw Exception("invalid model type");}
+
+		models.clear();
+
+		// model factory [create models based on XMl content]
+		WiFiModelFactory fac(map);
+
+		// parse all contained models [one per floor]
+		XML_FOREACH_ELEM_NAMED("floor", xfloor, src) {
+
+			// floor params
+			const float z1 = xfloor->FloatAttribute("z1");
+			const float z2 = xfloor->FloatAttribute("z2");
+
+			// node for the model
+			XMLElem* xmodel = xfloor->FirstChildElement("model");
+
+			// let the factory instantiate the model
+			WiFiModel* mdl = fac.readFromXML(doc, xmodel);
+
+			// add
+			models.push_back(ModelForFloor(z1, z2, mdl));
+
+		}
+
+	}
+
+	void writeToXML(XMLDoc* doc, XMLElem* dst) override {
+
+		// set my type
+		dst->SetAttribute("type", "WiFiModelPerFloor");
+
+		for (const ModelForFloor& mff : models) {
+
+			XMLElem* xfloor = doc->NewElement("floor"); {
+				xfloor->SetAttribute("z1", mff.fromZ);
+				xfloor->SetAttribute("z2", mff.toZ);
+				XMLElem* xmodel = doc->NewElement("model"); {
+					mff.mdl->writeToXML(doc, xmodel);
+				}; xfloor->InsertEndChild(xmodel);
+			}; dst->InsertEndChild(xfloor);
+
+		}
+
+	}
+
+};
+
+
+#endif // WIFIMODELPERFLOOR_H

sensors/radio/model/WiFiModels.h

@@ -0,0 +1,8 @@
+#ifndef WIFIMODELS_H
+#define WIFIMODELS_H
+
+#include "WiFiModel.h"
+#include "WiFiModelFactory.h"
+#include "WiFiModelFactoryImpl.h"
+
+#endif // WIFIMODELS_H