merge

CMakeLists.txt

@@ -68,7 +68,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-	-O0
+        #-O0
 	-march=native
 
 	-DWITH_TESTS

data/HistoryTS.h

@@ -0,0 +1,68 @@
+#ifndef HISTORYTS_H
+#define HISTORYTS_H
+
+#include <vector>
+#include "Timestamp.h"
+#include <algorithm>
+
+/**
+ * keep the history of values for a given amount of time
+ */
+template <typename T> class HistoryTS {
+
+private:
+
+	/** timestamp -> value combination */
+	struct Entry {
+		Timestamp ts;
+		T value;
+		Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;}
+	};
+
+	/** the time-window to keep */
+	Timestamp window;
+
+	/** the value history for the window-size */
+	std::vector<Entry> history;
+
+public:
+
+
+	/** ctor with the time-window to keep */
+	HistoryTS(const Timestamp window) : window(window) {
+
+	}
+
+	/** add a new entry */
+	void add(const Timestamp ts, const T& data) {
+
+		// append to history
+		history.push_back(Entry(ts, data));
+
+		// remove too-old history entries
+		const Timestamp oldest = ts - window;
+		while(history.front().ts < oldest) {
+
+			// remove from history
+			history.erase(history.begin());
+
+		}
+
+	}
+
+	/** get the most recent entry */
+	T getMostRecent() const {
+		return history.back().value;
+	}
+
+	/** get the oldest entry available */
+	T getOldest() const {
+		return history.front().value;
+	}
+
+
+
+};
+
+
+#endif // HISTORYTS_H

data/XMLload.h

@@ -0,0 +1,14 @@
+#ifndef XMLLOAD_H
+#define XMLLOAD_H
+
+#include "xml.h"
+
+class XMLload {
+
+public:
+
+	virtual void readFromXML(XMLDoc* doc, XMLElem* src) = 0;
+
+};
+
+#endif // XMLLOAD_H

data/XMLsave.h

@@ -0,0 +1,14 @@
+#ifndef XMLSAVE_H
+#define XMLSAVE_H
+
+#include "xml.h"
+
+class XMLsave {
+
+public:
+
+	virtual void writeToXML(XMLDoc* doc, XMLElem* dst) = 0;
+
+};
+
+#endif // XMLSAVE_H

data/XMLserialize.h

@@ -0,0 +1,44 @@
+#ifndef XMLSERIALIZE_H
+#define XMLSERIALIZE_H
+
+#include "XMLload.h"
+#include "XMLsave.h"
+#include "xml.h"
+
+class XMLserialize : public XMLload, public XMLsave {
+
+public:
+
+	void loadXML(const std::string& file) {
+
+		XMLDoc doc;
+		assertOK(doc.LoadFile(file.c_str()), doc, "error while loading file");
+		XMLElem* root = doc.FirstChildElement("root");
+		readFromXML(&doc, root);
+
+	}
+
+	void saveXML(const std::string& file) {
+
+		XMLDoc doc;
+		XMLElem* root = doc.NewElement("root");
+		doc.InsertFirstChild(root);
+		writeToXML(&doc, root);
+		assertOK(doc.SaveFile(file.c_str()), doc, "error while saving file");
+
+	}
+
+public:
+
+
+	static void assertOK(XMLErr res, XMLDoc& doc, const std::string& txt) {
+		if (res != tinyxml2::XMLError::XML_SUCCESS) {
+			const std::string err = doc.ErrorName();
+			const std::string add = doc.GetErrorStr1();
+			throw Exception(txt + ": " + err + " - " + add);
+		}
+	}
+
+};
+
+#endif // XMLSERIALIZE_H

data/xml.h

@@ -0,0 +1,33 @@
+#ifndef DATA_XML_H
+#define DATA_XML_H
+
+#include "../lib/tinyxml/tinyxml2.h"
+
+using XMLDoc = tinyxml2::XMLDocument;
+using XMLErr = tinyxml2::XMLError;
+using XMLNode = tinyxml2::XMLNode;
+using XMLElem = tinyxml2::XMLElement;
+using XMLAttr = tinyxml2::XMLAttribute;
+using XMLText = tinyxml2::XMLText;
+
+#define XML_FOREACH_ATTR(attr, root) \
+for (const XMLAttr* attr = root->FirstAttribute(); attr != nullptr; attr = attr->Next())
+
+#define XML_FOREACH_NODE(sub, root) \
+for (const XMLNode* sub = root->FirstChild(); sub != nullptr; sub = sub->NextSibling())
+
+#define XML_FOREACH_ELEM(sub, root) \
+for (XMLElem* sub = (XMLElem*)root->FirstChild(); sub != nullptr; sub = (XMLElem*)sub->NextSibling())
+
+#define XML_FOREACH_ELEM_NAMED(name, sub, root) \
+for (XMLElem* sub = root->FirstChildElement(name); sub != nullptr; sub = sub->NextSiblingElement(name))
+
+#define XML_ID(node)		node->Attribute("xml:id")
+#define XML_WITH_ID(node)	node->Attribute("with_id")
+
+#define XML_ASSERT_NODE_NAME(name, node)		if (std::string(name) != std::string(node->Name())) {throw Exception("expected " + std::string(name) + " got " + node->Name());}
+
+#define XML_MANDATORY_ATTR(node, attr)		(node->Attribute(attr) ? node->Attribute(attr) : throw Exception(std::string("missing XML attribute: ") + attr));
+
+
+#endif // DATA_XML_H

debug/PlotWifiMeasurements.h

@@ -0,0 +1,76 @@
+#ifndef PLOTWIFIMEASUREMENTS_H
+#define PLOTWIFIMEASUREMENTS_H
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+
+#include "../sensors/radio/WiFiMeasurements.h"
+#include "../sensors/radio/WiFiQualityAnalyzer.h"
+
+#include <unordered_map>
+
+/**
+ * helper-class that plots incoming wifi measurements
+ * one line per AP
+ */
+class PlotWifiMeasurements {
+
+	K::Gnuplot gp;
+	K::GnuplotPlot gplot;
+	K::GnuplotPlotElementLines lineQuality;
+	std::unordered_map<MACAddress, K::GnuplotPlotElementLines*> lineForMac;
+	WiFiQualityAnalyzer quality;
+
+public:
+
+	PlotWifiMeasurements(const std::string& labelX = "time (sec)", const std::string& labelY = "dBm") {
+
+		gplot.getAxisX().setLabel(labelX);
+		gplot.getAxisY().setLabel(labelY);
+
+		// quality indicator using the 2nd y axis
+		gplot.add(&lineQuality); lineQuality.getStroke().setWidth(2); lineQuality.setUseAxis(1, 2);
+		gplot.getAxisY2().setTicsVisible(true);
+		gplot.getAxisY2().setRange(K::GnuplotAxis::Range(0, 1));
+
+	}
+
+	K::Gnuplot& getGP() {
+		return gp;
+	}
+
+	K::GnuplotPlot& getPlot() {
+		return gplot;
+	}
+
+	void add(const WiFiMeasurements& mes) {
+
+		const Timestamp ts = mes.entries.front().getTimestamp();
+
+		for (const WiFiMeasurement& m : mes.entries) {
+			const auto& it = lineForMac.find(m.getAP().getMAC());
+			if (it == lineForMac.end()) {
+				K::GnuplotPlotElementLines* line = new K::GnuplotPlotElementLines();
+				line->setTitle(m.getAP().getMAC().asString());
+				line->getStroke().getColor().setAuto();
+				lineForMac[m.getAP().getMAC()] = line;
+				gplot.add(line);
+			}
+			const K::GnuplotPoint2 gp2(m.getTimestamp().sec(), m.getRSSI());
+			lineForMac[m.getAP().getMAC()]->add(gp2);
+		}
+
+		quality.add(mes);
+		lineQuality.add(K::GnuplotPoint2(ts.sec(), quality.getQuality()));
+
+	}
+
+	void plot() {
+		gp.draw(gplot);
+		gp.flush();
+	}
+
+};
+
+#endif // PLOTWIFIMEASUREMENTS_H

floorplan/v2/Floorplan.h

@@ -9,6 +9,7 @@
 
 #include "../../geo/Point3.h"
 #include "../../geo/Point2.h"
+#include "../../geo/EarthPos.h"
 
 namespace Floorplan {
 
@@ -21,20 +22,86 @@ namespace Floorplan {
 	/** a free key-value meta element */
 	struct Meta {
 
+		struct KeyVal {
+			std::string key;
+			std::string val;
+			KeyVal() {;}
+			KeyVal(const std::string& key, const std::string& val) : key(key), val(val) {;}
+		};
+
 		const std::string EMPTY = "";
-		std::unordered_map<std::string, std::string> params;
+
+//		std::unordered_map<std::string, std::string> params;
+//		const std::string& getVal(const std::string& key) const {
+//			const auto it = params.find(key);
+//			return (it == params.end()) ? (EMPTY) : (it->second);
+//		}
+
+		std::vector<KeyVal> params;
+
+		KeyVal* getKV(const std::string& key) {
+			for (KeyVal& kv : params) {
+				if (kv.key == key) {return &kv;}
+			}
+			return nullptr;
+		}
+
+		const KeyVal* getKV(const std::string& key) const {
+			for (const KeyVal& kv : params) {
+				if (kv.key == key) {return &kv;}
+			}
+			return nullptr;
+		}
 
 		const std::string& getVal(const std::string& key) const {
-			const auto it = params.find(key);
-			return (it == params.end()) ? (EMPTY) : (it->second);
+			static const std::string EMPTY = "";
+			const KeyVal* kv = getKV(key);
+			return (kv) ? (kv->val) : (EMPTY);
+		}
+		void setVal(const std::string& key, const std::string& val) {(*this)[key] = val;}
+
+		void add(const std::string& key, const std::string& val) {params.push_back(KeyVal(key,val));}
+
+		std::string& operator [] (const std::string& key) {
+			KeyVal* kv = getKV(key);
+			if (!kv) {params.push_back(KeyVal(key, ""));}
+			return getKV(key)->val;
 		}
-		void setVal(const std::string& key, const std::string& val) {params[key] = val;}
 
 		float getFloat(const std::string& key) const			{ return std::stof(getVal(key)); }
-		void setFloat(const std::string& key, const float val)	{ params[key] = std::to_string(val); }
+		void setFloat(const std::string& key, const float val)	{ (*this)[key] = std::to_string(val); }
 
 		int getInt(const std::string& key) const				{ return std::stoi(getVal(key)); }
-		void setInt(const std::string& key, const int val)		{ params[key] = std::to_string(val); }
+		void setInt(const std::string& key, const int val)		{ (*this)[key] = std::to_string(val); }
+
+
+
+		size_t size() const {return params.size();}
+
+		const std::string& getKey(const int idx) const {return params[idx].key;}
+		const std::string& getVal(const int idx) const {return params[idx].val;}
+
+		void set(const int idx, const KeyVal& kv) {params[idx] = kv;}
+		void setKey(const int idx, const std::string& key) {params[idx].key = key;}
+		void setVal(const int idx, const std::string& val) {params[idx].val = val;}
+
+		/** delete the idx-th entry */
+		void deleteEntry(const int idx) {params.erase(params.begin()+idx);}
+
+	};
+
+	class HasMeta {
+
+		Meta* meta = nullptr;
+
+	public:
+
+		Meta* getMeta() const {return meta;}
+
+		void setMeta(Meta* meta) {
+			if (this->meta) {delete this->meta;}
+			this->meta = meta;
+		}
 
 	};
 
@@ -112,21 +179,25 @@ namespace Floorplan {
 	struct FloorObstacle;
 	struct AccessPoint;
 	struct Beacon;
+	struct FingerprintLocation;
 	struct FloorRegion;
 	struct UnderlayImage;
 	struct POI;
 	struct Stair;
 	struct Elevator;
+        struct GroundTruthPoint;
 
 	using FloorOutline = std::vector<FloorOutlinePolygon*>;
 	using FloorObstacles = std::vector<FloorObstacle*>;
 	using FloorAccessPoints = std::vector<AccessPoint*>;
 	using FloorBeacons = std::vector<Beacon*>;
+	using FloorFingerprintLocations = std::vector<FingerprintLocation*>;
 	using FloorRegions = std::vector<FloorRegion*>;
 	using FloorUnderlays = std::vector<UnderlayImage*>;
 	using FloorPOIs = std::vector<POI*>;
 	using FloorStairs = std::vector<Stair*>;
 	using FloorElevators = std::vector<Elevator*>;
+	using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>;
 
 	/** describes one floor within the map, starting at a given height */
 	struct Floor {
@@ -139,10 +210,12 @@ namespace Floorplan {
 		FloorRegions regions;                   // all regions within the floor (rooms, ...)
 		FloorAccessPoints accesspoints;
 		FloorBeacons beacons;
+		FloorFingerprintLocations fpLocations;	// potential fingerprint locations
 		FloorUnderlays underlays;				// underlay images (used for map-building)
 		FloorPOIs pois;							// POIs within the floor
 		FloorStairs stairs;						// all stairs within one floor
 		FloorElevators elevators;				// all elevators within one floor
+		FloorGroundTruthPoints gtpoints;        // all ground truth points within one floor
 		//FloorKeyValue other;					// other, free elements
 
 		Floor() {;}
@@ -155,7 +228,15 @@ namespace Floorplan {
 
 	};
 
-
+	/** location for fingerprint measurements */
+	struct FingerprintLocation : public HasMeta {
+		std::string name;
+		Point2 posOnFloor;
+		float heightAboveFloor = 0;
+		FingerprintLocation() {;}
+		FingerprintLocation(const std::string& name, const Point2 posOnFloor, const float heightAboveFloor) : name(name), posOnFloor(posOnFloor), heightAboveFloor(heightAboveFloor) {;}
+		Point3 getPosition(const Floor& floor) const {return Point3(posOnFloor.x, posOnFloor.y, floor.atHeight + heightAboveFloor);}
+	};
 
 	/** a POI located somewhere on a floor */
 	struct POI {
@@ -167,15 +248,25 @@ namespace Floorplan {
 		bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);}
 	};
 
+        /** a GroundTruthPoint located somewhere on a floor */
+        struct GroundTruthPoint {
+            int id; //TODO: this value can be changed and isn't set incremental within the indoormap
+			Point3 pos;		// TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
+            GroundTruthPoint() : id(), pos() {;}
+			GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;}
+			const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
+            bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
+        };
+
 	/** an AccessPoint located somewhere on a floor */
-	struct AccessPoint {
+	struct AccessPoint : public HasMeta {
 		std::string name;
 		std::string mac;
 		Point3 pos;		// z is relative to the floor's height
 		struct Model {
-			float txp;
-			float exp;
-			float waf;
+			float txp = 0;
+			float exp = 0;
+			float waf = 0;
 		} model;
 		AccessPoint() : name(), mac(), pos() {;}
 		AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;}
@@ -199,6 +290,7 @@ namespace Floorplan {
 		Beacon() : name(), mac(), pos() {;}
 		Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
 		bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
+                Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);}	// relative to the floor's ground
 	};
 
 
@@ -207,15 +299,13 @@ namespace Floorplan {
 		OutlineMethod method;
 		std::string name;
 		Polygon2 poly;
-		FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly() {;}
-		FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly) : method(method), name(name), poly(poly) {;}
-		bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly);}
+		bool outdoor;	// special marker
+		FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly(), outdoor(false) {;}
+		FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly, bool outdoor) : method(method), name(name), poly(poly), outdoor(outdoor) {;}
+		bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly) && (o.outdoor == outdoor);}
 	};
 
 
-
-
-
 	/** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */
 	struct FloorObstacle {
 		Material material;
@@ -229,8 +319,9 @@ namespace Floorplan {
 		ObstacleType type;
 		Point2 from;
 		Point2 to;
-		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to) {;}
-		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2) {;}
+		float thickness_m;
+		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m) {;}
 	};
 
 	/** circle obstacle */
@@ -388,8 +479,7 @@ namespace Floorplan {
 
 
 	/** base-class for stairs */
-	struct Stair {
-		Meta* meta = nullptr;
+	struct Stair : public HasMeta {
 		virtual std::vector<StairPart> getParts() const = 0;
 	};
 
@@ -440,7 +530,7 @@ namespace Floorplan {
 
 
 	/** an image file that can be added to the map */
-		struct UnderlayImage {
+	struct UnderlayImage {
 		std::string name;
 		std::string filename;
 		Point2 anchor;
@@ -450,16 +540,47 @@ namespace Floorplan {
 
 
 
+	/** one correspondence point: earth <-> map */
+	struct EarthPosMapPos {
+
+		EarthPos posOnEarth;
+
+		Point3 posOnMap_m;
+
+		/** empty ctor */
+		EarthPosMapPos() : posOnEarth(), posOnMap_m() {;}
+
+		/** ctor */
+		EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;}
+
+	};
+
+
+	/** describe the floorplan's location on earth */
+	struct EarthRegistration {
+
+		/** all available correspondences: earth <-> map */
+		std::vector<EarthPosMapPos*> correspondences;
+
+	};
+
 	/** describes the whole indoor map */
 	struct IndoorMap {
 
 		float width;
 		float depth;
+
 		std::vector<Floor*> floors;
 
+		/** mapping: floorplan <-> earth */
+		EarthRegistration earthReg;
+
 		IndoorMap() {;}
 
+		/** no copy */
 		IndoorMap(const IndoorMap& o) = delete;
+
+		/** no copy assign */
 		void operator = (const IndoorMap& o) = delete;
 
 	};

floorplan/v2/FloorplanCeilings.h

@@ -0,0 +1,153 @@
+#ifndef FLOORPLANCEILINGS_H
+#define FLOORPLANCEILINGS_H
+
+#include "Floorplan.h"
+
+namespace Floorplan {
+
+	/**
+	 * helper-class for floorplan ceilings
+	 * e.g. to determine the number of ceilings between two given positions
+	 */
+	class Ceilings {
+
+	private:
+
+		/** position (height) of all ceilings (in meter) */
+		std::vector<float> ceilingsAtHeight_m;
+
+	public:
+
+		/** empty ctor */
+		Ceilings() {
+			;
+		}
+
+		/** ctor with the map to work with */
+		Ceilings(const IndoorMap* map) {
+
+			// sanity checks
+			Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
+
+			// get position of all ceilings
+			for (Floorplan::Floor* f : map->floors) {
+
+				const float h1 = f->atHeight;
+				const float h2 = f->atHeight + f->height;
+
+				if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h1) == ceilingsAtHeight_m.end()) {
+					ceilingsAtHeight_m.push_back(h1);
+				}
+				if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h2) == ceilingsAtHeight_m.end()) {
+					ceilingsAtHeight_m.push_back(h2);
+				}
+
+			}
+
+		}
+
+		std::vector<float> getCeilings() const {
+			return ceilingsAtHeight_m;
+		}
+
+		void addCeiling(const float height_m) {
+			ceilingsAtHeight_m.push_back(height_m);
+		}
+
+		void clear() {
+			ceilingsAtHeight_m.clear();
+		}
+
+		/** get a fading number of floors between ap and person using sigmod in the area where the ceiling is */
+		float numCeilingsBetweenFloat(const Point3 ap, const Point3 person) const {
+
+			// sanity checks
+			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
+
+			// fading between floors using sigmoid
+			const float near = 1.0;
+			float cnt = 0;
+
+
+			for (const float z : ceilingsAtHeight_m) {
+
+				const float myDistToCeil = (ap.z < person.z) ? (person.z - z) : (z - person.z);
+				if ( std::abs(myDistToCeil) < near ) {
+					cnt += sigmoid(myDistToCeil * 6);
+				} else if (ap.z < z && person.z >= z+near) {	// AP below celing, me above ceiling
+					cnt += 1;
+				} else if (ap.z > z && person.z <= z-near) {	// AP above ceiling, me below ceiling
+					cnt += 1;
+				}
+
+			}
+
+			return cnt;
+
+		}
+
+		float numCeilingsBetweenLinearInt(const Point3 ap, const Point3 person) const {
+
+			// sanity checks
+			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
+
+			int cnt = 0;
+			float sum = 0;
+			for (float z = -1.0; z <= +1.0; z+= 0.25) {
+				sum += numCeilingsBetween(ap, person + Point3(0,0,z));
+				++cnt;
+			}
+
+			return sum/cnt;
+
+		}
+
+		/** get the number of ceilings between z1 and z2 */
+		int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
+
+			// sanity checks
+			Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?");
+
+			// find min and max height given the to-be-compared points
+			const float zMin = std::min(pos1.z, pos2.z);
+			const float zMax = std::max(pos1.z, pos2.z);
+
+	#ifdef WITH_ASSERTIONS
+
+            static uint64_t numNear = 0;
+            static uint64_t numFar = 0;
+
+			for (const float z : ceilingsAtHeight_m) {
+				const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
+				if (diff < 0.1) {++numNear;} else {++numFar;}
+			}
+			if ((numNear + numFar) > 150000) {
+				Assert::isTrue(numNear < numFar*0.5,
+				    "many requests to Floorplan::Ceilings::numCeilingsBetween address nodes (very) near to a ground! \
+					due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
+					expect very wrong outputs! \
+					consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
+				);
+			}
+	#endif
+
+			int cnt = 0;
+			for (const float z : ceilingsAtHeight_m) {
+				if (zMin < z && zMax > z) {++cnt;}
+			}
+
+			return cnt;
+
+		}
+
+	private:
+
+		static inline float sigmoid(const float val) {
+			return 1.0f / (1.0f + std::exp(-val));
+		}
+
+	};
+
+}
+
+#endif // FLOORPLANCEILINGS_H

floorplan/v2/FloorplanHelper.h

@@ -7,11 +7,64 @@
 
 #include "Floorplan.h"
 
+#include "../../sensors/MACAddress.h"
+
 /**
  * helper methods for the floorplan
  */
 class FloorplanHelper {
 
+public:
+
+
+	/** get the AP for the given MAC [if available] */
+	static std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> getAP(const Floorplan::IndoorMap* map, const MACAddress& mac) {
+		for (Floorplan::Floor* f : map->floors) {
+			for (Floorplan::AccessPoint* ap : f->accesspoints) {
+				if (MACAddress(ap->mac) == mac) {
+					return std::make_pair(ap, f);
+				}
+			}
+		}
+		return std::make_pair(nullptr, nullptr);
+	}
+
+	/** get all APs within the map */
+	static std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> getAPs(const Floorplan::IndoorMap* map) {
+		std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> res;
+		for (Floorplan::Floor* f : map->floors) {
+			for (Floorplan::AccessPoint* ap : f->accesspoints) {
+				res.push_back(std::make_pair(ap,f));
+			}
+		}
+		return res;
+	}
+
+	/** get all ground-truth points within the map as hash-map: id->pos */
+	static std::unordered_map<int, Point3> getGroundTruthPoints(const Floorplan::IndoorMap* map) {
+		std::unordered_map<int, Point3> res;
+		for (const Floorplan::Floor* f : map->floors) {
+			for (const Floorplan::GroundTruthPoint* gtp : f->gtpoints) {
+				res[gtp->id] = gtp->getPosition(*f);
+			}
+		}
+		return res;
+	}
+
+	/** get all ground-truth points, identified by the given indices */
+	static std::vector<Point3> getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> indices) {
+
+		// get a map id->pos for all ground-truth-points within the map
+		const std::unordered_map<int, Point3> src = getGroundTruthPoints(map);
+		std::vector<Point3> res;
+		for (const int idx : indices) {
+			const auto& it = src.find(idx);
+			if (it == src.end()) {throw Exception("map does not contain a ground-truth-point with ID " + std::to_string(idx));}
+			res.push_back(it->second);
+		}
+		return res;
+	}
+
 public:
 
 	/** align all floorplan values to the given grid size. needed for the grid factory */

floorplan/v2/FloorplanLINT.h

@@ -0,0 +1,228 @@
+#ifndef FLOORPLANLINT_H
+#define FLOORPLANLINT_H
+
+#include "Floorplan.h"
+#include "../../geo/BBox2.h"
+
+#include <iostream>
+#include <vector>
+
+namespace Floorplan {
+
+	class LINT {
+
+	public:
+
+		/** possible issue types */
+		enum class Type {
+			WARN,
+			ERROR,
+		};
+
+		/** type -> string */
+		static std::string getTypeStr(const Type t) {
+			switch(t) {
+				case Type::WARN: return "WARNING";
+				case Type::ERROR: return "ERROR";
+				default: throw Exception("code error. invalid type. todo!");
+			}
+		}
+
+		/** a detected issue */
+		struct Issue {
+
+			Type type;
+			const Floor* floor;
+			std::string desc;
+
+			Issue(const Type type, const Floor* floor, const std::string& desc) : type(type), floor(floor), desc(desc) {;}
+
+			/** issue to string */
+			std::string asString() const {
+				return getTypeStr(type) + ": " + "floor '" + floor->name + "': " + desc;
+			}
+
+		};
+
+		using Issues = std::vector<Issue>;
+
+
+
+	public:
+
+		/** throw in case of errors within the map */
+		static void assertOK(IndoorMap* map) {
+
+			const Issues issues = check(map);
+			int err = 0;
+			for (const Issue& i : issues) {
+				std::cout << i.asString() << std::endl;
+				if (i.type == Type::ERROR) {++err;}
+			}
+			if (err > 0) {
+				throw Exception("detected floorplan errors");
+			}
+
+		}
+
+		/** get all errors within the map */
+		static Issues check(IndoorMap* map) {
+
+			Issues res;
+
+			for (const Floor* floor : map->floors) {
+
+				// outline present?
+				if (floor->outline.empty()) {
+					res.push_back(Issue(Type::ERROR, floor, "has no outline"));
+				}
+
+				// check outline
+				for (const FloorOutlinePolygon* poly : floor->outline) {
+					checkOutline(res, floor, poly);
+				}
+
+				// check obstacles
+				for (const FloorObstacle* obs : floor->obstacles) {
+					checkObstacle(res, floor, obs);
+				}
+
+				// check fingerprints
+				for (const FingerprintLocation* fpl : floor->fpLocations) {
+					checkFingerprintLoc(res, floor, fpl);
+				}
+
+				// check stairs
+				for (const Stair* s : floor->stairs) {
+					checkStair(res, floor, s);
+				}
+
+				// check elevators
+				for (const Elevator* e : floor->elevators) {
+					checkElevator(res, floor, e);
+				}
+
+			}
+
+			// done
+			return res;
+
+		}
+
+	private:
+
+		/** check a floor's outline */
+		static void checkOutline(Issues& res, const Floor* floor, const FloorOutlinePolygon* poly) {
+
+			// number of points valid?
+			if (poly->poly.points.size() < 3)		{res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' needs at least 3 edges"));}
+			if (poly->poly.points.size() > 1024)	{res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' has too many edges"));}
+
+			// outline size [bbox] valid?
+			BBox2 outline;
+			for (const Point2 pt : poly->poly.points) { outline.add(pt); }
+			const Point2 size = outline.getSize();
+			if (size.x < 1.0 || size.y < 1.0) {res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' seems too small"));}
+
+		}
+
+		/** check walls, doors, ... */
+		static void checkObstacle(Issues& res, const Floor* floor, const FloorObstacle* fo) {
+
+			// line? -> check
+			const FloorObstacleLine* line = dynamic_cast<const FloorObstacleLine*>(fo);
+			if (line) {
+				const float len_m = line->from.getDistance(line->to);
+				if (len_m < 0.15) {
+					res.push_back(Issue(Type::WARN, floor, "' line-obstacle is too short: " + std::to_string(len_m) + " meter from " + line->from.asString() + " to " + line->to.asString()));
+				}
+			}
+
+			// door? -> check
+			const FloorObstacleDoor* door = dynamic_cast<const FloorObstacleDoor*>(fo);
+			if (door) {
+				const float len_m = door->from.getDistance(door->to);
+				if (len_m < 0.40) {
+					res.push_back(Issue(Type::ERROR, floor, "' door is too narrow: " + std::to_string(len_m) + " meter from " + door->from.asString() + " to " + door->to.asString()));
+				}
+			}
+
+			// pillar? -> check
+			const FloorObstacleCircle* circle = dynamic_cast<const FloorObstacleCircle*>(fo);
+			if (circle) {
+				const float len_m = circle->radius;
+				if (len_m < 0.20) {
+					res.push_back(Issue(Type::ERROR, floor, "' pillar is too narrow: " + std::to_string(len_m) + " meter at " + circle->center.asString()));
+				}
+			}
+
+		}
+
+		static void checkFingerprintLoc(Issues& res, const Floor* floor, const FingerprintLocation* fpl) {
+
+			const std::string note = "does it belong to a stair? if so: fine! Note: fingerprints are currently measured using smartphones and smartphone are held by the pedestian. thus: fingerprints are ~1.3 meter above ground";
+			if (fpl->heightAboveFloor < 0.8) {
+				res.push_back(Issue(Type::ERROR, floor, std::string() + "fingerprint " + fpl->name + " @ " + fpl->getPosition(*floor).asString() + " is too near to the floor. " + note));
+			} else if (fpl->heightAboveFloor > 2.0) {
+				res.push_back(Issue(Type::WARN, floor, std::string() + "fingerprint " + fpl->name + " @ " + fpl->getPosition(*floor).asString() + " is too high above the floor. " + note));
+			}
+
+		}
+
+		static void checkStair(Issues& res, const Floor* floor, const Stair* stair) {
+
+			if (stair->getParts().empty()) {
+				res.push_back(Issue(Type::ERROR, floor, "stair does not contain any parts! [empty stair]"));
+				return;
+			}
+
+			const std::vector<Floorplan::StairPart> parts = stair->getParts();
+			const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
+
+			const Floorplan::Quad3& quadS = quads.front();
+			const Floorplan::Quad3& quadE = quads.back();
+
+			// disconnected start?
+			if (quadS.p1.z != floor->getStartingZ()) {
+				res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the starting floor's ground! [open stair start]"));
+			}
+
+			// disconnected end?
+			if (quadE.p3.z != floor->getEndingZ()) {
+				res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the ending floor's ground! [open stair end]"));
+			}
+
+
+			for (int i = 0; i < (int) parts.size(); ++i) {
+
+				//const Floorplan::Quad3& quad = quads[i];
+
+				// disconnected within?
+				if (i > 0) {
+					if (quads[i-1].p4.z != quads[i-0].p1.z) {
+						res.push_back(Issue(Type::ERROR, floor, "stair is disconnected within!"));
+					}
+				}
+
+			}
+
+		}
+
+		static void checkElevator(Issues& res, const Floor* floor, const Elevator* e) {
+
+			if (e->depth < 0.5) {
+				res.push_back(Issue(Type::ERROR, floor, "elevator's depth @" + e->center.asString() + " is too small: " + std::to_string(e->depth) + "m"));
+			}
+
+			if (e->width < 0.5) {
+				res.push_back(Issue(Type::ERROR, floor, "elevator's width @" + e->center.asString() + " is too small: " + std::to_string(e->width) + "m"));
+			}
+
+		}
+
+	};
+
+}
+
+
+#endif // FLOORPLANLINT_H

floorplan/v2/FloorplanReader.h

@@ -4,6 +4,7 @@
 #include <iostream>
 
 #include "Floorplan.h"
+#include "FloorplanLINT.h"
 
 #include "../../misc/Debug.h"
 #include "../../Assertions.h"
@@ -13,6 +14,7 @@ namespace Floorplan {
 
 	using XMLAttr = tinyxml2::XMLAttribute;
 	using XMLElem = tinyxml2::XMLElement;
+	using XMLNode = tinyxml2::XMLNode;
 
 	/**
 	 * read an IndoorMaps from XML-data
@@ -31,8 +33,15 @@ namespace Floorplan {
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.LoadFile(file.c_str());
-			if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);}
-			return parse(doc);
+			if (res != tinyxml2::XMLError::XML_SUCCESS) {
+				throw Exception(
+					std::string() + "error while loading XML " + file + "\n" +
+				    ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+				    ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+				);
+			}
+			IndoorMap* map = parse(doc);
+			return map;
 		}
 
 		/** read an IndoorMap from the given XMl-string */
@@ -41,13 +50,20 @@ namespace Floorplan {
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length());
-			if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");}
-			return parse(doc);
+			if (res != tinyxml2::XMLError::XML_SUCCESS) {
+				throw Exception(
+					std::string() + "error while parsing XML\n" +
+				    ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+				    ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+				);
+			}
+			IndoorMap* map = parse(doc);
+			return map;
 		}
 
 	private:
 
-	#define FOREACH_NODE(out, in)	for( const XMLElem* out = (XMLElem*) in->FirstChild(); out; out = (XMLElem*) out->NextSibling() )
+    #define FOREACH_NODE(out, in)	for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() )
 
 		static void assertNode(const std::string& node, const XMLElem* el) {
 			std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'";
@@ -56,7 +72,7 @@ namespace Floorplan {
 
 		/** parse the complete document */
 		static IndoorMap* parse(tinyxml2::XMLDocument& doc) {
-			return parseMap((XMLElem*)doc.FirstChild());
+			return parseMap(doc.FirstChildElement());
 		}
 
 		/** parse the <map> node */
@@ -65,12 +81,36 @@ namespace Floorplan {
 			IndoorMap* map = new IndoorMap();
 			map->width = el->FloatAttribute("width");
 			map->depth = el->FloatAttribute("depth");
+
 			FOREACH_NODE(n, el) {
-				if (std::string("floors") == n->Name()) {map->floors = parseFloors(n);}
+				if (std::string("floors") == n->Name())		{map->floors = parseFloors(n);}
+				if (std::string("earthReg") == n->Name())	{map->earthReg = parseEarthReg(n);}
 			}
 			return map;
 		}
 
+		/** parse the <earthReg> node */
+		static EarthRegistration parseEarthReg(const XMLElem* el) {
+			EarthRegistration reg;
+			FOREACH_NODE(n, el) {
+				if (std::string("correspondences") == n->Name()) {
+					FOREACH_NODE(n2, n) {
+						if (std::string("point") == n2->Name()) {
+							Floorplan::EarthPosMapPos* pos = new Floorplan::EarthPosMapPos();
+							pos->posOnMap_m.x = n2->FloatAttribute("mx");
+							pos->posOnMap_m.y = n2->FloatAttribute("my");
+							pos->posOnMap_m.z = n2->FloatAttribute("mz");
+							pos->posOnEarth.lat = n2->FloatAttribute("lat");
+							pos->posOnEarth.lon = n2->FloatAttribute("lon");
+							pos->posOnEarth.height = n2->FloatAttribute("alt");
+							reg.correspondences.push_back(pos);
+						}
+					}
+				}
+			}
+			return reg;
+		}
+
 		/** parse the <floors> node */
 		static std::vector<Floor*> parseFloors(const XMLElem* el) {
 			std::vector<Floor*> floors;
@@ -92,11 +132,13 @@ namespace Floorplan {
 				if (std::string("obstacles") == n->Name())		{floor->obstacles = parseFloorObstacles(n);}
 				if (std::string("accesspoints") == n->Name())	{floor->accesspoints = parseFloorAccessPoints(n);}
 				if (std::string("beacons") == n->Name())		{floor->beacons = parseFloorBeacons(n);}
+				if (std::string("fingerprints") == n->Name())	{floor->fpLocations = parseFingerprintLocations(n);}
 				if (std::string("regions") == n->Name())		{floor->regions = parseFloorRegions(n);}
 				if (std::string("underlays") == n->Name())		{floor->underlays = parseFloorUnderlays(n);}
 				if (std::string("pois") == n->Name())			{floor->pois = parseFloorPOIs(n);}
 				if (std::string("stairs") == n->Name())			{floor->stairs = parseFloorStairs(n);}
 				if (std::string("elevators") == n->Name())		{floor->elevators = parseFloorElevators(n);}
+                                if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
 			}
 			return floor;
 		}
@@ -157,9 +199,7 @@ namespace Floorplan {
 
 			// stair meta information?
 			const XMLElem* meta = el->FirstChildElement("meta");
-			if (meta) {
-				stair->meta = parseMetaElement(meta);
-			}
+			if (meta) {stair->setMeta(parseMetaElement(meta));}
 
 			return stair;
 
@@ -185,6 +225,24 @@ namespace Floorplan {
 		}
 
 
+                /** parse the <gtpoints> tag */
+                static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
+                    std::vector<GroundTruthPoint*> vec;
+                    FOREACH_NODE(n, el) {
+                        if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
+                    }
+                    return vec;
+                }
+
+                /** parse a <gtpoint> tag */
+                static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
+                    GroundTruthPoint* gtp = new GroundTruthPoint();
+                    gtp->id = el->IntAttribute("id");
+                    gtp->pos = parsePoint3(el);
+                    return gtp;
+                }
+
+
 		/** parse the <accesspoints> tag */
 		static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) {
 			std::vector<AccessPoint*> vec;
@@ -227,10 +285,18 @@ namespace Floorplan {
 		/** parse one <meta> element */
 		static Meta* parseMetaElement(const XMLElem* n) {
 			Meta* elem = new Meta();
-			const XMLAttr* attr = n->FirstAttribute();
-			while (attr) {
-				elem->params[attr->Name()] = attr->Value();
-				attr = attr->Next();
+//			const XMLAttr* attr = n->FirstAttribute();
+//			while (attr) {
+//				elem->setVal(attr->Name(), attr->Value());
+//				attr = attr->Next();
+//			}
+			const XMLElem* sub = n->FirstChildElement();
+			while(sub) {
+				// <entry key="123">abc</entry>
+				const std::string key = sub->Attribute("key");
+				const std::string val = sub->GetText();
+				elem->add(key, val);
+				sub = sub->NextSiblingElement();
 			}
 			return elem;
 		}
@@ -244,6 +310,8 @@ namespace Floorplan {
 			ap->model.txp = n->FloatAttribute("mdl_txp");
 			ap->model.exp = n->FloatAttribute("mdl_exp");
 			ap->model.waf = n->FloatAttribute("mdl_waf");
+			const XMLElem* meta = n->FirstChildElement("meta");
+			if (meta) {ap->setMeta(parseMetaElement(meta));}
 			return ap;
 		}
 
@@ -262,9 +330,9 @@ namespace Floorplan {
 			Beacon* b = new Beacon();
 			b->mac = n->Attribute("mac");
 			b->name = n->Attribute("name");
-			b->major = n->Attribute("major");
-			b->minor = n->Attribute("minor");
-			b->uuid = n->Attribute("uuid");
+			b->major = n->Attribute("major") ? n->Attribute("major") : "";
+			b->minor = n->Attribute("minor") ? n->Attribute("minor") : "";
+			b->uuid = n->Attribute("uuid") ? n->Attribute("uuid") : "";
 			b->model.txp = n->FloatAttribute("mdl_txp");
 			b->model.exp = n->FloatAttribute("mdl_exp");
 			b->model.waf = n->FloatAttribute("mdl_waf");
@@ -272,6 +340,28 @@ namespace Floorplan {
 			return b;
 		}
 
+		/** parse <fingerprints> <location>s */
+		static std::vector<FingerprintLocation*> parseFingerprintLocations(const XMLElem* el) {
+			assertNode("fingerprints", el);
+			std::vector<FingerprintLocation*> vec;
+			FOREACH_NODE(n, el) {
+				if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); }
+			}
+			return vec;
+		}
+
+		/** parse one fingerprint <location> */
+		static FingerprintLocation* parseFingerprintLocation(const XMLElem* n) {
+			assertNode("location", n);
+			FingerprintLocation* fpl = new FingerprintLocation();
+			fpl->name = n->Attribute("name");
+			fpl->posOnFloor.x = n->FloatAttribute("x");
+			fpl->posOnFloor.y = n->FloatAttribute("y");
+			fpl->heightAboveFloor = n->FloatAttribute("dz");
+			const XMLElem* meta = n->FirstChildElement("meta");
+			if (meta) {fpl->setMeta(parseMetaElement(meta));}
+			return fpl;
+		}
 
 		static std::vector<FloorRegion*> parseFloorRegions(const XMLElem* el) {
 			std::vector<FloorRegion*> vec;
@@ -312,7 +402,8 @@ namespace Floorplan {
 				parseObstacleType(el->Attribute("type")),
 				parseMaterial(el->Attribute("material")),
 				el->FloatAttribute("x1"), el->FloatAttribute("y1"),
-				el->FloatAttribute("x2"), el->FloatAttribute("y2")
+				el->FloatAttribute("x2"), el->FloatAttribute("y2"),
+				(el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15)	// default wall thickness in m
 			);
 		}
 
@@ -341,7 +432,7 @@ namespace Floorplan {
 			FloorOutline outline;
 			FOREACH_NODE(n, el) {
 				if (std::string("polygon") == n->Name()) {
-					outline.push_back(parseFloorPolygon(n));       // TODO
+					outline.push_back(parseFloorPolygon(n));
 				}
 			}
 			return outline;
@@ -351,6 +442,7 @@ namespace Floorplan {
 		static FloorOutlinePolygon* parseFloorPolygon(const XMLElem* el) {
 			FloorOutlinePolygon* poly = new FloorOutlinePolygon();
 			poly->name = el->Attribute("name");
+			poly->outdoor = el->BoolAttribute("outdoor");
 			poly->method = parseOutlineMethod(el->Attribute("method"));
 			poly->poly = parsePoly2(el);
 			return poly;

floorplan/v2/FloorplanWriter.h

@@ -49,11 +49,34 @@ namespace Floorplan {
 			root->SetAttribute("width", map->width);
 			root->SetAttribute("depth", map->depth);
 
+			// add earth registration to the map
+			addEarthReg(doc, root, map);
+
 			// add all floors to the map
 			addFloors(doc, root, map);
 
 		}
 
+		/** add earth registration to the map */
+		static void addEarthReg(XMLDoc& doc, XMLElem* root, const IndoorMap* map) {
+			XMLElem* earthReg = doc.NewElement("earthReg"); {
+				XMLElem* correspondences = doc.NewElement("correspondences");
+				for (const Floorplan::EarthPosMapPos* reg : map->earthReg.correspondences) {
+					XMLElem* point = doc.NewElement("point");
+					point->SetAttribute("lat", reg->posOnEarth.lat);
+					point->SetAttribute("lon", reg->posOnEarth.lon);
+					point->SetAttribute("alt", reg->posOnEarth.height);
+					point->SetAttribute("mx", reg->posOnMap_m.x);
+					point->SetAttribute("my", reg->posOnMap_m.y);
+					point->SetAttribute("mz", reg->posOnMap_m.z);
+					correspondences->InsertEndChild(point);
+				}
+				earthReg->InsertEndChild(correspondences);
+			} root->InsertEndChild(earthReg);
+		}
+
+
+
 		/** add all floors to the map */
 		static void addFloors(XMLDoc& doc, XMLElem* root, const IndoorMap* map) {
 			XMLElem* floors = doc.NewElement("floors");
@@ -105,7 +128,7 @@ namespace Floorplan {
 			XMLElem* stairs = doc.NewElement("stairs");
 			for (const Stair* _stair : mf->stairs) {
 				XMLElem* elem = doc.NewElement("stair");
-				addMetaElement(doc, elem, _stair->meta);
+				addMetaElement(doc, elem, _stair->getMeta());
 				if (dynamic_cast<const StairFreeform*>(_stair)) {
 					const StairFreeform* stair = (StairFreeform*) _stair;
 					elem->SetAttribute("type", 0);		// TODO: other types?
@@ -131,6 +154,7 @@ namespace Floorplan {
 
 		}
 
+
 		/** add all sorts of POI to the floor */
 		static void addFloorPOI(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
 
@@ -145,6 +169,17 @@ namespace Floorplan {
 			}
 			floor->InsertEndChild(pois);
 
+                        XMLElem* gtpoints = doc.NewElement("gtpoints");
+                        for (const GroundTruthPoint* gtp : mf->gtpoints) {
+                            XMLElem* elem = doc.NewElement("gtpoint");
+                            elem->SetAttribute("id", gtp->id);
+                            elem->SetAttribute("x", gtp->pos.x);
+                            elem->SetAttribute("y", gtp->pos.y);
+                            elem->SetAttribute("z", gtp->pos.z);
+                            gtpoints->InsertEndChild(elem);
+                        }
+                        floor->InsertEndChild(gtpoints);
+
 			XMLElem* accesspoints = doc.NewElement("accesspoints");
 			for (const AccessPoint* ap : mf->accesspoints) {
 				XMLElem* accesspoint = doc.NewElement("accesspoint");
@@ -156,6 +191,7 @@ namespace Floorplan {
 				accesspoint->SetAttribute("mdl_txp", ap->model.txp);
 				accesspoint->SetAttribute("mdl_exp", ap->model.exp);
 				accesspoint->SetAttribute("mdl_waf", ap->model.waf);
+				addMetaElement(doc, accesspoint, ap->getMeta());
 				accesspoints->InsertEndChild(accesspoint);
 			}
 			floor->InsertEndChild(accesspoints);
@@ -179,6 +215,18 @@ namespace Floorplan {
 			floor->InsertEndChild(beacons);
 
 
+			XMLElem* fingerprints = doc.NewElement("fingerprints");
+			for (const FingerprintLocation* fpl : mf->fpLocations) {
+				XMLElem* efpl = doc.NewElement("location");
+				efpl->SetAttribute("name", fpl->name.c_str());
+				efpl->SetAttribute("x", fpl->posOnFloor.x);
+				efpl->SetAttribute("y", fpl->posOnFloor.y);
+				efpl->SetAttribute("dz", fpl->heightAboveFloor);
+				addMetaElement(doc, efpl, fpl->getMeta());
+				fingerprints->InsertEndChild(efpl);
+			}
+			floor->InsertEndChild(fingerprints);
+
 		}
 
 		static void addFloorOutline(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
@@ -203,6 +251,7 @@ namespace Floorplan {
 			XMLElem* polygon = doc.NewElement("polygon");
 			polygon->SetAttribute("name", poly->name.c_str());
 			polygon->SetAttribute("method", method.c_str());
+			polygon->SetAttribute("outdoor", poly->outdoor);
 			dst->InsertEndChild(polygon);
 
 			for (Point2 p : poly->poly.points) {
@@ -219,8 +268,15 @@ namespace Floorplan {
 		static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) {
 			if (meta == nullptr) {return;}	// nothing to add
 			XMLElem* elem = doc.NewElement("meta");
-			for (auto it : meta->params) {
-				elem->Attribute(it.first.c_str(), it.second.c_str());
+//			for (auto it : meta->params) {
+//				elem->Attribute(it.first.c_str(), it.second.c_str());
+//			}
+			for (const Meta::KeyVal& kv : meta->params) {
+				// <entry key="123">abc</entry>
+				XMLElem* subElem = doc.NewElement("entry");
+				subElem->SetAttribute("key", kv.key.c_str());
+				subElem->SetText(kv.val.c_str());
+				elem->InsertEndChild(subElem);
 			}
 			other->InsertEndChild(elem);
 		}
@@ -274,6 +330,7 @@ namespace Floorplan {
 			obstacle->SetAttribute("y1", line->from.y);
 			obstacle->SetAttribute("x2", line->to.x);
 			obstacle->SetAttribute("y2", line->to.y);
+			obstacle->SetAttribute("thickness", line->thickness_m);
 			obstacles->InsertEndChild(obstacle);
 		}
 

geo/Angle.h

@@ -29,6 +29,13 @@ public:
 		return radToDeg(getRAD_2PI(x1,y1,x2,y2));
 	}
 
+	/** ensure the given radians-value is within [0:2pi] */
+	static float makeSafe_2PI(float rad) {
+		while(rad < 0) {rad += 2*PI;}
+		while(rad >= 2*PI) {rad -= 2*PI;}
+		return rad;
+	}
+
 
 	/**
 	 * gets the angular difference between
@@ -44,13 +51,14 @@ public:
 
 	/**
 	 * gets the angular difference between
+	 * "angular change from r1 to r2"
 	 * - the given radians [0:2PI]
 	 * - 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*PI), "r1 out of bounds");				// [0:360] deg
 		Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds");				// [0:360] deg
-		float diff = r1-r2;
+		float diff = r2-r1;
 		if		(diff > +PI) {diff = -(2*PI - diff);}
 		else if	(diff < -PI) {diff = +(2*PI + diff);}
 		Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds");	// [-180:+180] deg

geo/BBox2.h

@@ -37,7 +37,7 @@ public:
 	}
 
 	/** returns true if the bbox is not yet configured */
-	const bool isInvalid() const {
+	bool isInvalid() const {
 		return p1.x == MAX || p1.y == MAX || p2.x == MIN || p2.y == MIN;
 	}
 
@@ -47,6 +47,9 @@ public:
 	/** get the bbox's maximum */
 	const Point2& getMax() const {return p2;}
 
+	/** get the bbox's size [max-min] */
+	const Point2 getSize() const {return p2-p1;}
+
 	/** get the bbox's center point */
 	Point2 getCenter() const { return (p1+p2) / 2; }
 

geo/BBoxes3.h

@@ -0,0 +1,45 @@
+#ifndef BBOXES3_H
+#define BBOXES3_H
+
+#include "BBox3.h"
+#include <vector>
+
+class BBoxes3 {
+
+private:
+
+	/** all contained bboxes */
+	std::vector<BBox3> bboxes;
+
+public:
+
+	/** empty ctor */
+	BBoxes3() {;}
+
+	/** ctor with entries */
+	BBoxes3(const std::vector<BBox3>& bboxes) : bboxes(bboxes) {;}
+
+
+	/** add the given bbox */
+	void add(const BBox3& bbox) {
+		bboxes.push_back(bbox);
+	}
+
+	/** get all contained bboxes */
+	const std::vector<BBox3>& get() const {
+		return bboxes;
+	}
+
+	/** does the compound contain the given point? */
+	bool contains(const Point3& p) const {
+		for (const BBox3& bb : bboxes) {
+			if (bb.contains(p)) {return true;}
+		}
+		return false;
+	}
+
+
+
+};
+
+#endif // BBOXES3_H

geo/EarthMapping.h

@@ -0,0 +1,185 @@
+#ifndef EARTHMAPPING_H
+#define EARTHMAPPING_H
+
+#include "Point3.h"
+#include "EarthPos.h"
+#include "../floorplan/v2/Floorplan.h"
+#include "../floorplan/v2/FloorplanHelper.h"
+#include "../misc/Debug.h"
+
+/**
+ * mapping between positions on earth and positions within the floorplan
+ */
+class EarthMapping {
+
+private:
+
+	/** one 3D position within the floorplan */
+	Point3 center_map_m;
+
+	/** corresponding 3D position on earth */
+	EarthPos center_earth;
+
+	/** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */
+	float rotation_deg;
+
+	double m_per_deg_lat;
+	double m_per_deg_lon;
+
+	static constexpr const char* NAME = "EarthMap";
+
+
+public:
+
+	/** ctor with parameters */
+	EarthMapping(Point3 center_map_m, EarthPos center_earth, float rotation_deg) :
+		center_map_m(center_map_m), center_earth(center_earth), rotation_deg(rotation_deg) {
+
+		precalc();
+
+	}
+
+	/** ctor for a given map */
+	EarthMapping(Floorplan::IndoorMap* map) {
+
+		// get the map<->earth correspondences from the floorplan
+		const std::vector<Floorplan::EarthPosMapPos*> cor = map->earthReg.correspondences;
+
+		// sanity check
+		if (cor.size() < 3) {
+			throw Exception("for EarthMapping to work, the map needs at least 3 correspondence points");
+		}
+
+		Log::add(NAME, "calculating map<->earth correspondence using " + std::to_string(cor.size()) + " reference points");
+
+		// 1)
+		// to reduce errors, use the average of all correspondces for earth<->map mapping
+		Point3 _mapSum(0,0,0);
+		EarthPos _earthSum(0,0,0);
+		for (const Floorplan::EarthPosMapPos* epmp : cor) {
+			_mapSum += epmp->posOnMap_m;
+			_earthSum.lat += epmp->posOnEarth.lat;
+			_earthSum.lon += epmp->posOnEarth.lon;
+			_earthSum.height += epmp->posOnEarth.height;
+		}
+		const Point3 _mapAvg = _mapSum / cor.size();
+		const EarthPos _earthAvg = EarthPos(_earthSum.lat/cor.size(), _earthSum.lon/cor.size(), _earthSum.height/cor.size());
+
+		// 2)
+		// initialize the mapper with those values
+		// this allows a first mapping, but the map is not facing towards the north!
+		rotation_deg = 0;			// currently unkown
+		center_map_m = _mapAvg;
+		center_earth = _earthAvg;
+		precalc();
+
+		Log::add(NAME, "avg. reference points: " + _mapAvg.asString() + " <-> " + _earthAvg.asString());
+
+		// 3)
+		// now we use this initial setup to determine the rotation angle between map and world
+		float deltaAngleSum = 0;
+		for (Floorplan::EarthPosMapPos* epmp : cor) {
+
+			// angle between  mapAvg and mapCorrespondencePoint
+			const float angleMap = std::atan2(_mapAvg.y - epmp->posOnMap_m.y, _mapAvg.x - epmp->posOnMap_m.x);
+
+			// use the current setup to convert from map to world, WITHOUT correct rotation
+			const Point3 repro = this->worldToMap(epmp->posOnEarth);
+
+			// get the angle between mapAvg and projectedCorrespondencePoint
+			const float angleEarth = std::atan2(_mapAvg.y - repro.y, _mapAvg.x - repro.x);
+
+			// the difference between angleMap and angleEarth contains the angle needed to let the map face northwards
+			// we use the average of all those angles determined by each correspondence
+			const float dx_rad = angleEarth - angleMap;
+			float dx_deg = (dx_rad * 180 / M_PI);
+			if (dx_deg < 0) {dx_deg = 360 + dx_deg;}
+			deltaAngleSum += dx_deg;
+
+		}
+		const float deltaSumAvg = deltaAngleSum / cor.size();
+
+		Log::add(NAME, "avg angular difference [north-rotation]: " + std::to_string(deltaSumAvg));
+
+		// 4)
+		// the current center is not the rotation center we actually need:
+		// e.g. when correspondence points were outside of the building, the rotation center is wrong
+		// as real rotation center, we use the building's bbox center and the correspondence lon/lat on earth
+		const BBox3 bbox = FloorplanHelper::getBBox(map);
+		const Point2 _mapCenter2 = ((bbox.getMax() - bbox.getMin()) / 2).xy();
+		const Point3 _mapCenter3 = Point3(_mapCenter2.x, _mapCenter2.y, this->center_map_m.z);	// keep original z!
+		this->center_earth = mapToWorld(_mapCenter3);
+		this->center_map_m = _mapCenter3;
+
+		Log::add(NAME, "setting rotation center from bbox: " + center_map_m.asString() + " <-> " + center_earth.asString());
+
+		// 5)
+		// finally, let the mapper know the north-angle
+		this->rotation_deg = deltaSumAvg;
+
+	}
+
+	void build() {
+
+		// TODO
+		precalc();
+
+	}
+
+	/** convert from map-coordinates to earth-coordinates */
+	EarthPos mapToWorld(const Point3 mapPos_m) const {
+
+		// move to (0,0,0)
+		Point3 pos = mapPos_m - center_map_m;
+
+		// undo the rotation
+		const Point2 xy = pos.xy().rotated(degToRad(+rotation_deg));
+
+		// convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center
+		const double lat = center_earth.lat + (xy.y / m_per_deg_lat);
+		const double lon = center_earth.lon + (xy.x / m_per_deg_lon);
+		const float height = center_earth.height + pos.z;
+
+		// done
+		return EarthPos(lat, lon, height);
+
+	}
+
+	/** convert from earth-coordinates to map-coordinates */
+	Point3 worldToMap(const EarthPos earthPos) const {
+
+		// move to center and scale
+		const double y_m = +(earthPos.lat - center_earth.lat) * m_per_deg_lat;
+		const double x_m = +(earthPos.lon - center_earth.lon) * m_per_deg_lon;
+		const double z_m =  (earthPos.height - center_earth.height);
+
+		// rotate (our map is axis aligned)
+		Point2 xy(x_m, y_m);
+		xy = xy.rotated(degToRad(-rotation_deg));
+
+		// append height
+		Point3 pos3(xy.x, xy.y, z_m);
+
+		// move from center
+		pos3 += center_map_m;
+
+		return pos3;
+
+	}
+
+private:
+
+	/** perform some pre-calculations to speed things up */
+	void precalc() {
+		const double refLat = center_earth.lat / 180.0 * M_PI;
+		m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat);
+		m_per_deg_lon = 111132.954 * std::cos ( refLat );
+	}
+
+	static inline float degToRad(const float deg) {
+		return deg / 180.0f * (float) M_PI;
+	}
+
+};
+
+#endif // EARTHMAPPING_H

geo/EarthPos.h

@@ -0,0 +1,30 @@
+#ifndef EARTHPOS_H
+#define EARTHPOS_H
+
+/** describes the location on the earth's surface */
+struct EarthPos {
+
+	double lat;
+
+	double lon;
+
+	/** height above sea level */
+	float height;
+
+	/** empty ctor */
+	EarthPos() : lat(NAN), lon(NAN), height(NAN) {
+		;
+	}
+
+	/** ctor with values */
+	EarthPos(const double lat, const double lon, const float height) : lat(lat), lon(lon), height(height) {
+		;
+	}
+
+	std::string asString() const {
+		return "(lat: " + std::to_string(lat) + "°, lon: " + std::to_string(lon) + "°, alt: " + std::to_string(height) + ")";
+	}
+
+};
+
+#endif // EARTHPOS_H

geo/Heading.h

@@ -33,8 +33,11 @@ public:
 	}
 
 	/** signled angular difference [-PI:+PI] */
-	float getSignedDiff(const Heading other) const {
-		return Angle::getSignedDiffRAD_2PI(rad, other.rad);
+//	float getSignedDiff(const Heading other) const {
+//		return Angle::getSignedDiffRAD_2PI(other.rad, rad);
+//	}
+	static float getSignedDiff(const Heading from, const Heading to) {
+		return Angle::getSignedDiffRAD_2PI(from.rad, to.rad);
 	}
 
 	/** update the angle but ensure we stay within [0:2PI] */

geo/Line2.h

@@ -14,15 +14,18 @@ public:
 
 public:
 
-        /** empty ctor */
+	/** empty ctor */
 	Line2() : p1(), p2() {;}
 
-        /** value ctor */
-        Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;}
+	/** value ctor */
+	Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;}
 
-        /** value ctor */
+	/** value ctor */
 	Line2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;}
 
+
+	Line2 operator * (const float v) const {return Line2(p1*v, p2*v);}
+
 //	bool getSegmentIntersection(const Line& other) const {
 //		static K::Point p;
 //		return K::Line::getSegmentIntersection(other, p);

geo/Point2.h

@@ -2,6 +2,8 @@
 #define POINT2_H
 
 #include <cmath>
+#include <algorithm>
+#include <string>
 
 /**
  * 2D Point
@@ -17,6 +19,8 @@ struct Point2 {
 	/** ctor */
 	Point2(const float x, const float y) : x(x), y(y) {;}
 
+	Point2 operator - () const {return Point2(-x, -y);}
+
 
 	Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);}
 
@@ -58,6 +62,15 @@ struct Point2 {
 		return std::sqrt(dx*dx + dy*dy);
 	}
 
+	std::string asString() const {
+		return "(" + std::to_string(x) + "," + std::to_string(y) + ")";
+	}
+
 };
 
+inline void swap(Point2& p1, Point2& p2) {
+	std::swap(p1.x, p2.x);
+	std::swap(p1.y, p2.y);
+}
+
 #endif // POINT2_H

grid/GridNode.h

@@ -52,6 +52,9 @@ public:
 	static const uint8_t TYPE_ELEVATOR = 2;
 	static const uint8_t TYPE_DOOR = 3;
 
+	static const uint8_t TYPE_OUTDOOR = 100;
+
+
 public:
 
 	/** ctor */

grid/factory/v2/Elevators.h

@@ -79,8 +79,13 @@ public:
 
 			// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
 			for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
-				const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
-				const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
+				GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
+				GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
+
+				// above the ending floor? -> snap to ending floor
+				// note: this one is needed if the floor-heights are not dividable by the grid-size
+				if (gp.z_cm > floor->getEndingZ()*100) {gp.z_cm = floor->getEndingZ()*100;}
+
 				Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
 				Assert::isTrue(grid.hasNodeFor(gp), "missing node");
 				T& n1 = (T&) grid.getNodeFor(gpBelow);

grid/factory/v2/GridFactory.h

@@ -42,6 +42,8 @@ private:
 	Elevators<T> elevators;
 
 
+	std::vector<GridPoint> withinRemovePolygon;
+
 	bool _buildStairs = true;
 	bool _removeIsolated = true;
 
@@ -60,6 +62,14 @@ public:
 	void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;}
 
 
+	/** does the given grid-node have a stair-neighbor? */
+	bool hasStairNeighbor(const T& node) const {
+		for (const T& n : grid.neighbors(node)) {
+			if (n.getType() == GridNode::TYPE_STAIR) {return true;}
+		}
+		return false;
+	}
+
 	/** build using the given map */
 	void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) {
 
@@ -93,6 +103,27 @@ public:
 			}
 		}
 
+		// remove nodes within remove-OutlinePolygons
+		// this must happen AFTER stairs have been added, otherwise stairs might not be connectable due to removed/missing nodes.
+		// also, within this loop we prevent the deltion of nodes that are a direct neighbor of a stair!
+		// [otherwise the same thing would happen again!]
+		if (true) {
+
+			for (const GridPoint& gp : withinRemovePolygon) {
+				T* n = (T*) grid.getNodePtrFor(gp);
+
+				// delete if node is present and is no direct neighbor of a stair
+				if (n && !hasStairNeighbor(*n)) {
+					grid.remove(*n);
+				}
+
+			}
+
+			// remove all nodes that were just marked for removal
+			grid.cleanup();
+
+		}
+
 		// remove isolated nodes
 		if (_removeIsolated) {
 			if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
@@ -114,21 +145,43 @@ public:
 		return bb;
 	}
 
-	bool isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) {
+	struct PartOfOutline {
+		bool contained = false;			// contained within at-least one polygon?
+		bool markedForRemove = false;	// part of a "to-be-removed" polygon?
+		bool outdoor = false;			// otherwise: indoor
+	};
 
-		bool add = false;
+	/** get the part of outline the given location belongs to. currently: none, indoor, outdoor */
+	static PartOfOutline isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) {
+
+		// assume the point is not part of the outline
+		PartOfOutline res;
+		res.contained = false;
+		res.markedForRemove = false;
+
+		// process every outline polygon
 		for (Floorplan::FloorOutlinePolygon* poly : outline) {
-			if (poly->method != Floorplan::OutlineMethod::ADD) {continue;}
 			HelperPoly pol(*poly);
-			if (pol.contains(Point2(x_cm, y_cm))) {add = true;}
+			if (pol.contains(Point2(x_cm, y_cm))) {
+
+				// mark as "contained"
+				res.contained = true;
+
+				// belongs to a "remove" polygon? -> directly ignore this location!
+				if (poly->method == Floorplan::OutlineMethod::REMOVE) {
+					res.markedForRemove = true;
+				}
+
+				// belongs to a "add" polygon? -> remember until all polygons were checked
+				// [might still belong to a "remove" polygon]
+				res.outdoor = poly->outdoor;
+
+			}
 		}
-		if (!add) {return false;}
-		for (Floorplan::FloorOutlinePolygon* poly : outline) {
-			if (poly->method != Floorplan::OutlineMethod::REMOVE) {continue;}
-			HelperPoly pol(*poly);
-			if (pol.contains(Point2(x_cm, y_cm))) {add = false;}			// TODO
-		}
-		return add;
+
+		// done
+		return res;
+
 	}
 
 
@@ -155,7 +208,8 @@ public:
 			for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) {
 
 				// does the outline-polygon contain this position?
-				if (!isPartOfFloorOutline(x_cm, y_cm, floor->outline)) {continue;}
+				const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline);
+				if (!part.contained) {continue;}
 
 				// check intersection with the floorplan
 				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
@@ -164,12 +218,18 @@ public:
 				bbox.grow(0.42345);
 				if (intersects(bbox, floor)) {continue;}
 
-				// add to the grid
+				// add to the grid [once]
 				T t(x_cm, y_cm, z_cm);
-				t.setType(getType(bbox, floor));
 				if (grid.hasNodeFor(t)) {continue;}
+				updateType(t, part, bbox, floor);
 				grid.add(t);
 
+				// node part of remove-region?
+				// if so, remove >>AFTER<< stairs have been added
+				if (part.markedForRemove) {
+					withinRemovePolygon.push_back(t);
+				}
+
 				// debug
 				++numNodes;
 
@@ -376,7 +436,19 @@ public:
 		Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
 		Log::tick();
 		for (T& n2 : grid) {
-			if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
+			if (set.find(n2.getIdx()) == set.end()) {
+
+				// sanity check
+				// wouldn't make sense that a stair-node is removed..
+				// maybe something went wrong elsewhere???
+				Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
+				Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
+				//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");
+
+				// proceed ;)
+				grid.remove(n2);
+
+			}
 		}
 		Log::tock();
 
@@ -415,7 +487,23 @@ private:
 
 private:
 
-
+	/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
+	static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
+		//const Line2 base(line->from*100, line->to*100);
+		const float thickness_m = line->thickness_m;
+		const Point2 dir = (line->to - line->from);				// obstacle's direction
+		const Point2 perp = dir.perpendicular().normalized();	// perpendicular direction (90 degree)
+		const Point2 p1 = line->from + perp * thickness_m/2;		// start-up
+		const Point2 p2 = line->from - perp * thickness_m/2;		// start-down
+		const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
+		const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
+		return {
+			Line2(p1, p2),
+			Line2(p3, p4),
+			Line2(p2, p4),
+			Line2(p1, p3),
+		};
+	}
 
 	/** does the bbox intersect with any of the floor's walls? */
 	static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
@@ -427,8 +515,15 @@ private:
 			// depends on the type of obstacle
 			if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
 				const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
-				const Line2 l2(line->from*100, line->to*100);
-				if (bbox.intersects(l2)) {return true;}
+
+				// old method (does not support thickness)
+				//const Line2 l2(line->from*100, line->to*100);
+				//if (bbox.intersects(l2)) {return true;}
+				const std::vector<Line2> lines = getThickLines(line);
+				for (const Line2& l : lines) {
+					if (bbox.intersects(l*100)) {return true;}
+				}
+
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
 				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
@@ -487,23 +582,30 @@ private:
 
 	}
 
-	/** does the bbox intersect with any of the floor's walls? */
-	static inline int getType(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
+	/** adjust the given gridNode's type if needed [e.g. from "floor" to "door"] */
+	static inline void updateType(T& t, const PartOfOutline part, const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
 
-		// process each obstacle
+		// first, assume the type of the outline polygon
+		if (part.outdoor) {
+			t.setType(GridNode::TYPE_OUTDOOR);
+		} else {
+			//t.setType(GridNode::TYPE_FLOOR);
+		}
+
+		// hereafter, process each obstacle and mark doors
 		for (Floorplan::FloorObstacle* fo : floor->obstacles) {
 
 			if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
 				const Floorplan::FloorObstacleDoor* door = (Floorplan::FloorObstacleDoor*) fo;
 				const Line2 l2(door->from*100, door->to*100);
-				if (bbox.intersects(l2)) {return GridNode::TYPE_DOOR;}
-
+				if (bbox.intersects(l2)) {
+					t.setType(GridNode::TYPE_DOOR);
+					return;	// done
+				}
 			}
 
 		}
 
-		return GridNode::TYPE_FLOOR;
-
 	}
 
 

grid/factory/v2/Importance.h

@@ -153,6 +153,8 @@ public:
 
 
 			// sanity check
+			Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString());
+			Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString());
 			Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
 			Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");
 

grid/factory/v2/Stairs2.h

@@ -157,18 +157,24 @@ public:
 				// remember the z-position of the already-existing grid-node we connected the stair to
 				connectedWithHeights.insert(grid[sn.gridIdx].z_cm);
 
+				// mark the node as stair-node
+				//grid[sn.gridIdx].setType(GridNode::TYPE_STAIR);
+
 			} else  {
 
 				sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm));
 
 				// check if there is a nearby floor-node to delete
+				// -> remove nodes directly above/below the stair
 				const int deleteDist_cm = 100;
 				const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
 				const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
-				if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
+				//if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
+				if (distToBelow > 0 && distToBelow < deleteDist_cm) {
 					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
 					if (n) {toDelete.push_back(n);}
-				} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
+				//} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
+				} else if (distToAbove > 0 && distToAbove < deleteDist_cm) {
 					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
 					if (n) {toDelete.push_back(n);}
 				}
@@ -181,7 +187,11 @@ public:
 		}
 
 		// sanity check
-		Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
+		// connectedWithHeights should contain 2 entries:
+		// one for the starting floor
+		// one for the ending floor
+		// this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected
+	Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
 
 		// now connect all new nodes with their neighbors
 		// do not perform normal grid-connection but examine the nodes within the generated vector
@@ -225,6 +235,9 @@ public:
 
 	void finalize() {
 
+		//std::cout << "stairs.finalize() crashes!" << std::endl;
+		//return;
+
 		// delete all pending nodes and perform a cleanup
 		if (!toDelete.empty()) {
 			for (T* n : toDelete) {grid.remove(*n);}

grid/walk/v2/GridWalker.h

@@ -104,8 +104,15 @@ public:
 		//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
 		probability = 1.0;
 		//probability  = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0);		// dist_m might be zero -> no edges -> no maximum
-		probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0);			// "kill" particles that walk near walls (most probably trapped ones)
-		//probability = std::pow(probability, 5);
+
+		// add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...]
+		const float walkImportance = curNode->getWalkImportance();
+		Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?");
+		Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?");
+		probability *= walkImportance;// < 0.4f ? (0.1) : (1.0);			// "kill" particles that walk near walls (most probably trapped ones)
+
+		// sanity check
+		Assert::isNotNaN(probability, "detected NaN grid-walk probability");
 
 		// update after
 		updateAfter(currentState, *startNode, *curNode);

grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h

@@ -0,0 +1,84 @@
+#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H
+#define WALKMODULEABSOLUTEHEADINGCONTROL_H
+
+/**
+ * compare the state's absolute heading against a given compass [control]
+ */
+
+#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 WalkModuleAbsoluteHeadingControl : public WalkModule<Node, WalkState> {
+
+	const float sigma_rad;
+	const Control* ctrl;
+
+public:
+
+	/** ctor. 180 should be OK! */
+	WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
+		sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)),
+		ctrl(ctrl) {
+
+		// ensure the template WalkState inherits from 'WalkStateHeading'!
+		StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
+
+	}
+
+
+	virtual void updateBefore(WalkState& state, const Node& startNode) override {
+		(void) state;
+		(void) startNode;
+	}
+
+	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;
+
+		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
+		// reset this one after every transition!
+		Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds");
+
+		// ignore for stairs?
+		//if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}
+
+		// for elevator edges [same (x,y) but different z] just return 1
+		if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
+		if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
+		//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
+
+		// 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.heading.direction;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+		if (angularDiff > Angle::degToRad(100))	{return 0.10;}
+												{return 0.90;}
+
+	}
+
+};
+
+#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H

grid/walk/v2/modules/WalkModuleActivityControl.h

@@ -100,7 +100,7 @@ public:
 //				if (potentialNode.getType() == GridNode::TYPE_FLOOR)	{return kappa;}
 //																		{return 1-kappa;}
 			default:
-				throw Exception("not yet implemented");
+			    throw Exception("not yet implemented activity within WalkModuleActivityControl::getProbability");
 		}
 
 	}

grid/walk/v2/modules/WalkModuleActivityControlPercent.h

@@ -0,0 +1,91 @@
+#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H
+#define WALKMODULEACTIVITYCONTROLPERCENT_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+#include "../../../../sensors/pressure/ActivityButterPressurePercent.h"
+#include "../../../../grid/GridNode.h"
+
+
+/** favor z-transitions */
+template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControlPercent : public WalkModule<Node, WalkState> {
+
+private:
+
+    Control* ctrl;
+
+public:
+
+	/** ctor */
+    WalkModuleActivityControlPercent(Control* ctrl) : ctrl(ctrl) {
+		;
+	}
+
+    virtual void updateBefore(WalkState& state, const Node& startNode) override {
+        (void) state;
+        (void) startNode;
+    }
+
+	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;
+
+
+        const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
+
+        //floor and doors
+        if(potentialNode.getType() == 0 || potentialNode.getType() == 3){
+            return ctrl->activityPercent.stay;
+        }
+
+        //stairs
+        if(potentialNode.getType() == 1){
+
+//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown +  ctrl->barometer.actProbs.stairsUp){
+//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
+//            }
+
+            if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;}
+            if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;}
+            return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp);
+        }
+
+        //elevators
+        if(potentialNode.getType() == 2){
+
+//            //we need to do this, that particles are able to walk into an elevator even if the prob for that is low,
+//            //that happens often since the activity has some delay.
+//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown +  ctrl->barometer.actProbs.elevatorUp){
+//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
+//            }
+
+            if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;}
+            if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;}
+
+            //for walking out of the elevator
+            return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp);
+        }
+
+        std::cout << "Node has unknown Type" << std::endl;
+        return 1.0;
+    }
+};
+
+#endif // WALKMODULEACTIVITYCONTROLPERCENT_H

grid/walk/v2/modules/WalkModuleButterActivity.h

@@ -1,86 +0,0 @@
-#ifndef WALKMODULEBUTTERACTIVITY_H
-#define WALKMODULEBUTTERACTIVITY_H
-
-#include "WalkModule.h"
-
-#include "../../../../geo/Heading.h"
-#include "../../../../math/Distributions.h"
-#include "../../../../sensors/pressure/ActivityButterPressure.h"
-
-DEPREACTED
-SEE WalkModuleActivityControl
-
-struct WalkStateBarometerActivity {
-
-	/** innser-struct to prevent name-clashes */
-	struct Barometer {
-
-		/** activity currently detected from the baromter */
-		ActivityButterPressure::Activity activity;
-
-		Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
-
-	} barometer;
-
-	/** ctor */
-	WalkStateBarometerActivity() : barometer() {;}
-
-};
-
-/** favor z-transitions */
-template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
-
-public:
-
-	/** ctor */
-    WalkModuleButterActivity() {
-
-		// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
-		StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
-
-	}
-
-	virtual void updateBefore(WalkState& state, const Node& startNode) override {
-		(void) state;
-		(void) startNode;
-	}
-
-	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;
-
-        const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
-
-		if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
-            if (deltaZ_cm < 0) {return 0;}
-            if (deltaZ_cm == 0) {return 0.1;}
-            return 0.9;
-		} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
-            if (deltaZ_cm > 0) {return 0;}
-            if (deltaZ_cm == 0) {return 0.1;}
-            return 0.9;
-        } else {
-            if (deltaZ_cm == 0) {return 0.9;}
-            return 0.1;
-        }
-
-	}
-
-
-};
-
-#endif // WALKMODULEBUTTERACTIVITY_H

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -77,7 +77,8 @@ public:
 		const Heading stateHead = state.heading.direction;
 
 		// get the error (signed difference) between both
-		const float angularDiff = stateHead.getSignedDiff(head);
+//		const float angularDiff = stateHead.getSignedDiff(head);
+		const float angularDiff = Heading::getSignedDiff(head, stateHead);
 
 		// adjust the error.
 		// note:	the error may get > +/- 2PI but this is not an issue!

grid/walk/v2/modules/WalkModuleHeadingVonMises.h

@@ -0,0 +1,124 @@
+#ifndef WALKMODULEHEADINGVONMISES_H
+#define WALKMODULEHEADINGVONMISES_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 WalkModuleHeadingVonMises : public WalkModule<Node, WalkState> {
+
+private:
+
+    /** van-Mises distribution */
+    Distribution::VonMises<float> dist;
+
+    /** random noise */
+    Distribution::Normal<float> distNoise;
+
+    const Control* ctrl;
+
+public:
+
+    /** ctor 3.0 should be OK! */
+    WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) :
+        dist(Distribution::VonMises<float>(0.0f, 2.0f)),
+        distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
+        ctrl(ctrl) {
+
+        // ensure the template WalkState inherits from 'WalkStateHeading'!
+        StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
+
+    }
+
+
+    virtual void updateBefore(WalkState& state, const Node& startNode) override {
+
+        // NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
+        // reset this one after every transition!
+        Assert::isBetween(ctrl->turnSinceLastTransition_rad, -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
+        float var = distNoise.draw();
+
+        // stair? -> increase variance
+        if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
+
+        // adjust the state's heading using the control-data
+        state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
+
+        //set kappa of mises
+        float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
+        dist.setKappa(kappa);
+
+    }
+
+    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;
+
+        // ignore for stairs?
+        //if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
+
+        // for elevator edges [same (x,y) but different z] do not adjust anything
+        if (nextNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+        if (curNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+        //if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
+
+        // 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);
+		const float angularDiff = Heading::getSignedDiff(head, stateHead);
+
+        // 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;
+
+    }
+
+    double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+        (void) startNode;
+
+        // ignore for stairs?
+        //if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}
+
+        // for elevator edges [same (x,y) but different z] just return 1
+        if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
+        if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
+        //if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
+
+        // 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.heading.direction + state.heading.error;
+
+        // get the difference
+        const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+        // determine probability
+        return dist.getProbability(angularDiff);
+
+    }
+
+
+};
+
+#endif // WALKMODULEHEADING_H

lib/tinyxml/tinyxml2.h

@@ -30,6 +30,9 @@ distribution.
 #   include <stdio.h>
 #   include <stdlib.h>
 #   include <string.h>
+#	if defined(__PS3__)
+#		include <stddef.h>
+#	endif
 #else
 #   include <cctype>
 #   include <climits>
@@ -37,6 +40,7 @@ distribution.
 #   include <cstdlib>
 #   include <cstring>
 #endif
+#include <stdint.h>
 
 /*
    TODO: intern strings instead of allocation.
@@ -68,6 +72,8 @@ distribution.
 #   else
 #       define TINYXML2_LIB
 #   endif
+#elif __GNUC__ >= 4
+#   define TINYXML2_LIB __attribute__((visibility("default")))
 #else
 #   define TINYXML2_LIB
 #endif
@@ -92,9 +98,9 @@ distribution.
 /* Versioning, past 1.0.14:
 	http://semver.org/
 */
-static const int TIXML2_MAJOR_VERSION = 3;
+static const int TIXML2_MAJOR_VERSION = 4;
 static const int TIXML2_MINOR_VERSION = 0;
-static const int TIXML2_PATCH_VERSION = 0;
+static const int TIXML2_PATCH_VERSION = 1;
 
 namespace tinyxml2
 {
@@ -121,18 +127,20 @@ public:
         NEEDS_NEWLINE_NORMALIZATION		= 0x02,
         NEEDS_WHITESPACE_COLLAPSING     = 0x04,
 
-        TEXT_ELEMENT		            	= NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,
+        TEXT_ELEMENT		            = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,
         TEXT_ELEMENT_LEAVE_ENTITIES		= NEEDS_NEWLINE_NORMALIZATION,
-        ATTRIBUTE_NAME		            	= 0,
-        ATTRIBUTE_VALUE		            	= NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,
-        ATTRIBUTE_VALUE_LEAVE_ENTITIES  	= NEEDS_NEWLINE_NORMALIZATION,
-        COMMENT				        = NEEDS_NEWLINE_NORMALIZATION
+        ATTRIBUTE_NAME		            = 0,
+        ATTRIBUTE_VALUE		            = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,
+        ATTRIBUTE_VALUE_LEAVE_ENTITIES  = NEEDS_NEWLINE_NORMALIZATION,
+        COMMENT							= NEEDS_NEWLINE_NORMALIZATION
     };
 
     StrPair() : _flags( 0 ), _start( 0 ), _end( 0 ) {}
     ~StrPair();
 
     void Set( char* start, char* end, int flags ) {
+        TIXMLASSERT( start );
+        TIXMLASSERT( end );
         Reset();
         _start  = start;
         _end    = end;
@@ -152,13 +160,13 @@ public:
 
     void SetStr( const char* str, int flags=0 );
 
-    char* ParseText( char* in, const char* endTag, int strFlags );
+    char* ParseText( char* in, const char* endTag, int strFlags, int* curLineNumPtr );
     char* ParseName( char* in );
 
     void TransferTo( StrPair* other );
+	void Reset();
 
 private:
-    void Reset();
     void CollapseWhitespace();
 
     enum {
@@ -203,7 +211,8 @@ public:
     void Push( T t ) {
         TIXMLASSERT( _size < INT_MAX );
         EnsureCapacity( _size+1 );
-        _mem[_size++] = t;
+        _mem[_size] = t;
+        ++_size;
     }
 
     T* PushArr( int count ) {
@@ -217,7 +226,8 @@ public:
 
     T Pop() {
         TIXMLASSERT( _size > 0 );
-        return _mem[--_size];
+        --_size;
+        return _mem[_size];
     }
 
     void PopArr( int count ) {
@@ -311,7 +321,7 @@ public:
 /*
 	Template child class to create pools of the correct type.
 */
-template< int SIZE >
+template< int ITEM_SIZE >
 class MemPoolT : public MemPool
 {
 public:
@@ -334,7 +344,7 @@ public:
     }
 
     virtual int ItemSize() const	{
-        return SIZE;
+        return ITEM_SIZE;
     }
     int CurrentAllocs() const		{
         return _currentAllocs;
@@ -346,21 +356,23 @@ public:
             Block* block = new Block();
             _blockPtrs.Push( block );
 
-            for( int i=0; i<COUNT-1; ++i ) {
-                block->chunk[i].next = &block->chunk[i+1];
+            Item* blockItems = block->items;
+            for( int i = 0; i < ITEMS_PER_BLOCK - 1; ++i ) {
+                blockItems[i].next = &(blockItems[i + 1]);
             }
-            block->chunk[COUNT-1].next = 0;
-            _root = block->chunk;
+            blockItems[ITEMS_PER_BLOCK - 1].next = 0;
+            _root = blockItems;
         }
-        void* result = _root;
+        Item* const result = _root;
+        TIXMLASSERT( result != 0 );
         _root = _root->next;
 
         ++_currentAllocs;
         if ( _currentAllocs > _maxAllocs ) {
             _maxAllocs = _currentAllocs;
         }
-        _nAllocs++;
-        _nUntracked++;
+        ++_nAllocs;
+        ++_nUntracked;
         return result;
     }
     
@@ -369,20 +381,21 @@ public:
             return;
         }
         --_currentAllocs;
-        Chunk* chunk = static_cast<Chunk*>( mem );
+        Item* item = static_cast<Item*>( mem );
 #ifdef DEBUG
-        memset( chunk, 0xfe, sizeof(Chunk) );
+        memset( item, 0xfe, sizeof( *item ) );
 #endif
-        chunk->next = _root;
-        _root = chunk;
+        item->next = _root;
+        _root = item;
     }
     void Trace( const char* name ) {
         printf( "Mempool %s watermark=%d [%dk] current=%d size=%d nAlloc=%d blocks=%d\n",
-                name, _maxAllocs, _maxAllocs*SIZE/1024, _currentAllocs, SIZE, _nAllocs, _blockPtrs.Size() );
+                name, _maxAllocs, _maxAllocs * ITEM_SIZE / 1024, _currentAllocs,
+                ITEM_SIZE, _nAllocs, _blockPtrs.Size() );
     }
 
     void SetTracked() {
-        _nUntracked--;
+        --_nUntracked;
     }
 
     int Untracked() const {
@@ -398,21 +411,23 @@ public:
 	//		16k:	5200
 	//		32k:	4300
 	//		64k:	4000	21000
-    enum { COUNT = (4*1024)/SIZE }; // Some compilers do not accept to use COUNT in private part if COUNT is private
+    // Declared public because some compilers do not accept to use ITEMS_PER_BLOCK
+    // in private part if ITEMS_PER_BLOCK is private
+    enum { ITEMS_PER_BLOCK = (4 * 1024) / ITEM_SIZE };
 
 private:
     MemPoolT( const MemPoolT& ); // not supported
     void operator=( const MemPoolT& ); // not supported
 
-    union Chunk {
-        Chunk*  next;
-        char    mem[SIZE];
+    union Item {
+        Item*   next;
+        char    itemData[ITEM_SIZE];
     };
     struct Block {
-        Chunk chunk[COUNT];
+        Item items[ITEMS_PER_BLOCK];
     };
     DynArray< Block*, 10 > _blockPtrs;
-    Chunk* _root;
+    Item* _root;
 
     int _currentAllocs;
     int _nAllocs;
@@ -485,7 +500,6 @@ public:
 // WARNING: must match XMLDocument::_errorNames[]
 enum XMLError {
     XML_SUCCESS = 0,
-    XML_NO_ERROR = 0,
     XML_NO_ATTRIBUTE,
     XML_WRONG_ATTRIBUTE_TYPE,
     XML_ERROR_FILE_NOT_FOUND,
@@ -513,19 +527,23 @@ enum XMLError {
 /*
 	Utility functionality.
 */
-class XMLUtil
+class TINYXML2_LIB XMLUtil
 {
 public:
-    static const char* SkipWhiteSpace( const char* p )	{
+    static const char* SkipWhiteSpace( const char* p, int* curLineNumPtr )	{
         TIXMLASSERT( p );
+
         while( IsWhiteSpace(*p) ) {
+            if (curLineNumPtr && *p == '\n') {
+                ++(*curLineNumPtr);
+            }
             ++p;
         }
         TIXMLASSERT( p );
         return p;
     }
-    static char* SkipWhiteSpace( char* p )				{
-        return const_cast<char*>( SkipWhiteSpace( const_cast<const char*>(p) ) );
+    static char* SkipWhiteSpace( char* p, int* curLineNumPtr )				{
+        return const_cast<char*>( SkipWhiteSpace( const_cast<const char*>(p), curLineNumPtr ) );
     }
 
     // Anything in the high order range of UTF-8 is assumed to not be whitespace. This isn't
@@ -556,6 +574,9 @@ public:
         if ( p == q ) {
             return true;
         }
+        TIXMLASSERT( p );
+        TIXMLASSERT( q );
+        TIXMLASSERT( nChar >= 0 );
         return strncmp( p, q, nChar ) == 0;
     }
     
@@ -575,6 +596,7 @@ public:
     static void ToStr( bool v, char* buffer, int bufferSize );
     static void ToStr( float v, char* buffer, int bufferSize );
     static void ToStr( double v, char* buffer, int bufferSize );
+	static void ToStr(int64_t v, char* buffer, int bufferSize);
 
     // converts strings to primitive types
     static bool	ToInt( const char* str, int* value );
@@ -582,6 +604,18 @@ public:
     static bool	ToBool( const char* str, bool* value );
     static bool	ToFloat( const char* str, float* value );
     static bool ToDouble( const char* str, double* value );
+	static bool ToInt64(const char* str, int64_t* value);
+
+	// Changes what is serialized for a boolean value.
+	// Default to "true" and "false". Shouldn't be changed
+	// unless you have a special testing or compatibility need.
+	// Be careful: static, global, & not thread safe.
+	// Be sure to set static const memory as parameters.
+	static void SetBoolSerialization(const char* writeTrue, const char* writeFalse);
+
+private:
+	static const char* writeBoolTrue;
+	static const char* writeBoolFalse;
 };
 
 
@@ -687,6 +721,9 @@ public:
     */
     void SetValue( const char* val, bool staticMem=false );
 
+    /// Gets the line number the node is in, if the document was parsed from a file.
+    int GetLineNum() const { return _parseLineNum; }
+
     /// Get the parent of this node on the DOM.
     const XMLNode*	Parent() const			{
         return _parent;
@@ -852,15 +889,30 @@ public:
     */
     virtual bool Accept( XMLVisitor* visitor ) const = 0;
 
+	/** 
+		Set user data into the XMLNode. TinyXML-2 in 
+		no way processes or interprets user data.
+		It is initially 0.
+	*/
+	void SetUserData(void* userData)	{ _userData = userData; }
+
+	/**
+		Get user data set into the XMLNode. TinyXML-2 in
+		no way processes or interprets user data.
+		It is initially 0.
+	*/
+	void* GetUserData() const			{ return _userData; }
+
 protected:
     XMLNode( XMLDocument* );
     virtual ~XMLNode();
 
-    virtual char* ParseDeep( char*, StrPair* );
+    virtual char* ParseDeep( char*, StrPair*, int* );
 
     XMLDocument*	_document;
     XMLNode*		_parent;
     mutable StrPair	_value;
+    int             _parseLineNum;
 
     XMLNode*		_firstChild;
     XMLNode*		_lastChild;
@@ -868,11 +920,14 @@ protected:
     XMLNode*		_prev;
     XMLNode*		_next;
 
+	void*			_userData;
+
 private:
     MemPool*		_memPool;
     void Unlink( XMLNode* child );
     static void DeleteNode( XMLNode* node );
     void InsertChildPreamble( XMLNode* insertThis ) const;
+    const XMLElement* ToElementWithName( const char* name ) const;
 
     XMLNode( const XMLNode& );	// not supported
     XMLNode& operator=( const XMLNode& );	// not supported
@@ -893,7 +948,6 @@ private:
 */
 class TINYXML2_LIB XMLText : public XMLNode
 {
-    friend class XMLBase;
     friend class XMLDocument;
 public:
     virtual bool Accept( XMLVisitor* visitor ) const;
@@ -921,7 +975,7 @@ protected:
     XMLText( XMLDocument* doc )	: XMLNode( doc ), _isCData( false )	{}
     virtual ~XMLText()												{}
 
-    char* ParseDeep( char*, StrPair* endTag );
+    char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr );
 
 private:
     bool _isCData;
@@ -952,7 +1006,7 @@ protected:
     XMLComment( XMLDocument* doc );
     virtual ~XMLComment();
 
-    char* ParseDeep( char*, StrPair* endTag );
+    char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr);
 
 private:
     XMLComment( const XMLComment& );	// not supported
@@ -991,7 +1045,7 @@ protected:
     XMLDeclaration( XMLDocument* doc );
     virtual ~XMLDeclaration();
 
-    char* ParseDeep( char*, StrPair* endTag );
+    char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr );
 
 private:
     XMLDeclaration( const XMLDeclaration& );	// not supported
@@ -1026,7 +1080,7 @@ protected:
     XMLUnknown( XMLDocument* doc );
     virtual ~XMLUnknown();
 
-    char* ParseDeep( char*, StrPair* endTag );
+    char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr );
 
 private:
     XMLUnknown( const XMLUnknown& );	// not supported
@@ -1051,6 +1105,9 @@ public:
     /// The value of the attribute.
     const char* Value() const;
 
+    /// Gets the line number the attribute is in, if the document was parsed from a file.
+    int GetLineNum() const { return _parseLineNum; }
+
     /// The next attribute in the list.
     const XMLAttribute* Next() const {
         return _next;
@@ -1060,11 +1117,18 @@ public:
         If the value isn't an integer, 0 will be returned. There is no error checking;
     	use QueryIntValue() if you need error checking.
     */
-    int		 IntValue() const				{
-        int i=0;
-        QueryIntValue( &i );
-        return i;
-    }
+	int	IntValue() const {
+		int i = 0;
+		QueryIntValue(&i);
+		return i;
+	}
+
+	int64_t Int64Value() const {
+		int64_t i = 0;
+		QueryInt64Value(&i);
+		return i;
+	}
+
     /// Query as an unsigned integer. See IntValue()
     unsigned UnsignedValue() const			{
         unsigned i=0;
@@ -1091,13 +1155,15 @@ public:
     }
 
     /** QueryIntValue interprets the attribute as an integer, and returns the value
-    	in the provided parameter. The function will return XML_NO_ERROR on success,
+    	in the provided parameter. The function will return XML_SUCCESS on success,
     	and XML_WRONG_ATTRIBUTE_TYPE if the conversion is not successful.
     */
     XMLError QueryIntValue( int* value ) const;
     /// See QueryIntValue
     XMLError QueryUnsignedValue( unsigned int* value ) const;
-    /// See QueryIntValue
+	/// See QueryIntValue
+	XMLError QueryInt64Value(int64_t* value) const;
+	/// See QueryIntValue
     XMLError QueryBoolValue( bool* value ) const;
     /// See QueryIntValue
     XMLError QueryDoubleValue( double* value ) const;
@@ -1110,7 +1176,9 @@ public:
     void SetAttribute( int value );
     /// Set the attribute to value.
     void SetAttribute( unsigned value );
-    /// Set the attribute to value.
+	/// Set the attribute to value.
+	void SetAttribute(int64_t value);
+	/// Set the attribute to value.
     void SetAttribute( bool value );
     /// Set the attribute to value.
     void SetAttribute( double value );
@@ -1120,17 +1188,18 @@ public:
 private:
     enum { BUF_SIZE = 200 };
 
-    XMLAttribute() : _next( 0 ), _memPool( 0 ) {}
+    XMLAttribute() : _parseLineNum( 0 ), _next( 0 ), _memPool( 0 ) {}
     virtual ~XMLAttribute()	{}
 
     XMLAttribute( const XMLAttribute& );	// not supported
     void operator=( const XMLAttribute& );	// not supported
     void SetName( const char* name );
 
-    char* ParseDeep( char* p, bool processEntities );
+    char* ParseDeep( char* p, bool processEntities, int* curLineNumPtr );
 
     mutable StrPair _name;
     mutable StrPair _value;
+    int             _parseLineNum;
     XMLAttribute*   _next;
     MemPool*        _memPool;
 };
@@ -1142,7 +1211,6 @@ private:
 */
 class TINYXML2_LIB XMLElement : public XMLNode
 {
-    friend class XMLBase;
     friend class XMLDocument;
 public:
     /// Get the name of an element (which is the Value() of the node.)
@@ -1188,42 +1256,25 @@ public:
     const char* Attribute( const char* name, const char* value=0 ) const;
 
     /** Given an attribute name, IntAttribute() returns the value
-    	of the attribute interpreted as an integer. 0 will be
-    	returned if there is an error. For a method with error
-    	checking, see QueryIntAttribute()
+    	of the attribute interpreted as an integer. The default
+        value will be returned if the attribute isn't present,
+        or if there is an error. (For a method with error
+    	checking, see QueryIntAttribute()).
     */
-    int		 IntAttribute( const char* name ) const		{
-        int i=0;
-        QueryIntAttribute( name, &i );
-        return i;
-    }
+	int IntAttribute(const char* name, int defaultValue = 0) const;
     /// See IntAttribute()
-    unsigned UnsignedAttribute( const char* name ) const {
-        unsigned i=0;
-        QueryUnsignedAttribute( name, &i );
-        return i;
-    }
+	unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const;
+	/// See IntAttribute()
+	int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const;
+	/// See IntAttribute()
+	bool BoolAttribute(const char* name, bool defaultValue = false) const;
     /// See IntAttribute()
-    bool	 BoolAttribute( const char* name ) const	{
-        bool b=false;
-        QueryBoolAttribute( name, &b );
-        return b;
-    }
+	double DoubleAttribute(const char* name, double defaultValue = 0) const;
     /// See IntAttribute()
-    double 	 DoubleAttribute( const char* name ) const	{
-        double d=0;
-        QueryDoubleAttribute( name, &d );
-        return d;
-    }
-    /// See IntAttribute()
-    float	 FloatAttribute( const char* name ) const	{
-        float f=0;
-        QueryFloatAttribute( name, &f );
-        return f;
-    }
+	float FloatAttribute(const char* name, float defaultValue = 0) const;
 
     /** Given an attribute name, QueryIntAttribute() returns
-    	XML_NO_ERROR, XML_WRONG_ATTRIBUTE_TYPE if the conversion
+    	XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion
     	can't be performed, or XML_NO_ATTRIBUTE if the attribute
     	doesn't exist. If successful, the result of the conversion
     	will be written to 'value'. If not successful, nothing will
@@ -1242,7 +1293,8 @@ public:
         }
         return a->QueryIntValue( value );
     }
-    /// See QueryIntAttribute()
+
+	/// See QueryIntAttribute()
     XMLError QueryUnsignedAttribute( const char* name, unsigned int* value ) const	{
         const XMLAttribute* a = FindAttribute( name );
         if ( !a ) {
@@ -1250,7 +1302,17 @@ public:
         }
         return a->QueryUnsignedValue( value );
     }
-    /// See QueryIntAttribute()
+
+	/// See QueryIntAttribute()
+	XMLError QueryInt64Attribute(const char* name, int64_t* value) const {
+		const XMLAttribute* a = FindAttribute(name);
+		if (!a) {
+			return XML_NO_ATTRIBUTE;
+		}
+		return a->QueryInt64Value(value);
+	}
+
+	/// See QueryIntAttribute()
     XMLError QueryBoolAttribute( const char* name, bool* value ) const				{
         const XMLAttribute* a = FindAttribute( name );
         if ( !a ) {
@@ -1277,7 +1339,7 @@ public:
 
 	
     /** Given an attribute name, QueryAttribute() returns
-    	XML_NO_ERROR, XML_WRONG_ATTRIBUTE_TYPE if the conversion
+    	XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion
     	can't be performed, or XML_NO_ATTRIBUTE if the attribute
     	doesn't exist. It is overloaded for the primitive types,
 		and is a generally more convenient replacement of
@@ -1301,6 +1363,10 @@ public:
 		return QueryUnsignedAttribute( name, value );
 	}
 
+	int QueryAttribute(const char* name, int64_t* value) const {
+		return QueryInt64Attribute(name, value);
+	}
+
 	int QueryAttribute( const char* name, bool* value ) const {
 		return QueryBoolAttribute( name, value );
 	}
@@ -1328,7 +1394,14 @@ public:
         XMLAttribute* a = FindOrCreateAttribute( name );
         a->SetAttribute( value );
     }
-    /// Sets the named attribute to value.
+
+	/// Sets the named attribute to value.
+	void SetAttribute(const char* name, int64_t value) {
+		XMLAttribute* a = FindOrCreateAttribute(name);
+		a->SetAttribute(value);
+	}
+
+	/// Sets the named attribute to value.
     void SetAttribute( const char* name, bool value )			{
         XMLAttribute* a = FindOrCreateAttribute( name );
         a->SetAttribute( value );
@@ -1425,7 +1498,9 @@ public:
     void SetText( int value );
     /// Convenience method for setting text inside an element. See SetText() for important limitations.
     void SetText( unsigned value );  
-    /// Convenience method for setting text inside an element. See SetText() for important limitations.
+	/// Convenience method for setting text inside an element. See SetText() for important limitations.
+	void SetText(int64_t value);
+	/// Convenience method for setting text inside an element. See SetText() for important limitations.
     void SetText( bool value );  
     /// Convenience method for setting text inside an element. See SetText() for important limitations.
     void SetText( double value );  
@@ -1461,13 +1536,28 @@ public:
     XMLError QueryIntText( int* ival ) const;
     /// See QueryIntText()
     XMLError QueryUnsignedText( unsigned* uval ) const;
-    /// See QueryIntText()
+	/// See QueryIntText()
+	XMLError QueryInt64Text(int64_t* uval) const;
+	/// See QueryIntText()
     XMLError QueryBoolText( bool* bval ) const;
     /// See QueryIntText()
     XMLError QueryDoubleText( double* dval ) const;
     /// See QueryIntText()
     XMLError QueryFloatText( float* fval ) const;
 
+	int IntText(int defaultValue = 0) const;
+
+	/// See QueryIntText()
+	unsigned UnsignedText(unsigned defaultValue = 0) const;
+	/// See QueryIntText()
+	int64_t Int64Text(int64_t defaultValue = 0) const;
+	/// See QueryIntText()
+	bool BoolText(bool defaultValue = false) const;
+	/// See QueryIntText()
+	double DoubleText(double defaultValue = 0) const;
+	/// See QueryIntText()
+	float FloatText(float defaultValue = 0) const;
+
     // internal:
     enum {
         OPEN,		// <foo>
@@ -1481,7 +1571,7 @@ public:
     virtual bool ShallowEqual( const XMLNode* compare ) const;
 
 protected:
-    char* ParseDeep( char* p, StrPair* endTag );
+    char* ParseDeep( char* p, StrPair* endTag, int* curLineNumPtr );
 
 private:
     XMLElement( XMLDocument* doc );
@@ -1494,8 +1584,9 @@ private:
     }
     XMLAttribute* FindOrCreateAttribute( const char* name );
     //void LinkAttribute( XMLAttribute* attrib );
-    char* ParseAttributes( char* p );
+    char* ParseAttributes( char* p, int* curLineNumPtr );
     static void DeleteAttribute( XMLAttribute* attribute );
+    XMLAttribute* CreateAttribute();
 
     enum { BUF_SIZE = 200 };
     int _closingType;
@@ -1522,7 +1613,7 @@ class TINYXML2_LIB XMLDocument : public XMLNode
     friend class XMLElement;
 public:
     /// constructor
-    XMLDocument( bool processEntities = true, Whitespace = PRESERVE_WHITESPACE );
+    XMLDocument( bool processEntities = true, Whitespace whitespaceMode = PRESERVE_WHITESPACE );
     ~XMLDocument();
 
     virtual XMLDocument* ToDocument()				{
@@ -1536,7 +1627,7 @@ public:
 
     /**
     	Parse an XML file from a character string.
-    	Returns XML_NO_ERROR (0) on success, or
+    	Returns XML_SUCCESS (0) on success, or
     	an errorID.
 
     	You may optionally pass in the 'nBytes', which is
@@ -1548,7 +1639,7 @@ public:
 
     /**
     	Load an XML file from disk.
-    	Returns XML_NO_ERROR (0) on success, or
+    	Returns XML_SUCCESS (0) on success, or
     	an errorID.
     */
     XMLError LoadFile( const char* filename );
@@ -1561,14 +1652,14 @@ public:
         not text in order for TinyXML-2 to correctly
         do newline normalization.
 
-    	Returns XML_NO_ERROR (0) on success, or
+    	Returns XML_SUCCESS (0) on success, or
     	an errorID.
     */
     XMLError LoadFile( FILE* );
 
     /**
     	Save the XML file to disk.
-    	Returns XML_NO_ERROR (0) on success, or
+    	Returns XML_SUCCESS (0) on success, or
     	an errorID.
     */
     XMLError SaveFile( const char* filename, bool compact = false );
@@ -1577,7 +1668,7 @@ public:
     	Save the XML file to disk. You are responsible
     	for providing and closing the FILE*.
 
-    	Returns XML_NO_ERROR (0) on success, or
+    	Returns XML_SUCCESS (0) on success, or
     	an errorID.
     */
     XMLError SaveFile( FILE* fp, bool compact = false );
@@ -1586,7 +1677,7 @@ public:
         return _processEntities;
     }
     Whitespace WhitespaceMode() const	{
-        return _whitespace;
+        return _whitespaceMode;
     }
 
     /**
@@ -1671,25 +1762,35 @@ public:
     */
     void DeleteNode( XMLNode* node );
 
-    void SetError( XMLError error, const char* str1, const char* str2 );
+    void SetError( XMLError error, const char* str1, const char* str2, int lineNum );
+
+    void ClearError() {
+        SetError(XML_SUCCESS, 0, 0, 0);
+    }
 
     /// Return true if there was an error parsing the document.
     bool Error() const {
-        return _errorID != XML_NO_ERROR;
+        return _errorID != XML_SUCCESS;
     }
     /// Return the errorID.
     XMLError  ErrorID() const {
         return _errorID;
     }
 	const char* ErrorName() const;
+    static const char* ErrorIDToName(XMLError errorID);
 
     /// Return a possibly helpful diagnostic location or string.
     const char* GetErrorStr1() const {
-        return _errorStr1;
+        return _errorStr1.GetStr();
     }
     /// Return a possibly helpful secondary diagnostic location or string.
     const char* GetErrorStr2() const {
-        return _errorStr2;
+        return _errorStr2.GetStr();
+    }
+    /// Return the line where the error occured, or zero if unknown.
+    int GetErrorLineNum() const
+    {
+        return _errorLineNum;
     }
     /// If there is an error, print it to stdout.
     void PrintError() const;
@@ -1711,13 +1812,15 @@ private:
     XMLDocument( const XMLDocument& );	// not supported
     void operator=( const XMLDocument& );	// not supported
 
-    bool        _writeBOM;
-    bool        _processEntities;
-    XMLError    _errorID;
-    Whitespace  _whitespace;
-    const char* _errorStr1;
-    const char* _errorStr2;
-    char*       _charBuffer;
+    bool			_writeBOM;
+    bool			_processEntities;
+    XMLError		_errorID;
+    Whitespace		_whitespaceMode;
+    mutable StrPair	_errorStr1;
+    mutable StrPair	_errorStr2;
+    int             _errorLineNum;
+    char*			_charBuffer;
+    int				_parseCurLineNum;
 
     MemPoolT< sizeof(XMLElement) >	 _elementPool;
     MemPoolT< sizeof(XMLAttribute) > _attributePool;
@@ -1727,8 +1830,21 @@ private:
 	static const char* _errorNames[XML_ERROR_COUNT];
 
     void Parse();
+
+    template<class NodeType, int PoolElementSize>
+    NodeType* CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool );
 };
 
+template<class NodeType, int PoolElementSize>
+inline NodeType* XMLDocument::CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool )
+{
+    TIXMLASSERT( sizeof( NodeType ) == PoolElementSize );
+    TIXMLASSERT( sizeof( NodeType ) == pool.ItemSize() );
+    NodeType* returnNode = new (pool.Alloc()) NodeType( this );
+    TIXMLASSERT( returnNode );
+    returnNode->_memPool = &pool;
+    return returnNode;
+}
 
 /**
 	A XMLHandle is a class that wraps a node pointer with null checks; this is
@@ -1845,19 +1961,19 @@ public:
     }
     /// Safe cast to XMLElement. This can return null.
     XMLElement* ToElement() 					{
-        return ( ( _node == 0 ) ? 0 : _node->ToElement() );
+        return ( _node ? _node->ToElement() : 0 );
     }
     /// Safe cast to XMLText. This can return null.
     XMLText* ToText() 							{
-        return ( ( _node == 0 ) ? 0 : _node->ToText() );
+        return ( _node ? _node->ToText() : 0 );
     }
     /// Safe cast to XMLUnknown. This can return null.
     XMLUnknown* ToUnknown() 					{
-        return ( ( _node == 0 ) ? 0 : _node->ToUnknown() );
+        return ( _node ? _node->ToUnknown() : 0 );
     }
     /// Safe cast to XMLDeclaration. This can return null.
     XMLDeclaration* ToDeclaration() 			{
-        return ( ( _node == 0 ) ? 0 : _node->ToDeclaration() );
+        return ( _node ? _node->ToDeclaration() : 0 );
     }
 
 private:
@@ -1917,16 +2033,16 @@ public:
         return _node;
     }
     const XMLElement* ToElement() const			{
-        return ( ( _node == 0 ) ? 0 : _node->ToElement() );
+        return ( _node ? _node->ToElement() : 0 );
     }
     const XMLText* ToText() const				{
-        return ( ( _node == 0 ) ? 0 : _node->ToText() );
+        return ( _node ? _node->ToText() : 0 );
     }
     const XMLUnknown* ToUnknown() const			{
-        return ( ( _node == 0 ) ? 0 : _node->ToUnknown() );
+        return ( _node ? _node->ToUnknown() : 0 );
     }
     const XMLDeclaration* ToDeclaration() const	{
-        return ( ( _node == 0 ) ? 0 : _node->ToDeclaration() );
+        return ( _node ? _node->ToDeclaration() : 0 );
     }
 
 private:
@@ -1998,7 +2114,8 @@ public:
     void PushAttribute( const char* name, const char* value );
     void PushAttribute( const char* name, int value );
     void PushAttribute( const char* name, unsigned value );
-    void PushAttribute( const char* name, bool value );
+	void PushAttribute(const char* name, int64_t value);
+	void PushAttribute( const char* name, bool value );
     void PushAttribute( const char* name, double value );
     /// If streaming, close the Element.
     virtual void CloseElement( bool compactMode=false );
@@ -2009,7 +2126,9 @@ public:
     void PushText( int value );
     /// Add a text node from an unsigned.
     void PushText( unsigned value );
-    /// Add a text node from a bool.
+	/// Add a text node from an unsigned.
+	void PushText(int64_t value);
+	/// Add a text node from a bool.
     void PushText( bool value );
     /// Add a text node from a float.
     void PushText( float value );

main.cpp

@@ -25,12 +25,15 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
 	//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
 
-	//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*";
-	::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
+    //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
+    //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
 
 
+	//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
+    ::testing::GTEST_FLAG(filter) = "*Jensen*";
 
-	//::testing::GTEST_FLAG(filter) = "*Barometer*";
+
+    //::testing::GTEST_FLAG(filter) = "*Barometer*";
 	//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";
 
 	//::testing::GTEST_FLAG(filter) = "Heading*";

math/Distributions.h

@@ -8,5 +8,7 @@
 #include "distribution/VonMises.h"
 #include "distribution/Region.h"
 #include "distribution/Triangle.h"
+#include "distribution/NormalN.h"
+#include "distribution/Rectangular.h"
 
 #endif // DISTRIBUTIONS_H

math/Interpolator.h

@@ -3,6 +3,7 @@
 
 #include "../Assertions.h"
 #include "../Exception.h"
+#include "../data/Timestamp.h"
 
 #include <vector>
 #include <algorithm>
@@ -49,7 +50,7 @@ public:
 
 		// interpolate
 		const int idx1 = (idx2 > 0) ? (idx2 - 1) : (idx2);
-		const float percent = (key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key);
+		const float percent = getPercent(key, entries[idx1].key, entries[idx2].key); //(key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key);
 		const Value res = entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent;
 		return res;
 
@@ -57,6 +58,16 @@ public:
 
 protected:
 
+	/** special interpolation for the timestamp class */
+	static inline float getPercent(const Timestamp key, const Timestamp t1, const Timestamp t2) {
+		return (key - t1).ms() / (float) (t2 - t1).ms();
+	}
+
+	/** interpolation for generic datatypes [int, float, double, ..] */
+	template <typename T> static inline float getPercent(const T key, const T t1, const T t2) {
+		return (key - t1) / (float) (t2 - t1);
+	}
+
 	/** get the nearest index for the given key */
 	int getIdxAfter(const Key key) const {
 

math/MovingAVG.h

@@ -11,7 +11,7 @@ private:
 	std::vector<T> values;
 
 	/** track the current sum of the vector's values */
-	T curSum = 0;
+	T curSum;
 
 	/** the number of elements to average */
 	int size;
@@ -19,7 +19,7 @@ private:
 public:
 
 	/** ctor */
-	MovingAVG(const int size) : size(size) {;}
+	MovingAVG(const int size) : curSum(), size(size) {;}
 
 	/** add a new value */
 	void add(const T val) {

math/MovingStdDevTS.h

@@ -0,0 +1,49 @@
+#ifndef MOVINGSTDDEVTS_H
+#define MOVINGSTDDEVTS_H
+
+#include "MovingAverageTS.h"
+
+/**
+ * moving stadnard-deviation using a given time-region
+ */
+template <typename T> class MovingStdDevTS {
+
+private:
+
+	MovingAverageTS<T> avg;
+	MovingAverageTS<T> avg2;
+
+
+public:
+
+
+	/** ctor with the window-size to use */
+	MovingStdDevTS(const Timestamp window, const T zeroElement) : avg(window, zeroElement), avg2(window, zeroElement) {
+		;
+	}
+
+	/** add a new entry */
+	void add(const Timestamp ts, const T& data) {
+
+		// E(x)
+		avg.add(ts, data);
+
+		// E(x^2)
+		avg2.add(ts, data*data);
+
+	}
+
+	/** get the current std-dev */
+	T get() const {
+
+		const T e = avg.get();
+		const T e2 = avg2.get();
+		const T var = e2 - e*e;
+		return std::sqrt(var);
+
+	}
+
+
+};
+
+#endif // MOVINGSTDDEVTS_H

math/Random.h

@@ -18,10 +18,10 @@ class RandomGenerator : public std::minstd_rand {
 public:
 
 	/** ctor with default seed */
-	RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
+    RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
 
 	/** ctor with custom seed */
-	RandomGenerator(result_type seed) : std::minstd_rand(seed) {;}
+    RandomGenerator(result_type seed) : std::minstd_rand(seed) {;}
 
 };
 

math/distribution/KernelDensity.h

@@ -0,0 +1,35 @@
+#ifndef KERNELDENSITY_H
+#define KERNELDENSITY_H
+
+#include <cmath>
+#include <random>
+#include <functional>
+
+#include <eigen3/Eigen/Dense>
+
+#include "../../Assertions.h"
+#include "../Random.h"
+
+
+namespace Distribution {
+
+    template <typename T, typename Sample> class KernelDensity{
+
+    private:
+        const std::function<T(Sample)> probabilityFunction;
+
+
+    public:
+        KernelDensity(const std::function<T(Sample)> probabilityFunction) : probabilityFunction(probabilityFunction){
+
+        }
+
+        T getProbability(Sample sample){
+            return probabilityFunction(sample);
+        }
+
+    };
+
+}
+
+#endif // KERNELDENSITY_H

math/distribution/Normal.h

@@ -44,6 +44,15 @@ namespace Distribution {
 			gen.seed(seed);
 		}
 
+        /** get the mean value */
+        const T getMu() {
+            return this->mu;
+        }
+
+        /** get the standard deviation */
+        const T getSigma() {
+            return this->sigma;
+        }
 
 		/** get the probability for the given value */
 		static T getProbability(const T mu, const T sigma, const T val) {

math/distribution/NormalN.h

@@ -0,0 +1,102 @@
+#ifndef NORMALN_H
+#define NORMALN_H
+
+#include <cmath>
+#include <random>
+
+#include <eigen3/Eigen/Dense>
+
+#include "../../Assertions.h"
+#include "../Random.h"
+
+namespace Distribution {
+
+    class NormalDistributionN {
+
+    private:
+
+        Eigen::VectorXd mu;
+        Eigen::MatrixXd sigma;
+
+        const double _a;
+        const Eigen::MatrixXd _sigmaInv;
+
+        const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
+        Eigen::MatrixXd transform; //can i make this const?
+
+        RandomGenerator gen;
+        std::normal_distribution<> dist;
+
+    public:
+
+        /** ctor */
+        NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) :
+            mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()), eigenSolver(sigma) {
+
+            transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
+        }
+
+        /** get probability for the given value */
+        double getProbability(const Eigen::VectorXd val) const {
+            const double b = ((val-this->mu).transpose() * this->_sigmaInv * (val-this->mu));
+            return this->_a * std::exp(-b/2.0);
+        }
+
+        /** get a randomly drawn sample from the given normalN distribution*/
+        Eigen::VectorXd draw() {
+            return this->mu + this->transform * Eigen::VectorXd{ this->mu.size() }.unaryExpr([&](double x) { return dist(gen); });
+        }
+
+        /** get the mean vector */
+        const Eigen::VectorXd getMu(){
+            return this->mu;
+        }
+
+        /** get covariance matrix */
+        const Eigen::MatrixXd getSigma(){
+            return this->sigma;
+        }
+
+        const Eigen::MatrixXd getSigmaInv(){
+            return this->_sigmaInv;
+        }
+
+        void setSigma(Eigen::MatrixXd sigma){
+            this->sigma = sigma;
+        }
+
+        void setMu(Eigen::VectorXd mu){
+            this->mu = mu;
+        }
+
+        /** return a NormalN based on given data */
+        static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) {
+
+                const int numElements = data.rows();
+                Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the distribution!");
+                Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!");
+
+                const Eigen::VectorXd mean = data.colwise().mean();
+                const Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
+                const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1);
+
+                return NormalDistributionN(mean, cov);
+        }
+
+        /** return a NormalN based on given data and a given mean vector mu*/
+        static NormalDistributionN getNormalNFromSamplesAndMean(const Eigen::MatrixXd& data, const Eigen::VectorXd mean) {
+
+                const int numElements = data.rows();
+                Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the distribution!");
+                Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!");
+
+                const Eigen::MatrixXd centered = data.rowwise() - mean.transpose();
+                Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1);
+
+                return NormalDistributionN(mean, cov);
+        }
+
+    };
+
+}
+#endif // NORMALN_H

math/distribution/Rectangular.h

@@ -0,0 +1,46 @@
+#ifndef RECTANGULAR_H
+#define RECTANGULAR_H
+
+
+#include <cmath>
+#include <random>
+#include "../Random.h"
+#include "../../Assertions.h"
+#include "Normal.h"
+
+namespace Distribution {
+
+	/** normal distribution */
+	template <typename T> class Rectangular {
+
+	private:
+
+		const T mu;
+		const T h;
+		const T width;
+
+	public:
+
+		/** ctor */
+		Rectangular(const T mu, const T width) : mu(mu), h(1.0/width), width(width) {
+
+		}
+
+		/** get probability for the given value */
+		T getProbability(const T val) const {
+			const T diff = std::abs(val - mu);
+			return (diff < width/2)	? (h) : (0.0);
+		}
+
+		/** get the probability for the given value */
+		static T getProbability(const T mu, const T width, const T val) {
+			Rectangular<T> dist(mu, width);
+			return dist.getProbability(val);
+		}
+
+	};
+
+}
+
+
+#endif // RECTANGULAR_H

math/distribution/VonMises.h

@@ -19,7 +19,7 @@ namespace Distribution {
 		const T mu;
 
 		/** like 1.0/variance of the distribution */
-		const T kappa;
+        T kappa;
 
 		/** pre-calcuated look-up-table */
 		std::vector<T> lut;
@@ -30,7 +30,7 @@ namespace Distribution {
 	public:
 
 		/** ctor */
-		VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) {
+        VonMises(const T mu, T kappa) : mu(mu), kappa(kappa) {
 
 		}
 
@@ -49,6 +49,10 @@ namespace Distribution {
 
 		}
 
+        void setKappa(T _kappa){
+            kappa = _kappa;
+        }
+
 	};
 
 }

math/divergence/JensenShannon.h

@@ -0,0 +1,36 @@
+#ifndef JENSENSHANNON_H
+#define JENSENSHANNON_H
+
+#include "KullbackLeibler.h"
+
+#include "../../Assertions.h"
+#include <string>
+
+
+namespace Divergence {
+
+template <typename Scalar> class JensenShannon {
+
+public:
+    /** Calculate the Jensen Shannon Divergece from a set of sample densities
+     *  Info: https://en.wikipedia.org/wiki/Jensen–Shannon_divergence
+     * @param P is the vector containing the densities of a set of samples
+     * @param Q is a vector containg the densities of the same samples set then P
+    */
+    static inline Scalar getGeneralFromSamples(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){
+        // normalize
+        P /= P.sum();
+        Q /= Q.sum();
+
+        Assert::isNear((double)P.sum(), 1.0, 0.01,"Normalization failed.. this shouldn't happen");
+        Assert::isNear((double)Q.sum(), 1.0, 0.01, "Normalization failed.. this shouldn't happen");
+
+        Eigen::VectorXd M = 0.5 * (P + Q);
+
+        return (0.5 * KullbackLeibler<Scalar>::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler<Scalar>::getGeneralFromSamples(Q, M, mode));
+    }
+};
+
+}
+
+#endif // JENSENSHANNON_H

math/divergence/KullbackLeibler.h

@@ -0,0 +1,168 @@
+#ifndef KULLBACKLEIBLER_H
+#define KULLBACKLEIBLER_H
+
+#include "../distribution/Normal.h"
+#include "../distribution/NormalN.h"
+
+#include "../../Assertions.h"
+#include <string>
+
+namespace Divergence {
+
+    enum LOGMODE{
+        BASE2,
+        BASE10,
+        NATURALIS
+    };
+
+    template <typename Scalar> class KullbackLeibler {
+
+        public:
+
+            /** Calculate the Kullback Leibler Distance from a set of sample densities
+             *  Info: https://en.wikipedia.org/wiki/Kullback%E2%80%93Leibler_divergence
+             * @param P is the vector containing the densities of a set of samples
+             * @param Q is a vector containg the densities of the same samples set then P
+            */
+            static inline Scalar getGeneralFromSamples(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){
+
+                // Assure P and Q have the same size and are finite in all values
+                Assert::equal(P.size(), Q.size(), "The sample vectors P and Q do not have the same size.");
+                if(!P.allFinite() || !Q.allFinite()){
+                    Assert::doThrow("The sample vectors P and Q are not finite. Check for NaN or Inf.");
+                }
+
+                Scalar dist = 0.0;
+                Scalar PQratio = 0.0;
+
+                //normalize to 1
+                P /= P.sum();
+                Q /= Q.sum();
+
+                Assert::isNear((double)P.sum(), 1.0, 0.01,"Normalization failed.. this shouldn't happen");
+                Assert::isNear((double)Q.sum(), 1.0, 0.01, "Normalization failed.. this shouldn't happen");
+
+                //sum up the logarithmic difference between P and Q, also called kullback leibler...
+                for(int i = 0; i < P.size(); ++i){
+
+                    // if both prob are near zero we assume a 0 distance since lim->0 = 0
+                    if((P[i] == 0.0) && (Q[i] == 0.0)){
+                        dist += 0.0;
+                    }
+                    //if prob of P is 0 we also assume a 0 distance since lim->0 0 * log(0) = 0
+                    else if ((P[i] == 0.0) && (Q[i] > 0.0)){
+                        dist += 0.0;
+                    } else{
+
+                        // calc PQratio
+                        if(Q[i] == 0.0){
+                            Assert::doThrow("Division by zero is not allowed ;).");
+                            //PQratio = P[i] / 0.00001;
+                        } else {
+                            PQratio = P[i] / Q[i];
+                        }
+
+                        //use log for dist
+                        if (mode == NATURALIS){
+                            dist += P[i] * log(PQratio);
+                        }
+
+                        else if (mode == BASE2){
+                            dist += P[i] * log2(PQratio);
+                        }
+
+                        else if (mode == BASE10){
+                            dist += P[i] * log10(PQratio);
+                        }
+
+                    }
+                }
+
+                return dist;
+            }
+
+            static inline Scalar getGeneralFromSamplesSymmetric(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){
+                return getGeneralFromSamples(P, Q, mode) + getGeneralFromSamples(Q, P, mode);
+            }
+
+            /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution
+            *   Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/
+            */
+            static inline Scalar getUnivariateGauss(Distribution::Normal<Scalar> norm1, Distribution::Normal<Scalar> norm2){
+
+                auto sigma1Quad = norm1.getSigma() * norm1.getSigma();
+                auto sigma2Quad = norm2.getSigma() * norm2.getSigma();
+                auto mu12Quad = (norm1.getMu() - norm2.getMu()) * (norm1.getMu() - norm2.getMu());
+                auto log1 = std::log(norm1.getSigma());
+                auto log2 = std::log(norm2.getSigma());
+
+                // kl = log(sigma_2 / sigma_1) + ((sigma_1^2 + (mu_1 - mu_2)^2) / 2 * sigma_2^2) - 0.5
+                double klb = (log2 - log1) + ((sigma1Quad + mu12Quad)/(2.0 * sigma2Quad)) - 0.5;
+
+                //klb is always greater 0
+                if(klb < 0.0){
+                    Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible");
+                }
+                return klb;
+            }
+
+            /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution symmetric*/
+            static inline Scalar getUnivariateGaussSymmetric(Distribution::Normal<Scalar> norm1, Distribution::Normal<Scalar> norm2){
+                return getUnivariateGauss(norm1, norm2) + getUnivariateGauss(norm2, norm1);
+            }
+
+            /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution */
+            static inline Scalar getMultivariateGauss(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){
+
+                //both gaussian have the same dimension.
+                Assert::equal(norm1.getMu().rows(), norm2.getMu().rows(), "mean vectors do not have the same dimension");
+                Assert::equal(norm1.getSigma().rows(), norm2.getSigma().rows(), "cov matrices do not have the same dimension");
+                Assert::equal(norm1.getSigma().cols(), norm2.getSigma().cols(), "cov matrices do not have the same dimension");
+
+                //log
+                auto det1 = norm1.getSigma().determinant();
+                auto det2 = norm2.getSigma().determinant();
+                auto log1 = std::log(det1);
+                auto log2 = std::log(det2);
+
+                //determinate shouldn't be 0!
+                Assert::isNot0(det1, "Determinat of the first Gauss is Zero! Check the Cov Matrix.");
+                Assert::isNot0(det2, "Determinat of the second Gauss is Zero! Check the Cov Matrix.");
+
+                //trace
+                Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols());
+                toTrace = norm2.getSigmaInv() * norm1.getSigma();
+                auto trace = toTrace.trace();
+
+                //transpose
+                Eigen::VectorXd toTranspose(norm1.getMu().rows());
+                toTranspose = norm2.getMu() - norm1.getMu();
+                auto transpose = toTranspose.transpose();
+
+                //rawdensity
+                auto rawDensity = transpose * norm2.getSigmaInv() * toTranspose;
+                auto dimension = norm1.getMu().rows();
+
+                //0.5 * ((log(det(cov_2)/det(cov_1)) + tr(cov_2^-1 cov_1) + (mu_2 - mu_1)^T * cov_2^-1 * (mu_2 - mu_1) - dimension)
+                double klb = 0.5 * ((log2 - log1) + trace + rawDensity - dimension);
+
+                //klb is always greater 0
+                if(klb < 0.0){
+                    Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible");
+                }
+                Assert::isNotNaN(klb, "The Kullback Leibler Distance is NaN!");
+
+                return klb;
+            }
+
+             /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution symmetric*/
+            static inline Scalar getMultivariateGaussSymmetric(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){
+                return getMultivariateGauss(norm1, norm2) + getMultivariateGauss(norm2, norm1);
+            }
+
+
+    };
+
+}
+
+#endif // KULLBACKLEIBLER_H

math/filter/Butterworth.h

@@ -77,9 +77,9 @@ namespace Filter {
 
                 const Scalar _b0, _b1, _b2, _a1, _a2, _gain;
 
-                const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2;
+				Scalar _z1, _z2;
 
-                Scalar _z1, _z2;
+                const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2;
 
         };
 

sensors/MACAddress.h

@@ -24,7 +24,7 @@ private:
 	} fields;
 
 	/** store as 64-bit integer */
-	uint64_t mac;
+	uint64_t mac = 0;
 
 public:
 
@@ -47,15 +47,27 @@ public:
 	MACAddress(const std::string& str) {
 
 		// sanity check
-		if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");}
-
-		mac = 0;	// all zeros
-		fields.h5 = hexWordToInt(str[ 0], str[ 1]);
-		fields.h4 = hexWordToInt(str[ 3], str[ 4]);
-		fields.h3 = hexWordToInt(str[ 6], str[ 7]);
-		fields.h2 = hexWordToInt(str[ 9], str[10]);
-		fields.h1 = hexWordToInt(str[12], str[13]);
-		fields.h0 = hexWordToInt(str[15], str[16]);
+        if (str.size() == 17 ){
+            mac = 0;	// all zeros
+            fields.h5 = hexWordToInt(str[ 0], str[ 1]);
+            fields.h4 = hexWordToInt(str[ 3], str[ 4]);
+            fields.h3 = hexWordToInt(str[ 6], str[ 7]);
+            fields.h2 = hexWordToInt(str[ 9], str[10]);
+            fields.h1 = hexWordToInt(str[12], str[13]);
+            fields.h0 = hexWordToInt(str[15], str[16]);
+        }
+        else if (str.size() == 12){
+            mac = 0;	// all zeros
+            fields.h5 = hexWordToInt(str[ 0], str[ 1]);
+            fields.h4 = hexWordToInt(str[ 2], str[ 3]);
+            fields.h3 = hexWordToInt(str[ 4], str[ 5]);
+			fields.h2 = hexWordToInt(str[ 6], str[ 7]);
+			fields.h1 = hexWordToInt(str[ 8], str[ 9]);
+            fields.h0 = hexWordToInt(str[10], str[11]);
+        }
+        else{
+            throw Exception("invalid hex string length. must be 17 or 12 (without :)");
+        }
 
 	}
 
@@ -77,7 +89,9 @@ public:
 
 	/** get the mac address as a long-int value */
 	uint64_t asLong() const {
-		return mac;
+		// even if we initialized all 8 bytes with zero
+		// we mask the 2 unsued bytes to be absolutely safe
+		return mac & 0x0000FFFFFFFFFFFF;
 	}
 
 	/** equal? */
@@ -93,13 +107,19 @@ public:
 private:
 
 	/** convert the given hex char [0-F] to an integer [0-15] */
-	static uint8_t hexCharToInt(const char _hex) {
-
-		// to upper case
-		const char hex = (_hex >= 'a') ? (_hex - ('a' - 'A')) : (_hex);
+	static uint8_t hexCharToInt(const char hex) {
 
 		// convert
-		return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10);
+		if (hex >= '0' && hex <= '9') {			// digits
+			return (hex - '0');
+		} else if (hex >= 'a' && hex <= 'f') {	// lower case
+			return (hex - 'a' + 10);
+		} else if (hex >= 'A' && hex <= 'F') {	// upper case
+			return (hex - 'A' + 10);
+		}
+
+		// found an invalid character
+		throw Exception("invalid character within MAC address");
 
 	}
 

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,158 @@
+#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> baroAvgSlow;
+	MovingAverageTS<float> baroAvgFast;
+
+	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/beacon/Beacon.h

@@ -0,0 +1,59 @@
+#ifndef BEACON_H
+#define BEACON_H
+
+#include "../MACAddress.h"
+
+/**
+ * represents a single beacon
+ * a beacon is represented by its MAC-Address and
+ * may provide a sending power TXP
+ */
+class Beacon {
+
+private:
+
+	/** the AP's MAC-Address */
+	MACAddress mac;
+
+    /** OPTIONAL the beacons sending power */
+    float txp;
+
+public:
+
+	/** empty ctor */
+    Beacon() {
+		;
+	}
+
+    /** ctor with MAC and TXP */
+    Beacon(const MACAddress& mac, const float& txp) : mac(mac), txp(txp) {
+		;
+	}
+
+    /** ctor with MAC and TXP */
+    Beacon(const std::string& mac, const float& txp) : mac(mac), txp(txp) {
+		;
+	}
+
+    /** ctor with MAC and without TXP */
+    Beacon(const MACAddress& mac) : mac(mac), txp() {
+		;
+	}
+
+    /** ctor with MAC and without TXP */
+    Beacon(const std::string& mac) : mac(mac), txp() {
+		;
+	}
+
+public:
+
+	/** get the AP's MAC address */
+	inline const MACAddress& getMAC() const {return mac;}
+
+	/** OPTIONAL: get the AP's ssid (if any) */
+    inline const float& getTXP() const {return txp;}
+
+
+};
+
+#endif // BEACON_H

sensors/beacon/BeaconMeasurement.h

@@ -0,0 +1,44 @@
+#ifndef BEACONMEASUREMENT_H
+#define BEACONMEASUREMENT_H
+
+#include "../MACAddress.h"
+#include "../../data/Timestamp.h"
+#include "Beacon.h"
+
+#include <vector>
+
+
+/** one observed AP and its signal strength */
+class BeaconMeasurement {
+
+private:
+
+    /** the timestamp this beacon was discovered at */
+    Timestamp ts;
+
+    /** the beacon's mac address */
+    Beacon beacon;
+
+    /** signal strength */
+    float rssi;
+
+public:
+
+    /** ctor */
+    BeaconMeasurement(const Timestamp ts, const Beacon& beacon, const float rssi) : ts(ts), beacon(beacon), rssi(rssi) {;}
+
+
+public:
+
+    /** get the beacon */
+    const Beacon& getBeacon() const {return beacon;}
+
+    /** get the measurements timestamp */
+    const Timestamp& getTimestamp() const {return ts;}
+
+    /** get the rssi */
+    float getRSSI() const {return rssi;}
+};
+
+
+#endif // BEACONMEASUREMENT_H

sensors/beacon/BeaconMeasurements.h

@@ -0,0 +1,26 @@
+#ifndef BEACONMEASUREMENTS_H
+#define BEACONMEASUREMENTS_H
+
+#include <vector>
+
+#include "BeaconMeasurement.h"
+
+/**
+ * group of several beacon measurements
+ */
+struct BeaconMeasurements {
+
+    std::vector<BeaconMeasurement> entries;
+
+    /** remove entries older then 3000 ms*/
+    void removeOld(const Timestamp latestTS) {
+        auto lambda = [latestTS] (const BeaconMeasurement& e) {
+            Timestamp age = latestTS - e.getTimestamp();
+            return age > Timestamp::fromMS(1000*3);
+        };
+        entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end());
+    }
+
+};
+
+#endif // BEACONMEASUREMENTS_H

sensors/beacon/BeaconProbability.h

@@ -0,0 +1,15 @@
+#ifndef BEACONPROBABILITY_H
+#define BEACONPROBABILITY_H
+
+#include "BeaconMeasurements.h"
+
+/**
+ * base class for all Beacon probability calculators.
+ * such a calculator determines the probabilty for a location (e.g. x,y,z)
+ * given BeaconMeasurements
+ */
+class BeaconProbability {
+
+};
+
+#endif // BEACONPROBABILITY_H

sensors/beacon/BeaconProbabilityFree.h

@@ -0,0 +1,94 @@
+#ifndef BEACONPROBABILITYFREE_H
+#define BEACONPROBABILITYFREE_H
+
+#include "BeaconProbability.h"
+#include "BeaconMeasurements.h"
+#include "model/BeaconModel.h"
+#include "../../math/Distributions.h"
+#include "../../data/Timestamp.h"
+
+#include "../../floorplan/v2/Floorplan.h"
+
+#include <unordered_map>
+
+/**
+ * compare BeaconMeasurements within predictions of a given model.
+ * predictions are just based on the distance to the observed beacon.
+ */
+class BeaconObserverFree : public BeaconProbability {
+
+private:
+
+	const float sigma = 8.0f;
+	const float sigmaPerSecond = 3.0f;
+
+	/** the RSSI prediction model */
+    BeaconModel& model;
+
+	/** the map's floorplan */
+	Floorplan::IndoorMap* map;
+
+public:
+
+    /** ctor */
+    BeaconObserverFree(const float sigma, BeaconModel& model) : sigma(sigma), model(model) {
+
+	}
+
+    /** provides the probability for a specific point in space */
+    double getProbability(const Point3& pos_m, const Timestamp curTime, const BeaconMeasurements& obs) const {
+
+		double prob = 1.0;
+        int numMatchingBeacons = 0;
+
+		// process each measured AP
+        for (const BeaconMeasurement& entry : obs.entries) {
+
+			// sanity check
+			Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");
+
+            // updating the beacons sended txp if available
+            if(entry.getBeacon().getTXP() != 0.0f){
+                //TODO: check if this works
+                model.updateBeacon(entry);
+            }
+
+			// get the model's RSSI (if possible!)
+            const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m);
+
+			// NaN? -> AP not known to the model -> skip
+            if (modelRSSI != modelRSSI) {
+                continue;
+            }
+
+
+			// the scan's RSSI
+			const float scanRSSI = entry.getRSSI();
+
+			// the measurement's age
+			const Timestamp age = curTime - entry.getTimestamp();
+
+			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
+			Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
+
+			// sigma grows with measurement age
+			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
+
+			// update probability
+			prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
+
+            ++numMatchingBeacons;
+
+		}
+
+		// sanity check
+        //Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?");
+
+		return prob;
+
+	}
+
+};
+
+#endif // WIFIPROBABILITYFREE_H

sensors/beacon/model/BeaconModel.h

@@ -0,0 +1,46 @@
+#ifndef BEACONMODEL_H
+#define BEACONMODEL_H
+
+#include "../Beacon.h"
+#include "../BeaconMeasurement.h"
+#include "../../../geo/Point3.h"
+
+#include <vector>
+
+/**
+ * interface for signal-strength prediction models.
+ *
+ * the model is passed a MAC-address of an AP in question, and a position.
+ * hereafter the model returns the RSSI for this AP at the questioned location.
+ */
+class BeaconModel {
+
+public:
+
+//	/** get the given access-point's RSSI at the provided location */
+//	virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
+
+	/** get a list of all APs known to the model */
+    virtual std::vector<Beacon> getAllBeacons() const = 0;
+
+    /**
+     * update the beacons signal strength using the current measurement
+     * this could happen if the txp is not updated within the floorplan
+     *
+     * be careful and don't use fantasy values, this could ruin your localitions
+     * completely
+     */
+    virtual void updateBeacon(const BeaconMeasurement beacon) = 0;
+
+
+	/**
+	 * get the RSSI expected at the given location (in meter)
+     * for an beacon identified by the given MAC.
+	 *
+     * if the model can not predict the RSSI for an beacon, it returns NaN!
+	 */
+	virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0;
+
+};
+
+#endif // BEACONMODEL_H

sensors/beacon/model/BeaconModelLogDist.h

@@ -0,0 +1,92 @@
+#ifndef BEACONMODELLOGDIST_H
+#define BEACONMODELLOGDIST_H
+
+#include "BeaconModel.h"
+#include "../../radio/model/LogDistanceModel.h"
+
+#include <unordered_map>
+
+/**
+ * signal-strength estimation using log-distance model
+ */
+class BeaconModelLogDist : public BeaconModel {
+
+public:
+
+    /** parameters describing one beacons to the model */
+	struct APEntry {
+
+		Point3 position_m;	// the AP's position (in meter)
+		float txp;			// sending power (-40)
+		float exp;			// path-loss-exponent (~2.0 - 4.0)
+
+		/** ctor */
+		APEntry(const Point3 position_m, const float txp, const float exp) :
+			position_m(position_m), txp(txp), exp(exp) {;}
+
+	};
+
+private:
+
+    /** map of all beacons (and their parameters) known to the model */
+    std::unordered_map<MACAddress, APEntry> beacons;
+
+public:
+
+	/** ctor */
+    BeaconModelLogDist() {
+		;
+	}
+
+    /** get a list of all beacons known to the model */
+    std::vector<Beacon> getAllBeacons() const {
+        std::vector<Beacon> aps;
+        for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
+		return aps;
+	}
+
+    /** make the given beacon (and its parameters) known to the model */
+    void addAP(const MACAddress& beacon, const APEntry& params) {
+
+		// sanity check
+		Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-90:-30]");
+		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+
+		// add
+        beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
+
+	}
+
+    void updateBeacon(const BeaconMeasurement beacon) override{
+        // try to get the corresponding parameters
+        const auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return;}
+
+        it->second.txp = beacon.getBeacon().getTXP();
+    }
+
+    virtual float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
+
+		// try to get the corresponding parameters
+        const auto it = beacons.find(beacon);
+
+		// AP unknown? -> NAN
+        if (it == beacons.end()) {return NAN;}
+
+        // the beacons' parameters
+		const APEntry& params = it->second;
+
+		// free-space (line-of-sight) RSSI
+		const float distance_m = position_m.getDistance(params.position_m);
+		const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
+
+		// done
+		return rssiLOS;
+
+	}
+
+};
+
+#endif // BEACONMODELLOGDIST_H

sensors/beacon/model/BeaconModelLogDistCeiling.h

@@ -0,0 +1,208 @@
+#ifndef BEACONMODELLOGDISTCEILING_H
+#define BEACONMODELLOGDISTCEILING_H
+
+#include "../../../floorplan/v2/Floorplan.h"
+
+#include "../../../Assertions.h"
+#include "BeaconModel.h"
+#include "../../radio/model/LogDistanceModel.h"
+#include "../BeaconMeasurement.h"
+
+#include <unordered_map>
+
+/**
+ * signal-strength estimation using log-distance model
+ * including ceilings between beacon and position
+ */
+class BeaconModelLogDistCeiling : public BeaconModel {
+
+public:
+
+    /** parameters describing one beacon to the model */
+	struct APEntry {
+
+        Point3 position_m;	// the beacon's position (in meter)
+		float txp;			// sending power (-40)
+		float exp;			// path-loss-exponent (~2.0 - 4.0)
+		float waf;			// attenuation per ceiling/floor (~-8.0)
+
+		/** ctor */
+		APEntry(const Point3 position_m, const float txp, const float exp, const float waf) :
+			position_m(position_m), txp(txp), exp(exp), waf(waf) {;}
+
+	};
+
+private:
+
+    /** map of all beacons (and their parameters) known to the model */
+    std::unordered_map<MACAddress, APEntry> beacons;
+
+	/** position (height) of all ceilings (in meter) */
+	std::vector<float> ceilingsAtHeight_m;
+
+public:
+
+	/** ctor with floorplan (needed for ceiling position) */
+    BeaconModelLogDistCeiling(const Floorplan::IndoorMap* map) {
+
+		// sanity checks
+		Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
+
+		// position of all ceilings
+		for (Floorplan::Floor* f : map->floors) {
+			ceilingsAtHeight_m.push_back(f->atHeight);
+		}
+
+	}
+
+    /** get a list of all beacons known to the model */
+    std::vector<Beacon> getAllBeacons() const {
+        std::vector<Beacon> aps;
+        for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
+		return aps;
+	}
+
+    /** load beacon information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
+    void loadBeaconsFromMap(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+
+		for (const Floorplan::Floor* floor : map->floors) {
+            for (const Floorplan::Beacon* beacon : floor->beacons) {
+                APEntry ape(beacon->getPos(floor), txp, exp, waf);
+                addBeacon(MACAddress(beacon->mac), ape);
+			}
+		}
+
+	}
+
+    /** load beacon information from a vector. use the given fixed TXP/EXP/WAF for all APs */
+    void loadBeaconsFromVector(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+
+        for (const Floorplan::Floor* floor : map->floors) {
+            for (const Floorplan::Beacon* beacon : floor->beacons) {
+                APEntry ape(beacon->getPos(floor), txp, exp, waf);
+                addBeacon(MACAddress(beacon->mac), ape);
+            }
+        }
+
+    }
+
+    /** make the given beacon (and its parameters) known to the model */
+    void addBeacon(const MACAddress& beacon, const APEntry& params) {
+
+		// sanity check
+		Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
+                Assert::isBetween(params.txp, -90.0f, -30.0f, "TXP out of bounds [-50:-30]");
+		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+
+        //Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!");
+
+		// add
+        beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
+
+	}
+
+    void updateBeacon(const BeaconMeasurement beacon) override {
+        // try to get the corresponding parameters
+        auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return;}
+
+
+        // TODO: Check if this works as expected
+        it->second.txp = beacon.getBeacon().getTXP();
+    }
+
+	/** remove all added APs */
+	void clear() {
+        beacons.clear();
+	}
+
+    float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
+
+		// try to get the corresponding parameters
+        const auto it = beacons.find(beacon);
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return NAN;}
+
+		// the access-points' parameters
+		const APEntry& params = it->second;
+
+		// free-space (line-of-sight) RSSI
+		const float distance_m = position_m.getDistance(params.position_m);
+        const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
+
+		// WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value
+		const float wafLoss = params.waf * numCeilingsBetween(position_m, params.position_m);
+
+		// combine
+		return rssiLOS + wafLoss;
+
+	}
+
+
+
+protected:
+
+    FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilings);
+    FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilingsFloat);
+
+
+	/** get the number of ceilings between z1 and z2 */
+	float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
+
+
+		const float zMin = std::min(pos1.z, pos2.z);
+		const float zMax = std::max(pos1.z, pos2.z);
+
+		float cnt = 0;
+
+		for (const float z : ceilingsAtHeight_m) {
+			if (zMin < z && zMax > z) {
+				const float dmax = zMax - z;
+				cnt += (dmax > 1) ? (1) : (dmax);
+			}
+		}
+
+		return cnt;
+
+	}
+
+	/** get the number of ceilings between z1 and z2 */
+	int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
+
+		int cnt = 0;
+		const float zMin = std::min(pos1.z, pos2.z);
+		const float zMax = std::max(pos1.z, pos2.z);
+
+#ifdef WITH_ASSERTIONS
+
+		static int numNear = 0;
+		static int numFar = 0;
+		for (const float z : ceilingsAtHeight_m) {
+			const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
+			if (diff < 0.1) {++numNear;} else {++numFar;}
+		}
+		if ((numNear + numFar) > 150000) {
+			Assert::isTrue(numNear < numFar*0.1,
+			    "many requests to the WiFiModel address nodes (very) near to a ground! \
+			    due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
+			    expect very wrong outputs! \
+			    consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
+			);
+		}
+#endif
+
+		for (const float z : ceilingsAtHeight_m) {
+			if (zMin < z && zMax > z) {++cnt;}
+		}
+
+		return cnt;
+
+	}
+
+};
+
+
+#endif // WIFIMODELLOGDISTCEILING_H

sensors/gps/GPSData.h

@@ -2,6 +2,7 @@
 #define GPSDATA_H
 
 #include "../../data/Timestamp.h"
+#include "../../geo/EarthPos.h"
 
 struct GPSData {
 
@@ -15,12 +16,41 @@ struct GPSData {
 	float accuracy;		// m [might be NAN]
 	float speed;		// m/s [might be NAN]
 
+	/** ctor for invalid/unknown data */
 	GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
 
+	/** ctor */
 	GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN)  {;}
 
+	/** ctor */
 	GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN)  {;}
 
+	/** get as EarthPos struct */
+	EarthPos toEarthPos() const {
+		return EarthPos(lat, lon, alt);
+	}
+
+	/** data valid? */
+	bool isValid() const {
+		return (lat == lat) && (lon == lon);
+	}
+
+	bool operator == (const GPSData& o) const {
+		return	EQ_OR_NAN(lat, o.lat) &&
+				EQ_OR_NAN(lon, o.lon) &&
+				EQ_OR_NAN(alt, o.alt) &&
+				EQ_OR_NAN(accuracy, o.accuracy) &&
+				EQ_OR_NAN(speed, o.speed);
+	}
+
+	std::string asString() const {
+		return "(lat: " + std::to_string(lat) + ", lon: " + std::to_string(lon) + ", alt: " + std::to_string(alt) + " acur: " + std::to_string(accuracy) + ")";
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
 };
 
 #endif // GPSDATA_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/imu/AccelerometerData.h

@@ -42,11 +42,25 @@ struct AccelerometerData {
 		return AccelerometerData(x/val, y/val, z/val);
 	}
 
-        std::string asString() const {
-            std::stringstream ss;
-            ss << "(" << x << "," << y << "," << z << ")";
-            return ss.str();
-        }
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const AccelerometerData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
 
 };
 

sensors/imu/CompassData.h

@@ -0,0 +1,56 @@
+#ifndef COMPASSDATA_H
+#define COMPASSDATA_H
+
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from a compass sensor */
+struct CompassData {
+
+	/** azimuth angle. NAN if not available */
+	float azimuth = NAN;
+
+	/** describes the sensor's quality */
+	float quality01 = 0;
+
+
+	/** empty ctor */
+	CompassData() : azimuth(NAN) {;}
+
+	/** data ctor */
+	CompassData(const float azimuth) : azimuth(azimuth), quality01(0) {;}
+
+	/** data ctor */
+	CompassData(const float azimuth, const float quality01) : azimuth(azimuth), quality01(quality01) {;}
+
+	/** get an instance describing invalid data */
+	static CompassData INVALID() {
+		return CompassData(NAN);
+	}
+
+	/** convert to string */
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << azimuth << ")";
+		return ss.str();
+	}
+
+	/** is the compass data valid? [compass present] */
+	bool isValid() const {
+		return azimuth == azimuth;
+	}
+
+	bool operator == (const CompassData& o) const {
+		return	EQ_OR_NAN(azimuth, o.azimuth) &&
+				EQ_OR_NAN(quality01, o.quality01);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
+};
+
+#endif // COMPASSDATA_H

sensors/imu/GravityData.h

@@ -0,0 +1,67 @@
+#ifndef GRAVITYDATA_H
+#define GRAVITYDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct GravityData {
+
+	float x;
+	float y;
+	float z;
+
+    GravityData() : x(0), y(0), z(0) {;}
+
+    GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    GravityData& operator += (const GravityData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    GravityData& operator -= (const GravityData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    GravityData operator - (const GravityData& o) const {
+        return GravityData(x-o.x, y-o.y, z-o.z);
+	}
+
+    GravityData operator / (const float val) const {
+        return GravityData(x/val, y/val, z/val);
+	}
+
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const GravityData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
+};
+
+#endif // GRAVITYDATA_H

sensors/imu/GyroscopeData.h

@@ -23,11 +23,25 @@ struct GyroscopeData {
 		return std::sqrt( x*x + y*y + z*z );
 	}
 
-        std::string asString() const {
-            std::stringstream ss;
-            ss << "(" << x << "," << y << "," << z << ")";
-            return ss.str();
-        }
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const GyroscopeData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
 
 };
 

sensors/imu/LinearAccelerationData.h

@@ -0,0 +1,67 @@
+#ifndef LINEARACCELERATIONDATA_H
+#define LINEARACCELERATIONDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct LinearAccelerationData {
+
+	float x;
+	float y;
+	float z;
+
+    LinearAccelerationData() : x(0), y(0), z(0) {;}
+
+    LinearAccelerationData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    LinearAccelerationData& operator += (const LinearAccelerationData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    LinearAccelerationData& operator -= (const LinearAccelerationData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    LinearAccelerationData operator - (const LinearAccelerationData& o) const {
+        return LinearAccelerationData(x-o.x, y-o.y, z-o.z);
+	}
+
+    LinearAccelerationData operator / (const float val) const {
+        return LinearAccelerationData(x/val, y/val, z/val);
+	}
+
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const LinearAccelerationData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
+};
+
+#endif // LINEARACCELERATIONDATA_H

sensors/imu/MotionDetection.h

@@ -0,0 +1,163 @@
+#ifndef MOTIONDETECTION_H
+#define MOTIONDETECTION_H
+
+#include "GravityData.h"
+#include "LinearAccelerationData.h"
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../misc/Debug.h"
+
+#include <eigen3/Eigen/Dense>
+
+#include <cmath>
+#include <vector>
+
+#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>
+
+
+#include "../../Assertions.h"
+
+class MotionDetection {
+
+private:
+
+    bool newAccelerationMeasurementArrived = false;
+    bool newGravityMeasurementArrived = false;
+
+    Eigen::Vector3f curGravity;
+    Eigen::Vector3f curLinearAcceleration;
+
+    //fast algo
+    Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix
+
+    int numMeasurementsPerInterval, updateCnt;
+    int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms
+
+    struct Motion{
+        Eigen::Vector2f vec = Eigen::Vector2f::Identity();
+        Timestamp lastEstimation;
+    };
+
+    Motion curMotion;
+    Motion prevMotion;
+
+    const char* name = "MotionDetection";
+
+public:
+
+	/** ctor */
+    MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
+        ;
+	}
+
+    void addGravity(const Timestamp& ts, const GravityData& grav){
+
+            curGravity << grav.x, grav.y, grav.z;
+            newGravityMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {
+
+            curLinearAcceleration << acc.x, acc.y, acc.z;
+            newAccelerationMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    /** return the current motion axis
+     * NOTE: if no data is available, this vector is the Identity
+    */
+    Eigen::Vector2f getCurrentMotionAxis(){
+        return curMotion.vec;
+    }
+
+    /** returns the radians between [-pi, pi] between successive motion axis estimations */
+    float getMotionChangeInRad(){
+        //TODO: put this in an EigenHelper Class within geo
+        const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
+        const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));
+
+        //nan?
+        if(std::isnan(ang)){
+            Log::add(name, "The motion change angle is nAn, this is not correct!");
+        }
+
+        if(updateCnt < 2){
+            return 0;
+        }
+
+        return ang;
+    }
+
+private:
+
+    FRIEND_TEST(MotionDetection, motionAngle);
+    FRIEND_TEST(MotionDetection, motionAxis);
+
+    Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){
+
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
+        return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
+    }
+
+    void updateProjectionVectorFast(const Timestamp& ts){
+
+        //make sure we have both measurements for calculation
+        if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){
+
+            numMeasurementsPerInterval++;
+
+            //project acc into perpendicular plane of grav (using standard vector projection)
+            Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;
+
+            //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
+            if(proj.isZero()){
+                proj = curLinearAcceleration;
+                Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
+            }
+
+            //we are only interested in x,y
+            Eigen::Vector2f vec;
+            vec << proj.x(), proj.y();
+
+            // sum this up for later averaging.
+            sumProjectedCov += vec*vec.transpose();
+
+            // start with the first available timestamp
+            if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;}
+
+            //update the motion axis depending on the update interval
+            if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){
+
+                prevMotion = curMotion;
+
+                //calculate the average of the coveriance matrix
+                Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;
+
+                curMotion.vec = updateMotionAxis(Q);
+                curMotion.lastEstimation = ts;
+
+                reset();
+            }
+
+            newGravityMeasurementArrived = false;
+            newAccelerationMeasurementArrived = false;
+        }
+        //do nothing
+    }
+
+    void reset(){
+        numMeasurementsPerInterval = 0;
+        sumProjectedCov = Eigen::Matrix2f::Zero();
+        ++updateCnt;
+    }
+
+};
+
+#endif // MOTIONDETECTION_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/offline/FileReader.h

@@ -0,0 +1,414 @@
+#ifndef FILEREADER_H
+#define FILEREADER_H
+
+#include <fstream>
+#include <Indoor/Exception.h>
+#include <vector>
+#include <unordered_map>
+
+#include "../../math/Interpolator.h"
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include "../../sensors/pressure/BarometerData.h"
+#include "../../sensors/imu/AccelerometerData.h"
+#include "../../sensors/imu/GyroscopeData.h"
+#include "../../sensors/imu/GravityData.h"
+#include "../../sensors/imu/LinearAccelerationData.h"
+#include "../../sensors/beacon/BeaconMeasurements.h"
+#include "../../sensors/gps/GPSData.h"
+#include "../../sensors/imu/CompassData.h"
+
+#include "../../geo/Point2.h"
+#include "../../grid/factory/v2/GridFactory.h"
+#include "../../grid/factory/v2/Importance.h"
+#include "../../floorplan/v2/Floorplan.h"
+#include "../../floorplan/v2/FloorplanHelper.h"
+
+#include "Splitter.h"
+#include "Sensors.h"
+#include "Listener.h"
+
+#warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]"
+
+namespace Offline {
+
+	/**
+	 * read and parse previously recorded ["offline"] files
+	 */
+	class FileReader {
+
+	public:
+
+		using GroundTruth = Interpolator<Timestamp, Point3>;
+
+		/** all entries grouped by sensor */
+		std::vector<TS<int>> groundTruth;
+		std::vector<TS<WiFiMeasurements>> wifi;
+		std::vector<TS<BeaconMeasurement>> beacon;
+		std::vector<TS<AccelerometerData>> acc;
+		std::vector<TS<GyroscopeData>> gyro;
+		std::vector<TS<BarometerData>> barometer;
+		std::vector<TS<LinearAccelerationData>> lin_acc;
+		std::vector<TS<GravityData>> gravity;
+		std::vector<TS<GPSData>> gps;
+		std::vector<TS<CompassData>> compass;
+
+		/** all entries in linear order as they appeared while recording */
+		std::vector<Entry> entries;
+
+		static constexpr char sep = ';';
+
+	public:
+
+		/** empty ctor. call open() */
+		FileReader() {
+			;
+		}
+
+		/** ctor with filename */
+		FileReader(const std::string& file) {
+			open(file);
+		}
+
+		/** open the given file */
+		void open(const std::string& file) {
+			clear();
+			parse(file);
+		}
+
+		/** remove all parsed entries */
+		void clear() {
+			entries.clear();
+			groundTruth.clear();
+			wifi.clear();
+			beacon.clear();
+			acc.clear();
+			gyro.clear();
+			gps.clear();
+			compass.clear();
+			barometer.clear();
+			lin_acc.clear();
+			gravity.clear();
+		}
+
+		const std::vector<Entry>& getEntries() const {return entries;}
+
+
+		const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
+
+		const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
+
+		const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
+
+		const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
+
+		const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
+
+		const std::vector<TS<GPSData>>& getGPS() const {return gps;}
+
+		const std::vector<TS<CompassData>>& getCompass() const {return compass;}
+
+		const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
+
+		const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
+
+		const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
+
+		/** get an interpolateable ground-truth based on the time-clicks during recording */
+		GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
+
+			// sanity check: given path [indices to ground-truth points within the map]
+			// must have the same size as the number of clicks during recording
+			Assert::equal(groundTruthPoints.size(), groundTruth.size(), "mismatch of ground-truth points between given path and recording");
+
+			// allows getting a position on the ground-truth given a timestamp
+			GroundTruth interpol;
+
+			// all ground-truth points within the map
+			static std::unordered_map<int, Point3> gt = FloorplanHelper::getGroundTruthPoints(map);
+
+			// process each "tap smartphone when reaching ground-truth-point"
+			for (const TS<int>& entry : groundTruth) {
+				const Timestamp ts = Timestamp::fromMS(entry.ts);
+				const int idx = entry.data;					// starting at 0, incrementing over time [1st point, 2nd points, 3d points, ...]
+				const int id = groundTruthPoints[idx];		// convert point number to point-id within floorplan
+				const auto& it = gt.find(id);
+				if (it == gt.end()) {throw Exception("missing ground-truth point ID:" + std::to_string(id));}
+				const Point3 pos = it->second;
+				interpol.add(ts, pos);
+			}
+
+			// done
+			return interpol;
+
+		}
+
+	private:
+
+		void parse(const std::string& file) {
+
+			std::ifstream inp(file);
+			if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
+
+			while(!inp.eof() && !inp.bad()) {
+
+				uint64_t ts;
+				char delim;
+				int idx = -1;
+				std::string data;
+
+				inp >> ts;
+				inp >> delim;
+				inp >> idx;
+				inp >> delim;
+				inp >> data;
+
+				if		(idx == (int)Sensor::WIFI)			{parseWiFi(ts, data);}
+				else if	(idx == (int)Sensor::BEACON)		{parseBeacons(ts, data);}
+				else if	(idx == (int)Sensor::GROUND_TRUTH)	{parseGroundTruth(ts, data);}
+				else if	(idx == (int)Sensor::ACC)			{parseAccelerometer(ts, data);}
+				else if	(idx == (int)Sensor::GYRO)			{parseGyroscope(ts, data);}
+				else if (idx == (int)Sensor::BARO)			{parseBarometer(ts, data);}
+				else if (idx == (int)Sensor::LIN_ACC)		{parseLinearAcceleration(ts,data);}
+				else if (idx == (int)Sensor::GRAVITY)		{parseGravity(ts,data);}
+				else if (idx == (int)Sensor::COMPASS)		{parseCompass(ts,data);}
+				else if (idx == (int)Sensor::GPS)			{parseGPS(ts,data);}
+
+				// TODO: this is a hack...
+				// the loop is called one additional time after the last entry
+				// and keeps the entries of entry
+
+			}
+
+			inp.close();
+
+		}
+
+		void parseLinearAcceleration(const uint64_t ts, const std::string& data){
+
+			const auto pos1 = data.find(';');
+			const auto pos2 = data.find(';', pos1+1);
+
+			const std::string x = data.substr(0, pos1);
+			const std::string y = data.substr(pos1+1, pos2-pos1-1);
+			const std::string z = data.substr(pos2+1);
+
+			TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
+			lin_acc.push_back(elem);
+			entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
+
+		}
+
+		void parseGravity(const uint64_t ts, const std::string& data){
+
+			GravityData gravData;
+
+			const auto pos1 = data.find(';');
+			const auto pos2 = data.find(';', pos1+1);
+
+			gravData.x = std::stof(data.substr(0, pos1));
+			gravData.y = std::stof(data.substr(pos1+1, pos2-pos1-1));
+			gravData.z = std::stof(data.substr(pos2+1));
+
+			TS<GravityData> elem(ts, gravData);
+			gravity.push_back(elem);
+			entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);}
+
+		}
+
+		void parseAccelerometer(const uint64_t ts, const std::string& data) {
+
+			const auto pos1 = data.find(';');
+			const auto pos2 = data.find(';', pos1+1);
+
+			const std::string x = data.substr(0, pos1);
+			const std::string y = data.substr(pos1+1, pos2-pos1-1);
+			const std::string z = data.substr(pos2+1);
+
+			const AccelerometerData accData(std::stof(x), std::stof(y), std::stof(z));
+			TS<AccelerometerData> elem(ts, accData);
+			acc.push_back(elem);
+			entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);}
+
+		}
+
+		void parseGyroscope(const uint64_t ts, const std::string& data) {
+
+			const auto pos1 = data.find(';');
+			const auto pos2 = data.find(';', pos1+1);
+
+			const std::string x = data.substr(0, pos1);
+			const std::string y = data.substr(pos1+1, pos2-pos1-1);
+			const std::string z = data.substr(pos2+1);
+
+			const GyroscopeData gyroData(std::stof(x), std::stof(y), std::stof(z));
+			TS<GyroscopeData> elem(ts, gyroData);
+			gyro.push_back(elem);
+			entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);}
+
+		}
+
+		void parseWiFi(const uint64_t ts, const std::string& data) {
+
+			WiFiMeasurements wifi;
+			Splitter s(data, sep);
+
+			// the -1 is due to some old files containing a trailing ";" resulting in one additional stray column
+			for (size_t i = 0; i < s.size()-1; i += 3) {
+
+				const std::string mac = s.get(i+0);
+				const float freq = s.getFloat(i+1);
+				const float rssi = s.getFloat(i+2);
+
+				// append AP to current scan-entry
+				WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts));
+				wifi.entries.push_back(e);
+
+			}
+
+			// add new wifi reading
+			this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
+			entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);}
+
+		}
+
+		void parseBeacons(const uint64_t ts, const std::string& data) {
+
+			const auto pos1 = data.find(';');
+			const auto pos2 = data.find(';', pos1+1);
+			const auto pos3 = data.find(';', pos2+1);
+
+			const std::string mac = data.substr(0, pos1);
+			const std::string rssi = data.substr(pos1+1, pos2);
+			const std::string txp = data.substr(pos2+1, pos3);
+
+			//yes the timestamp is redundant here, but in case of multiusage...
+			TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
+			beacon.push_back(e);
+			entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
+
+		}
+
+		void parseGroundTruth(const uint64_t ts, const std::string& data) {
+
+			const auto pos1 = data.find(';');
+			std::string gtIndex = data.substr(0, pos1);
+
+			TS<int> elem(ts, std::stoi(gtIndex));
+			groundTruth.push_back(elem);
+
+		}
+
+		void parseBarometer(const uint64_t ts, const std::string& data) {
+
+			BarometerData baro;
+			Splitter s(data, sep);
+
+			baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);
+
+			TS<BarometerData> elem(ts, baro);
+			barometer.push_back(elem);
+			entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);}
+
+		}
+
+		void parseCompass(const uint64_t ts, const std::string& data) {
+
+			CompassData compass;
+			Splitter s(data, sep);
+
+			compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
+			compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
+
+			TS<CompassData> elem(ts, compass);
+			this->compass.push_back(elem);
+			entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
+
+		}
+
+		/** parse the given GPS entry */
+		void parseGPS(const uint64_t ts, const std::string& data) {
+
+			GPSData gps;
+			Splitter s(data, sep);
+
+			gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
+			gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
+			gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
+			gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
+			gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
+
+			TS<GPSData> elem(ts, gps);
+			this->gps.push_back(elem);
+			entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);}
+
+		}
+
+
+	public:
+
+		const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {
+
+			// finde alle positionen der waypoints im gtPath aus map
+			std::unordered_map<int, Point3> waypointsMap;
+			for(Floorplan::Floor* f : map->floors){
+				float h = f->atHeight;
+				for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){
+
+					//wenn die gleiche id 2x vergeben wurde, knallt es
+					if(waypointsMap.find(gtp->id) == waypointsMap.end()){
+						waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)});
+					}
+					else{
+						throw std::string("the floorplan's ground truth contains two points with identical id's!");
+					}
+
+				}
+			}
+
+			// bringe diese in richtige reihenfolge und füge timestamp hinzu
+			Interpolator<uint64_t, Point3> interpol;
+
+			int it = 0;
+			for(int id : gtPath){
+				auto itMap = waypointsMap.find(id);
+				if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}
+
+				//the time, when the gt button was clicked on the app
+				uint64_t tsGT = groundTruth[it++].ts;
+				interpol.add(tsGT, itMap->second);
+
+			}
+
+			if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){
+				throw std::string("No Ground Truth points found within the map.xml file");
+			}
+
+			return interpol;
+		}
+
+	};
+
+}
+
+#endif // FILEREADER_H

sensors/offline/FileWriter.h

@@ -0,0 +1,92 @@
+#ifndef FILEWRITER_H
+#define FILEWRITER_H
+
+#include "../gps/GPSData.h"
+#include "../imu/CompassData.h"
+#include "../imu/LinearAccelerationData.h"
+#include "../imu/GravityData.h"
+#include "../pressure/BarometerData.h"
+#include "../imu/GyroscopeData.h"
+#include "../imu/AccelerometerData.h"
+#include "../radio/WiFiMeasurements.h"
+#include "Sensors.h"
+
+#include <fstream>
+
+namespace Offline {
+
+	class FileWriter {
+
+	private:
+
+		std::ofstream out;
+		static constexpr char sep = ';';
+		const std::string nl = "\n";
+
+	public:
+
+		FileWriter() {
+			;
+		}
+
+		~FileWriter() {
+			close();
+		}
+
+		void open(const std::string& file) {
+			out.open(file);
+			if (!out) {throw Exception("error opening file: " + file);}
+		}
+
+		void close() {
+			out.flush();
+			out.close();
+		}
+
+		void flush() {
+			out.flush();
+		}
+
+		void add(const Timestamp ts, const AccelerometerData& data) {
+			out << ts.ms() << sep << (int) Sensor::ACC << sep << data.x << sep << data.y << sep << data.z << nl;
+		}
+
+		void add(const Timestamp ts, const LinearAccelerationData& data) {
+			out << ts.ms() << sep << (int) Sensor::LIN_ACC << sep << data.x << sep << data.y << sep << data.z << nl;
+		}
+
+		void add(const Timestamp ts, const GravityData& data) {
+			out << ts.ms() << sep << (int) Sensor::GRAVITY << sep << data.x << sep << data.y << sep << data.z << nl;
+		}
+
+		void add(const Timestamp ts, const GyroscopeData& data) {
+			out << ts.ms() << sep  << (int) Sensor::GYRO << sep << data.x << sep << data.y << sep << data.z << nl;
+		}
+
+		void add(const Timestamp ts, const BarometerData& data) {
+			out << ts.ms() << sep  << (int) Sensor::BARO << sep << data.hPa << nl;
+		}
+
+		void add(const Timestamp ts, const GPSData& data) {
+			out << ts.ms() << sep  << (int) Sensor::GPS << sep << data.lat << sep << data.lon << sep << data.alt << sep << data.accuracy << sep << data.speed << nl;
+		}
+
+		void add(const Timestamp ts, const CompassData& data) {
+			out << ts.ms() << sep  << (int) Sensor::COMPASS << sep << data.azimuth << sep << data.quality01 << nl;
+		}
+
+		void add(const Timestamp ts, const WiFiMeasurements& data) {
+			out << ts.ms() << sep << (int) Sensor::WIFI;
+			for (const WiFiMeasurement& m : data.entries) {
+				out << sep << m.getAP().getMAC().asString();
+				out << sep << m.getFrequency();
+				out << sep << m.getRSSI();
+			}
+			out << "\n";
+		}
+
+	};
+
+}
+
+#endif // FILEWRITER_H

sensors/offline/Listener.h

@@ -0,0 +1,33 @@
+#ifndef OFFLINE_LISTENER_H
+#define OFFLINE_LISTENER_H
+
+#include "../gps/GPSData.h"
+#include "../imu/CompassData.h"
+#include "../imu/GravityData.h"
+#include "../pressure/BarometerData.h"
+#include "../imu/GyroscopeData.h"
+#include "../imu/AccelerometerData.h"
+#include "../radio/WiFiMeasurements.h"
+
+namespace Offline {
+
+	/**
+	 * listen for events/callbacks while parsing offline files
+	 */
+	class Listener {
+
+	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 GravityData data) = 0;
+		virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
+		virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
+		virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
+		virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
+
+	};
+
+}
+
+#endif // OFFLINE_LISTENER_H

sensors/offline/OfflineAndroid.h

@@ -12,8 +12,14 @@
 #include "../radio/WiFiMeasurements.h"
 #include "../imu/AccelerometerData.h"
 #include "../imu/GyroscopeData.h"
+#include "../imu/CompassData.h"
+#include "../gps/GPSData.h"
 #include "../pressure/BarometerData.h"
 
+#include "Splitter.h"
+#include "Listener.h"
+#include "Sensors.h"
+
 template <typename SensorData> struct OfflineEntry {
 
 	Timestamp ts;
@@ -35,20 +41,12 @@ 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;
-	virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
-};
-
-/** read recorded android sensor data files */
+/**
+ * read sensor data files that were recorded using
+ * the old java android app
+ */
 class OfflineAndroid {
 
-
 private:
 
 	std::vector<OfflineEntry<WiFiMeasurements>> wifi;
@@ -56,12 +54,16 @@ private:
 	std::vector<OfflineEntry<GyroscopeData>> gyro;
 
 	std::vector<OfflineEntry<AccelerometerData>> accel;
-	std::vector<OfflineEntry<AccelerometerData>> gravity;
+	std::vector<OfflineEntry<GravityData>> gravity;
+	std::vector<OfflineEntry<CompassData>> compass;
 
 	std::vector<OfflineEntry<BarometerData>> barometer;
 
+	std::vector<OfflineEntry<GPSData>> gps;
+
 	WalkedPath walkedPath;
 
+	static constexpr char sep = ';';
 	const char* name = "OfflineData";
 
 public:
@@ -84,11 +86,17 @@ public:
 	const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}
 
 	/** get all gravity readings */
-	const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}
+	const std::vector<OfflineEntry<GravityData>>& getGravity() const {return gravity;}
 
 	/** get all barometer readings */
 	const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
 
+	/** get all compass readings */
+	const std::vector<OfflineEntry<CompassData>>& getCompass() const {return compass;}
+
+	/** get all gps readings */
+	const std::vector<OfflineEntry<GPSData>>& getGPS() const {return gps;}
+
 
 	/** get the walked path */
 	const WalkedPath& getWalkedPath() const {return walkedPath;}
@@ -105,7 +113,7 @@ public:
 
 public:
 
-	void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) {
+	void parse(const std::string& file, Offline::Listener* listener = nullptr) {
 
 		Log::add(name, "parsing data file: " + file , false);
 		Log::tick();
@@ -152,47 +160,61 @@ public:
 private:
 
 	/** parse the given data */
-	void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) {
+	void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, Offline::Listener* listener) {
 
 		// how to parse
 		switch(sensorID) {
 
-			case 0: {
+			case (int) Offline::Sensor::ACC: {
 				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));
+			case (int) Offline::Sensor::GRAVITY: {
+				const GravityData data = parseGravity(sensorData);
+				gravity.push_back(OfflineEntry<GravityData>(ts, data));
 				if (listener) {listener->onGravity(ts, data);}
 				break;
 			}
 
-			case 3: {
+			case (int) Offline::Sensor::GYRO: {
 				const GyroscopeData data = parseGyroscope(sensorData);
 				gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
 				if (listener) {listener->onGyroscope(ts, data);}
 				break;
 			}
 
-		    case 5: {
+			case (int) Offline::Sensor::BARO: {
 			    const BarometerData data = parseBarometer(sensorData);
 				barometer.push_back(OfflineEntry<BarometerData>(ts, data));
 				if (listener) {listener->onBarometer(ts, data);}
 				break;
 		    }
 
-			case 8: {
+			case (int) Offline::Sensor::WIFI: {
 			    const WiFiMeasurements data = parseWiFi(ts, sensorData);
 				wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
 				if (listener) {listener->onWiFi(ts, data);}
 				break;
 			}
 
-			case 99: {
+			case (int) Offline::Sensor::COMPASS: {
+				const CompassData data = parseCompass(sensorData);
+				compass.push_back(OfflineEntry<CompassData>(ts, data));
+				if (listener) {listener->onCompass(ts, data);}
+				break;
+		    }
+
+			case (int) Offline::Sensor::GPS: {
+				const GPSData data = parseGPS(sensorData);
+				gps.push_back(OfflineEntry<GPSData>(ts, data));
+				if (listener) {listener->onGPS(ts, data);}
+				break;
+		    }
+
+			case (int) Offline::Sensor::GROUND_TRUTH: {
 				const GroundTruthID data = parseGroundTruthTick(sensorData);
 				groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data));
 				// TODO listener
@@ -279,6 +301,24 @@ private:
 
 	}
 
+	static inline GravityData parseGravity(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 GravityData(std::stof(sx), std::stof(sy), std::stof(sz));
+
+	}
+
 	/** parse the given Barometer entry */
 	static inline BarometerData parseBarometer(const std::string& data) {
 
@@ -326,6 +366,35 @@ private:
 
 	}
 
+	/** parse the given Compass entry */
+	static inline CompassData parseCompass(const std::string& data) {
+
+		CompassData compass;
+		Splitter s(data, sep);
+
+		compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
+		compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
+
+		return compass;
+
+	}
+
+	/** parse the given GPS entry */
+	static inline GPSData parseGPS(const std::string& data) {
+
+		GPSData gps;
+		Splitter s(data, sep);
+
+		gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
+		gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
+		gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
+		gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
+		gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
+
+		return gps;
+
+	}
+
 };
 
 #endif // OFFLINEANDROID_H

sensors/offline/Sensors.h

@@ -0,0 +1,38 @@
+#ifndef OFFLINE_SENSORS_H
+#define OFFLINE_SENSORS_H
+
+namespace Offline {
+
+	enum class Sensor {
+		ACC = 0,
+		GRAVITY = 1,
+		LIN_ACC = 2,
+		GYRO = 3,
+		BARO = 5,
+		COMPASS = 6,		// also called "orientatioN"
+		WIFI = 8,
+		BEACON = 9,
+		GPS = 16,
+		GROUND_TRUTH = 99,
+		POS = 1001,			// IPIN2016
+	};
+
+	template <typename T> struct TS {
+		const uint64_t ts;
+		T data;
+		TS(const uint64_t ts) : ts(ts) {;}
+		TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
+	};
+
+	/** entry for one sensor */
+	struct Entry {
+		Sensor type;
+		uint64_t ts;
+		int idx;
+		Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
+	};
+
+
+}
+
+#endif // OFFLINE_SENSORS_H

sensors/offline/Splitter.h

@@ -0,0 +1,53 @@
+#ifndef DATA_SPLITTER_H
+#define DATA_SPLITTER_H
+
+#include <string>
+#include <vector>
+
+/**
+ * split an input-file into various tokens
+ */
+class Splitter {
+
+	std::string str;
+	char sep = ';';
+	std::vector<std::string> split;
+
+public:
+
+	/** ctor */
+	Splitter(const std::string& str, const char sep = ';') : str(str), sep(sep) {
+		build();
+	}
+
+	bool has(const int idx) const {return split.size() > idx;}
+
+	const std::string& get(const int idx) const {return split.at(idx);}
+
+	const float getFloat(const int idx) const {return std::stof(get(idx));}
+
+	size_t size() const {return split.size();}
+
+private:
+
+	void build() {
+
+		std::string cur;
+
+		for (char c : str) {
+			if (c == sep) {
+				split.push_back(cur);
+				cur = "";
+			} else {
+				cur += c;
+			}
+		}
+
+		split.push_back(cur);
+
+	}
+
+};
+
+
+#endif // DATA_SPLITTER_H

sensors/pressure/ActivityButterPressure.h

@@ -18,8 +18,6 @@ public:
 
     enum Activity {DOWN, STAY, UP};
 
-
-
     struct History {
         Timestamp ts;
         BarometerData data;
@@ -27,23 +25,25 @@ public:
     };
 
     private:
-    //just for debugging and plotting
-    std::vector<History> input;
-    std::vector<History> inputInterp;
-    std::vector<History> output;
-    std::vector<float> sumHist;
-    std::vector<float> mvAvgHist;
-    std::vector<History> actHist;
 
+    std::vector<History> output;
     Activity currentActivity;
     MovingAVG<float> mvAvg = MovingAVG<float>(20);
 
-    /** change this values for much success */
+    /** change this values for much success
+    *
+    *   Nexus 6:
+    *   butter =  Filter::ButterworthLP<float>(10,0.1f,2);
+    *   threshold = 0.025;
+    *   diffSize = 20;
+    *   FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
+    */
     const bool additionalLowpassFilter = false;
-	const int diffSize = 20; //the number values used for finding the activity.
-    const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
-	Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.1f,2);
-    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.1f,2);
+    const unsigned long diffSize = 20; //the number values used for finding the activity.
+    const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
+    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
+    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.05f,2);
+
     FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
 
 public:
@@ -68,14 +68,11 @@ public:
             return STAY;
         }
 
-        input.push_back(History(ts, baro));
-
         bool newInterpolatedValues = false;
 
         //interpolate & butter
         auto callback = [&] (const Timestamp ts, const float val) {
             float interpValue = val;
-            inputInterp.push_back(History(ts, BarometerData(interpValue)));
 
             //butter
             float butterValue = butter.process(interpValue);
@@ -89,10 +86,10 @@ public:
         if(newInterpolatedValues == true){
 
             //getActivity
-			if((int)output.size() > diffSize){
+            if(output.size() > diffSize){
                 //diff
                 std::vector<float> diff;
-                for(int i = output.size() - diffSize; i < output.size() - 1; ++i){
+                for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
 
                     float diffVal = output[i+1].data.hPa - output[i].data.hPa;
 
@@ -113,7 +110,6 @@ public:
                 }else{
                     actValue = sum;
                 }
-                sumHist.push_back(actValue);
 
                 if(actValue > threshold){
                     currentActivity = DOWN;
@@ -127,8 +123,6 @@ public:
             }
         }
 
-        actHist.push_back(History(ts, BarometerData(currentActivity)));
-
         return currentActivity;
 
 	}
@@ -137,49 +131,6 @@ public:
     Activity getCurrentActivity() {
         return currentActivity;
 	}
-
-    std::vector<float> getSensorHistory(){
-        std::vector<float> tmp;
-
-        for(History val : input){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getInterpolatedHistory(){
-        std::vector<float> tmp;
-
-        for(History val : inputInterp){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getOutputHistory(){
-
-        std::vector<float> tmp;
-
-        for(History val : output){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getSumHistory(){
-        return sumHist;
-    }
-
-
-    std::vector<History> getActHistory(){
-
-        return actHist;
-    }
-
-
 };
 
 #endif // ACTIVITYBUTTERPRESSURE_H

sensors/pressure/ActivityButterPressurePercent.h

@@ -0,0 +1,187 @@
+#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H
+#define ACTIVITYBUTTERPRESSUREPERCENT_H
+
+#include "../../data/Timestamp.h"
+#include "../../math/filter/Butterworth.h"
+#include "../../math/FixedFrequencyInterpolator.h"
+#include "../../math/distribution/Normal.h"
+#include <KLib/Assertions.h>
+#include <vector>
+
+
+#include "BarometerData.h"
+
+/**
+ * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
+ * activity recognition based on a small window given by matlabs diff(window)
+ *
+ * todo: if an elevator is detected, first we have a short time the stairs are more prober.
+ */
+class ActivityButterPressurePercent {
+
+public:
+
+    struct ActivityProbabilities{
+        float elevatorDown;
+        float stairsDown;
+        float stay;
+        float stairsUp;
+        float elevatorUp;
+
+        ActivityProbabilities(float elevatorDown, float stairsDown,
+                              float stay, float stairsUp, float elevatorUp) :
+            elevatorDown(elevatorDown), stairsDown(stairsDown),
+            stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;}
+
+        ActivityProbabilities() :
+            elevatorDown(0.01f), stairsDown(0.01f),
+            stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;}
+    };
+
+
+    struct History {
+        Timestamp ts;
+        BarometerData data;
+        History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;}
+    };
+
+private:
+
+    std::vector<History> output;
+    bool initialize;
+
+    ActivityProbabilities currentActivity;
+
+    /** change this values for much success */
+    const unsigned long diffSize = 20; //the number values used for finding the activity.
+    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
+    FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
+
+    const float variance = 0.02f;
+
+    const float muStairs = 0.04f;
+    const float muStay = 0.00f;
+    const float muEleveator = 0.08f;
+
+    std::vector<float> densities = std::vector<float>(5, 1);
+    std::vector<float> densitiesOld = std::vector<float>(5, 1);
+
+public:
+
+
+	/** ctor */
+    ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){
+        initialize = true;
+	}
+
+
+	/** add new sensor readings that were received at the given timestamp */
+    ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) {
+
+        //init
+        if(initialize){
+            butter.stepInitialization(baro.hPa);
+            initialize = false;
+
+            return currentActivity;
+        }
+
+        bool newInterpolatedValues = false;
+
+        //interpolate & butter
+        auto callback = [&] (const Timestamp ts, const float val) {
+            float interpValue = val;
+
+            //butter
+            float butterValue = butter.process(interpValue);
+            output.push_back(History(ts, BarometerData(butterValue)));
+
+            newInterpolatedValues = true;
+
+        };
+        ffi.add(ts, baro.hPa, callback);
+
+        if(newInterpolatedValues == true){
+
+            //getActivity
+            if(output.size() > diffSize){
+                //diff
+                std::vector<float> diff;
+                for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
+
+                    float diffVal = output[i+1].data.hPa - output[i].data.hPa;
+
+                    diff.push_back(diffVal);
+                }
+
+                float sum = 0;
+                for(float val : diff){
+                    sum += val;
+                }
+
+                float actValue = sum;
+
+                //calculate the probabilites of walking down/up etc...
+                densitiesOld = densities;
+
+                //in one building there is an ultra fast elevator, therefore we need to clip the activity value...
+                if(actValue > muEleveator){
+                    actValue = muEleveator;
+                }
+                if(actValue < -muEleveator){
+                    actValue = -muEleveator;
+                }
+
+                float densityElevatorDown = Distribution::Normal<float>::getProbability(muEleveator, variance, actValue);
+                float densityStairsDown = Distribution::Normal<float>::getProbability(muStairs, variance, actValue);
+                float densityStay = Distribution::Normal<float>::getProbability(muStay, variance, actValue);
+                float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
+                float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);
+
+                _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
+                _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
+                _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
+                _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
+                _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
+
+                _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
+                _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
+                _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
+                _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
+                _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");
+
+                //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
+                densities[0] = densityElevatorDown;
+                densities[1] = densityStairsDown;
+                densities[2] = densityStay;
+                densities[3] = densityStairsUp;
+                densities[4] = densityElevatorUp;
+
+                //normalize
+                float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4];
+
+                for(unsigned long i = 0; i < densities.size(); ++i){
+                    densities[i] /= densitySum;
+
+                    //values cant be zero!
+                    densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f);
+                }
+
+                currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]);
+
+            }
+        }
+
+        //retruns for every call, indepedent of callback.
+        return currentActivity;
+	}
+
+    /** get the current Activity */
+    ActivityProbabilities getCurrentActivity() {
+        return currentActivity;
+	}
+
+
+};
+
+#endif // ACTIVITYBUTTERPRESSUREPERCENT_H

sensors/pressure/BarometerData.h

@@ -13,6 +13,19 @@ struct BarometerData {
 
 	explicit BarometerData(const float hPa) : hPa(hPa) {;}
 
+	/** valid data? */
+	bool isValid() const {
+		return hPa == hPa;
+	}
+
+	bool operator == (const BarometerData& o ) const {
+		return EQ_OR_NAN(hPa, o.hPa);
+	}
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
 };
 
 #endif // BAROMETERDATA_H

sensors/pressure/PressureTendence.h

@@ -51,9 +51,9 @@ public:
 		K::GnuplotPlotElementLines tendence;
 
 		Debug() {
-			plot.add(&raw);			raw.setColorHex("#999999");
-			plot.add(&avg);			avg.setColorHex("#000000");
-			plot.add(&tendence);	tendence.setLineWidth(2);
+			plot.add(&raw);			raw.getStroke().getColor().setHexStr("#999999");
+			plot.add(&avg);			avg.getStroke().getColor().setHexStr("#000000");
+			plot.add(&tendence);	tendence.getStroke().setWidth(2);
 			tendence.setCustomAttr(" axes x1y2 ");
 			gp << "set y2tics\n";
 			gp << "set y2range[-0.3:+0.3]\n";

sensors/radio/AccessPoint.h

@@ -45,6 +45,11 @@ public:
 		;
 	}
 
+	/** equals? */
+	bool operator == (const AccessPoint& o) {
+		return (o.mac == mac) && (o.ssid == ssid);
+	}
+
 public:
 
 	/** get the AP's MAC address */

sensors/radio/LocatedAccessPoint.h

@@ -1,27 +0,0 @@
-#ifndef LOCATEDACCESSPOINT_H
-#define LOCATEDACCESSPOINT_H
-
-#include "AccessPoint.h"
-#include "../../geo/Point3.h"
-#include "../../floorplan/v2/Floorplan.h"
-
-/**
- * describes an access-point including its position (in meter)
- */
-class LocatedAccessPoint : public AccessPoint, public Point3 {
-
-public:
-
-	/** ctor */
-	LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) {
-		;
-	}
-
-	/** ctor */
-	LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) {
-		;
-	}
-
-};
-
-#endif // LOCATEDACCESSPOINT_H

sensors/radio/VAPGrouper.h

@@ -7,6 +7,8 @@
 
 #include "../../math/Stats.h"
 
+#include "../../misc/Debug.h"
+
 #include "WiFiMeasurements.h"
 
 class VAPGrouper {
@@ -40,24 +42,35 @@ public:
 
 private:
 
+	static constexpr const char* name = "VAPGrp";
+
 	/** the mode to use for grouping VAPs */
 	const Mode mode;
 
 	/** the signal-strength aggregation algorithm to use */
 	const Aggregation agg;
 
+	/** respect only outputs with at-least X occurences of one physical hardware [can be used to prevent issues] */
+	int minOccurences;
+
 public:
 
 	/** ctor */
-	VAPGrouper(const Mode mode, const Aggregation agg) : mode(mode), agg(agg) {
-
+	VAPGrouper(const Mode mode, const Aggregation agg, const int minOccurences = 2) :
+		mode(mode), agg(agg), minOccurences(minOccurences) {
+		;
 	}
 
+	/** set the number of needed occurences per VAP-group to be accepted */
+	void setMinOccurences(const int min) {
+		this->minOccurences = min;
+	}
 
 	/** get a vap-grouped version of the given input */
 	WiFiMeasurements group(const WiFiMeasurements& original) const {
 
 		// first, group all VAPs into a vector [one vector per VAP-group]
+		// by using the modified MAC address that all VAPs have in common
 		std::unordered_map<MACAddress, std::vector<WiFiMeasurement>> grouped;
 
 		for (const WiFiMeasurement& m : original.entries) {
@@ -68,8 +81,9 @@ public:
 
 		}
 
-		// output
+		// to-be-constructed output
 		WiFiMeasurements result;
+		int skipped = 0;
 
 		// perform aggregation on each VAP-group
 		for (auto it : grouped) {
@@ -77,6 +91,9 @@ public:
 			const MACAddress& base = it.first;
 			const std::vector<WiFiMeasurement>& vaps = it.second;
 
+			// remove entries that do not satify the min-occurences metric
+			if ((int)vaps.size() < minOccurences) {++skipped; continue;}
+
 			// group all VAPs into one measurement
 			const WiFiMeasurement groupedMeasurement = groupVAPs(base, vaps);
 
@@ -85,6 +102,13 @@ public:
 
 		}
 
+		// debug
+		Log::add(name,
+				 "grouped " + std::to_string(original.entries.size()) + " measurements " +
+				 "into " + std::to_string(result.entries.size()) + " [omitted: " + std::to_string(skipped) + "]",
+				true
+		);
+
 		// done
 		return result;
 

sensors/radio/WiFiMeasurement.h

@@ -9,7 +9,7 @@
  */
 class WiFiMeasurement {
 
-public:
+private:
 
 	friend class VAPGrouper;
 
@@ -19,32 +19,55 @@ public:
 	/** the measured signal strength */
 	float rssi;
 
+    /** OPTIONAL. frequence the signal was received */
+	float freq = NAN;
+
 	/** OPTIONAL. timestamp the measurement was recorded at */
 	Timestamp ts;
 
 public:
 
 	/** ctor */
-	WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {
+	WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi), freq(NAN) {
 		;
 	}
 
-	/** ctor with timestamp */
-	WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
-		;
-	}
+    /** ctor with freq*/
+    WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) {
+            ;
+    }
+
+    /** ctor with timestamp */
+    WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
+            ;
+    }
+
+    /** ctor with timestamp and freq*/
+    WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) {
+            ;
+    }
 
 public:
 
-	/** get the AP we got the measurement for */
-	const AccessPoint& getAP() const {return ap;}
+    /** get the AP we got the measurement for */
+    const AccessPoint& getAP() const {return ap;}
 
-	/** get the measurement's signal strength */
-	float getRSSI() const {return rssi;}
+    /** get the measurement's signal strength */
+    float getRSSI() const {return rssi;}
 
-	/** OPTIONAL: get the measurement's timestamp (if known!) */
-	const Timestamp& getTimestamp() const {return ts;}
+    /** OPTIONAL: get the measurement's timestamp (if known!) */
+    const Timestamp& getTimestamp() const {return ts;}
 
+    /** OPTIONAL: get the measurement's frequence (if known!) */
+    float getFrequency() const {return freq;}
+
+    /** set another signal strength */
+    void setRssi(float value){rssi = value;}
+
+    /** set the timestamp */
+    void setTimestamp(const Timestamp& val){ts = val;}
 };
 
+
+
 #endif // WIFIMEASUREMENT_H

sensors/radio/WiFiMeasurements.h

@@ -22,6 +22,56 @@ struct WiFiMeasurements {
 		return res;
 	}
 
+	/** get the measurements for the given MAC [if available] otherwise null */
+	const WiFiMeasurement* getForMac(const MACAddress& mac) const {
+		for (const WiFiMeasurement& m : entries) {
+			if (m.getAP().getMAC() == mac) {
+				return &m;
+			}
+		}
+		return nullptr;
+	}
+
+	/** remove the entry for the given MAC (if any) */
+	void remove(const MACAddress& mac) {
+		for (size_t i = 0; i < entries.size(); ++i) {
+			if (entries[i].getAP().getMAC() == mac) {
+				entries.erase(entries.begin() + i);
+				break;
+			}
+		}
+	}
+
+
+	/** create a combination */
+	static WiFiMeasurements mix(const WiFiMeasurements& a, const WiFiMeasurements& b, float sec = 3) {
+
+		Timestamp max;
+		WiFiMeasurements res;
+
+		for (const WiFiMeasurement& m : a.entries) {
+			res.entries.push_back(m);
+			if (m.getTimestamp() > max) {max = m.getTimestamp();}
+		}
+
+		for (const WiFiMeasurement& m : b.entries) {
+			res.entries.push_back(m);
+			if (m.getTimestamp() > max) {max = m.getTimestamp();}
+		}
+
+		std::vector<WiFiMeasurement> tmp;
+		std::swap(res.entries, tmp);
+
+		for (const WiFiMeasurement& m : tmp) {
+			if ((max - m.getTimestamp()).sec() < sec) {
+				res.entries.push_back(m);
+			}
+		}
+
+		return res;
+
+	}
+
 };
 
 #endif // WIFIMEASUREMENTS_H

sensors/radio/WiFiProbabilityFree.h

@@ -5,6 +5,7 @@
 #include "model/WiFiModel.h"
 #include "../../math/Distributions.h"
 #include "VAPGrouper.h"
+#include "../../floorplan/v2/Floorplan.h"
 
 #include <unordered_map>
 
@@ -16,7 +17,7 @@ class WiFiObserverFree : public WiFiProbability {
 
 private:
 
-	const float sigma = 8.0f;
+	const float sigma;
 
 	const float sigmaPerSecond = 3.0f;
 
@@ -26,15 +27,25 @@ private:
 	/** the map's floorplan */
 	Floorplan::IndoorMap* map;
 
+	std::vector<AccessPoint> allAPs;
+
+	bool useError = false;
+
 public:
 
 	WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) {
+		allAPs = model.getAllAPs();
+	}
 
+	void setUseError(const bool useError) {
+		this->useError = useError;
 	}
 
 	double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
 
 		double prob = 1.0;
+		//double prob = 0;
+
 		int numMatchingAPs = 0;
 
 		// process each measured AP
@@ -49,7 +60,6 @@ public:
 			// NaN? -> AP not known to the model -> skip
 			if (modelRSSI != modelRSSI) {continue;}
 
-
 			// the scan's RSSI
 			const float scanRSSI = entry.getRSSI();
 
@@ -57,28 +67,85 @@ public:
 			const Timestamp age = curTime - entry.getTimestamp();
 
 			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
-			Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
+			Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?");
+
+			Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range");
+			//Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range");
 
 			// sigma grows with measurement age
 			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
+			// probability for this AP
+			double local = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			//double local = Distribution::Exponential<double>::getProbability(0.1, std::abs(modelRSSI-scanRSSI));
+
+			// also add the error value? [location is OK but model is wrong]
+			if (useError) {
+				local = 0.95 * local + 0.05 * (1.0-local);
+				//local = 0.95 * local + 0.05;
+			}
+
 			// update probability
-			prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
-			//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			prob *= local;
 
 			++numMatchingAPs;
 
 		}
 
 		// sanity check
-		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
+		//Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
 
 		return prob;
 
 	}
 
+	/**
+	 * for some locations it might make sense to also look at APs
+	 * that have NOT been discovered by the smartphone but SHOULD
+	 * be very well receivable at a location.
+	 * such locations can be opted-out by using this veto-probability
+	 */
+	double getVeto(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
+
+		(void) curTime;
+
+		struct APR {
+			AccessPoint ap;
+			float rssi;
+			APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;}
+		};
+
+		std::vector<APR> all;
+		for (const AccessPoint& ap : allAPs) {
+			const float rssi = model.getRSSI(ap.getMAC(), pos_m);
+			if (rssi != rssi) {continue;}	// unknown to the model
+			all.push_back(APR(ap, rssi));
+		}
+
+		// stort by RSSI
+		auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;};
+		std::sort(all.begin(), all.end(), comp);
+
+		int numVetos = 0;
+
+		for (int i = 0; i < 3; ++i) {
+			const APR& apr = all[i];
+			const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
+			const float rssiScan = (mes) ? (mes->getRSSI()) : (-100);
+			const float rssiModel = apr.rssi;
+			const float diff = std::abs(rssiScan - rssiModel);
+			if (diff > 20) {++numVetos;}
+		}
+
+		if (numVetos == 0)	{return 0.70;}
+		if (numVetos == 1)	{return 0.29;}
+		else				{return 0.01;}
+
+	}
+
 	template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {
-		throw Exception("todo??");
+
+        return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs);
 	}
 
 };

sensors/radio/WiFiProbabilityGrid.h

@@ -59,7 +59,7 @@ public:
 		// after some runtime, check whether the wifi timestamps make sense
 		// those must not be zero, otherwise something is wrong!
 		if (!obs.entries.empty()) {
-			Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
+            Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().getTimestamp().isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
 		}
 
 		// process each observed measurement
@@ -73,7 +73,7 @@ public:
 			const Timestamp age = curTime - measurement.getTimestamp();
 
 			// sigma grows with measurement age
-			float sigma = this->sigma + this->sigmaPerSecond * age.sec();
+            float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
 			// the RSSI from the scan
 			const float measuredRSSI = measurement.getRSSI();
@@ -102,8 +102,7 @@ public:
 		}
 
 		// sanity check
-//		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
-//		if (numMatchingAPs == 0) {return 0;}
+        //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
 
 		// as not every node has the same number of visible/matching APs
 		// we MUST return something like the average probability

sensors/radio/WiFiQualityAnalyzer.h

@@ -0,0 +1,106 @@
+#ifndef WIFIQUALITYANALYZER_H
+#define WIFIQUALITYANALYZER_H
+
+#include "WiFiMeasurements.h"
+#include <unordered_set>
+
+class WiFiQualityAnalyzer {
+
+private:
+
+	int historySize = 3;
+	std::vector<WiFiMeasurements> history;
+	float quality = 0;
+
+public:
+
+	/** attach the current measurement and infer the quality */
+	void add(const WiFiMeasurements& mes) {
+
+		// update the history
+		history.push_back(mes);
+		while(history.size() > historySize) {
+			history.erase(history.begin());
+		}
+
+		// recalculate
+		update();
+
+	}
+
+	/** get a quality score for the WiFi */
+	float getQuality() const {
+		return quality;
+	}
+
+private:
+
+	/** re-calculate the current quality */
+	void update() {
+
+		const float qCnt = getQualityByCount();
+		const float qAvgdB = getQualityByAvgDB();
+
+		quality = qAvgdB;
+
+	}
+
+	/** score [0:1] based on the average sig-strength. the higher the better */
+	float getQualityByAvgDB() const {
+
+		// stats
+		float sum = 0;
+		float sum2 = 0;
+		float cnt = 0;
+
+		for (const WiFiMeasurements& mes : history) {
+			for (const WiFiMeasurement& m : mes.entries) {
+				const float rssi = m.getRSSI();
+				sum += rssi;
+				sum2 += rssi*rssi;
+				++cnt;
+			}
+		}
+
+		// average sig strength
+		const float avg = sum/cnt;
+		const float avg2 = sum2/cnt;
+		const float stdDev = std::sqrt(avg2 - avg*avg);
+
+		// avg rssi score
+		const float minRSSI = -85;
+		const float maxRSSI = -70;
+		float score1 = (avg-minRSSI) / (maxRSSI-minRSSI);	// min = 0; max = 1
+		if (score1 > 1) {score1 = 1;}
+		if (score1 < 0) {score1 = 0;}
+		//const float score1 = 1.0 - std::exp(-0.07 * (avg-minRSSI));
+
+		// std dev score
+		const float score2 = 1.0 - std::exp(-1.0 * stdDev);
+
+		return score1 * score2;
+
+	}
+
+	/** quality score [0:1] according to the number of seen APs */
+	float getQualityByCount() const {
+
+		// distinct macs
+		std::unordered_set<MACAddress> macs;
+		for (const WiFiMeasurements& mes : history) {
+			for (const WiFiMeasurement& m : mes.entries) {
+				macs.insert(m.getAP().getMAC());
+			}
+		}
+
+		// number of distinct macs
+		const int cnt = macs.size();
+
+		// see function plot. function between [0.0:1.0] has 0.5 around 7 seen APs
+		return 1.0 - std::exp(-0.1 * cnt);
+
+	}
+
+};
+
+#endif // WIFIQUALITYANALYZER_H

sensors/radio/model/WiFiModel.h

@@ -1,7 +1,13 @@
 #ifndef WIFIMODEL_H
 #define WIFIMODEL_H
 
-#include "../LocatedAccessPoint.h"
+#include "../AccessPoint.h"
+#include "../../../geo/Point3.h"
+#include <vector>
+
+
+#include "../../../data/XMLserialize.h"
+
 
 /**
  * interface for signal-strength prediction models.
@@ -9,10 +15,13 @@
  * the model is passed a MAC-address of an AP in question, and a position.
  * hereafter the model returns the RSSI for this AP at the questioned location.
  */
-class WiFiModel {
+class WiFiModel : public XMLserialize {
 
 public:
 
+	/** dtor */
+	virtual ~WiFiModel() {;}
+
 //	/** get the given access-point's RSSI at the provided location */
 //	virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
 
@@ -27,6 +36,7 @@ public:
 	 */
 	virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0;
 
+
 };
 
 #endif // WIFIMODEL_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/WiFiModelLogDist.h

@@ -4,6 +4,8 @@
 #include "WiFiModel.h"
 #include "LogDistanceModel.h"
 
+#include <unordered_map>
+
 /**
  * signal-strength estimation using log-distance model
  */
@@ -86,6 +88,14 @@ public:
 
 	}
 
+	void readFromXML(XMLDoc* doc, XMLElem* src) override {
+		throw Exception("not yet implemented");
+	}
+
+	void writeToXML(XMLDoc* doc, XMLElem* dst) override {
+		throw Exception("not yet implemented");
+	}
+
 };
 
 #endif // WIFIMODELLOGDIST_H

sensors/radio/model/WiFiModelLogDistCeiling.h

@@ -2,6 +2,7 @@
 #define WIFIMODELLOGDISTCEILING_H
 
 #include "../../../floorplan/v2/Floorplan.h"
+#include "../../../floorplan/v2/FloorplanCeilings.h"
 
 #include "../../../Assertions.h"
 #include "WiFiModel.h"
@@ -9,6 +10,8 @@
 #include "../VAPGrouper.h"
 #include "../../../misc/Debug.h"
 
+#include "../../../data/XMLserialize.h"
+
 /**
  * signal-strength estimation using log-distance model
  * including ceilings between AP and position
@@ -36,24 +39,28 @@ private:
 	/** map of all APs (and their parameters) known to the model */
 	std::unordered_map<MACAddress, APEntry> accessPoints;
 
-	/** position (height) of all ceilings (in meter) */
-	std::vector<float> ceilingsAtHeight_m;
+//	/** position (height) of all ceilings (in meter) */
+//	std::vector<float> ceilingsAtHeight_m;
+	Floorplan::Ceilings ceilings;
 
 public:
 
 	/** ctor with floorplan (needed for ceiling position) */
-	WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) {
+	WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) : ceilings(map) {
 
 		// sanity checks
 		Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
 
-		// position of all ceilings
-		for (Floorplan::Floor* f : map->floors) {
-			ceilingsAtHeight_m.push_back(f->atHeight);
-		}
 
 	}
 
+	/** get the entry for the given mac. exception if missing */
+	const APEntry& getAP(const MACAddress& mac) const {
+		const auto& it = accessPoints.find(mac);
+		if (it == accessPoints.end()) {throw Exception("model does not contain an entry for " + mac.asString());}
+		return it->second;
+	}
+
 	/** get a list of all APs known to the model */
 	std::vector<AccessPoint> getAllAPs() const {
 		std::vector<AccessPoint> aps;
@@ -62,38 +69,47 @@ public:
 	}
 
 	/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
-	void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+	void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) {
 
 		for (const Floorplan::Floor* floor : map->floors) {
 			for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
 				const APEntry ape(ap->getPos(floor), txp, exp, waf);
-				addAP(MACAddress(ap->mac), ape);
+				addAP(MACAddress(ap->mac), ape, assertSafe);
 			}
 		}
 
 	}
 
-	/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
-	void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+	/**
+	 * load AP information from the floorplan.
+	 * use the given fixed TXP/EXP/WAF for all APs.
+	 * usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
+	 */
+	void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) {
 
 		for (const Floorplan::Floor* floor : map->floors) {
 			for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
 				const APEntry ape(ap->getPos(floor), txp, exp, waf);
 				const MACAddress mac = vg.getBaseMAC(MACAddress(ap->mac));
-				Log::add("WiModLDC", "AP: " + ap->mac + " -> " + mac.asString());
-				addAP(MACAddress(mac), ape);
+				Log::add("WiModLDC", "AP added! given: " + ap->mac + " -> after VAP: " + mac.asString());
+				addAP(MACAddress(mac), ape, assertSafe);
 			}
 		}
 
 	}
 
-	/** make the given AP (and its parameters) known to the model */
-	void addAP(const MACAddress& accessPoint, const APEntry& params) {
+	/**
+	 * make the given AP (and its parameters) known to the model
+	 * usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
+	 */
+	void addAP(const MACAddress& accessPoint, const APEntry& params, const bool assertSafe = true) {
 
 		// sanity check
-		Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
-		Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
-		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+		if (assertSafe) {
+			Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
+			Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
+			Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+		}
 
 		Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?");
 
@@ -102,6 +118,14 @@ public:
 
 	}
 
+	/**
+	 * make the given AP (and its parameters) known to the model
+	 * usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
+	 */
+	void addAP(const MACAddress& accessPoint, const Point3 pos_m, const float txp, const float exp, const float waf, const bool assertSafe = true) {
+		addAP(accessPoint, APEntry(pos_m, txp, exp, waf), assertSafe);
+	}
+
 	/** remove all added APs */
 	void clear() {
 		accessPoints.clear();
@@ -134,71 +158,71 @@ public:
 		const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
 
 		// WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value
-		const float wafLoss = params.waf * numCeilingsBetween(position_m, params.position_m);
+		//const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m);
+		const float wafLoss = params.waf * ceilings.numCeilingsBetweenFloat(position_m, params.position_m);
+		//const float wafLoss = params.waf * ceilings.numCeilingsBetweenLinearInt(position_m, params.position_m);
 
 		// combine
-		return rssiLOS + wafLoss;
+		const float res = rssiLOS + wafLoss;
+
+		// sanity check
+		Assert::isNotNaN(res, "detected NaN within WiFiModelLogDistCeiling::getRSSI()");
+
+		// ok
+		return res;
 
 	}
 
+	void writeToXML(XMLDoc* doc, XMLElem* dst) override {
 
+		// set my type
+		dst->SetAttribute("type", "WiFiModelLogDistCeiling");
 
-protected:
-
-	FRIEND_TEST(LogDistanceCeilingModel, numCeilings);
-	FRIEND_TEST(LogDistanceCeilingModel, numCeilingsFloat);
-
-
-	/** get the number of ceilings between z1 and z2 */
-	float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
-
-
-		const float zMin = std::min(pos1.z, pos2.z);
-		const float zMax = std::max(pos1.z, pos2.z);
-
-		float cnt = 0;
-
-		for (const float z : ceilingsAtHeight_m) {
-			if (zMin < z && zMax > z) {
-				const float dmax = zMax - z;
-				cnt += (dmax > 1) ? (1) : (dmax);
-			}
+		for (const auto& it : accessPoints) {
+			const MACAddress& mac = it.first;
+			const APEntry& ape = it.second;
+			XMLElem* xap = doc->NewElement("ap");
+			xap->SetAttribute("mac", mac.asString().c_str());
+			xap->SetAttribute("px", ape.position_m.x);
+			xap->SetAttribute("py", ape.position_m.y);
+			xap->SetAttribute("pz", ape.position_m.z);
+			xap->SetAttribute("txp", ape.txp);
+			xap->SetAttribute("exp", ape.exp);
+			xap->SetAttribute("waf", ape.waf);
+			dst->InsertEndChild(xap);
 		}
 
-		return cnt;
+		for (const float atHeight_m : ceilings.getCeilings()) {
+			XMLElem* xceil = doc->NewElement("ceiling");
+			xceil->SetAttribute("atHeight", atHeight_m);
+			dst->InsertEndChild(xceil);
+		}
 
 	}
 
-	/** get the number of ceilings between z1 and z2 */
-	int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
+	void readFromXML(XMLDoc* doc, XMLElem* src) override {
 
-		int cnt = 0;
-		const float zMin = std::min(pos1.z, pos2.z);
-		const float zMax = std::max(pos1.z, pos2.z);
+		// check type
+		if (std::string("WiFiModelLogDistCeiling") != src->Attribute("type")) {throw Exception("invalid model type");}
 
-#ifdef WITH_ASSERTIONS
+		accessPoints.clear();
+		ceilings.clear();
 
-		static int numNear = 0;
-		static int numFar = 0;
-		for (const float z : ceilingsAtHeight_m) {
-			const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
-			if (diff < 0.1) {++numNear;} else {++numFar;}
-		}
-		if ((numNear + numFar) > 150000) {
-			Assert::isTrue(numNear < numFar*0.1,
-			    "many requests to the WiFiModel address nodes (very) near to a ground! \
-			    due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
-			    expect very wrong outputs! \
-			    consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
+		XML_FOREACH_ELEM_NAMED("ap", xap, src) {
+			MACAddress mac = MACAddress(xap->Attribute("mac"));
+			APEntry ape(
+				Point3(xap->FloatAttribute("px"), xap->FloatAttribute("py"), xap->FloatAttribute("pz")),
+			    xap->FloatAttribute("txp"),
+			    xap->FloatAttribute("exp"),
+			    xap->FloatAttribute("waf")
 			);
-		}
-#endif
-
-		for (const float z : ceilingsAtHeight_m) {
-			if (zMin < z && zMax > z) {++cnt;}
+			accessPoints.insert( std::make_pair(mac, ape) );
 		}
 
-		return cnt;
+		XML_FOREACH_ELEM_NAMED("ceiling", xceil, src) {
+			const float atHeight_m = xceil->FloatAttribute("atHeight");
+			ceilings.addCeiling(atHeight_m);
+		}
 
 	}
 

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

sensors/radio/setup/WiFiFingerprint.h

@@ -8,10 +8,10 @@
 #include <unordered_map>
 
 /**
- * denotes a wifi fingerprint
- * known position and several measurements conducted at this position
+ * denotes a wifi fingerprint:
+ *	known position and 1-n measurements [wifi-scan on all channels] conducted at this position
  *
- * as several measurements were conducted, each AP is usually contained more than once!
+ * if more than one measurement is conducted, each AP is contained more than once!
  */
 struct WiFiFingerprint {
 
@@ -19,7 +19,7 @@ struct WiFiFingerprint {
 	/** real-world-position that was measured */
 	Point3 pos_m;
 
-	/** measurements (APs) at the given location */
+	/** seen APs at the given location */
 	WiFiMeasurements measurements;
 
 
@@ -29,9 +29,11 @@ struct WiFiFingerprint {
 	/** ctor */
 	WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;}
 
+	/** ctor */
+	WiFiFingerprint(const Point3 pos_m, const WiFiMeasurements& measurements) : pos_m(pos_m), measurements(measurements) {;}
 
 
-	/** as each AP is contained more than once (scanned more than once), group them by MAC and use the average RSSI */
+	/** as each AP might be contained more than once (scanned more than once), group them by MAC and use the average RSSI */
 	WiFiMeasurements average() {
 
 		// group scans by MAC (all measurements for one AP)
@@ -46,9 +48,9 @@ struct WiFiFingerprint {
 			const WiFiMeasurements& apMeasurements = it.second;
 			WiFiMeasurement avg = apMeasurements.entries.front();			// average starts with a copy of the first entry (to get all data-fields beside the rssi)
 			for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) {		// sum up all other entries [1:end]
-				avg.rssi += apMeasurements.entries[i].rssi;
+                avg.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI());
 			}
-			avg.rssi /= apMeasurements.entries.size();
+            avg.setRssi(avg.getRSSI() / apMeasurements.entries.size());
 			res.entries.push_back(avg);										// add to output
 		}
 
@@ -57,12 +59,21 @@ struct WiFiFingerprint {
 
 	}
 
+	/** get all measurements for the given MAC */
+	std::vector<WiFiMeasurement> getAllForMAC(const MACAddress& mac) const {
+		std::vector<WiFiMeasurement> res;
+		for (const WiFiMeasurement& m : measurements.entries) {
+			if (m.getAP().getMAC() == mac) {res.push_back(m);}
+		}
+		return res;
+	}
+
 	/** serialize */
 	void write(std::ostream& out) const {
 		out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n";
 		out << "num: " << measurements.entries.size() << "\n";
 		for (const WiFiMeasurement& wm : measurements.entries) {
-			out << wm.getTimestamp().ms() << " " << wm.ap.getMAC().asString() << " " << wm.getRSSI() << "\n";
+            out << wm.getTimestamp().ms() << " " << wm.getAP().getMAC().asString() << " " << wm.getRSSI() << "\n";
 		}
 	}
 

sensors/radio/setup/WiFiFingerprints.h

@@ -0,0 +1,140 @@
+#ifndef WIFIFINGERPRINTS_H
+#define WIFIFINGERPRINTS_H
+
+#include <fstream>
+
+#include "WiFiFingerprint.h"
+#include <iostream>
+
+/**
+ * helper class to load and save fingerprints.
+ * fingerprints are wifi-measurements given at some known location
+ * those can be used to e.g. optimize access-point models etc.
+ */
+class WiFiFingerprints {
+
+private:
+
+//	/** the file to save the calibration model to */
+//	std::string file;
+
+	/** all fingerprints (position -> measurements) within the model */
+	std::vector<WiFiFingerprint> fingerprints;
+
+public:
+
+	/** empty ctor */
+	WiFiFingerprints() {
+
+	}
+
+	/** ctor from file */
+	WiFiFingerprints(const std::string& file) {
+		load(file);
+	}
+
+	const std::vector<WiFiFingerprint>& getFingerprints() const {
+		return fingerprints;
+	}
+
+	std::vector<WiFiFingerprint>& getFingerprints() {
+		return fingerprints;
+	}
+
+	/** attach the given fingerprint */
+	void add(const WiFiFingerprint& fp) {
+		fingerprints.push_back(fp);
+	}
+
+	/** get all fingerprints that measured exactly the given mac [no VAP grouping] */
+	const std::vector<WiFiFingerprint> getFingerprintsFor(const MACAddress& mac) const {
+
+		std::vector<WiFiFingerprint> res;
+
+		// process each fingerprint location
+		for (const WiFiFingerprint& _fp : fingerprints) {
+
+			// start with an empty copy
+			WiFiFingerprint fp = _fp; fp.measurements.entries.clear();
+
+			// only add the measurements that belong to the requested mac
+			for (const WiFiMeasurement& _m : _fp.measurements.entries) {
+				if (_m.getAP().getMAC() == mac) {
+					fp.measurements.entries.push_back(_m);
+				}
+			}
+			if (fp.measurements.entries.size() > 0) {	// got some measurements for this AP?
+				res.push_back(fp);
+			}
+		}
+
+		return res;
+
+	}
+
+	/** deserialize the model */
+	void load(const std::string& file) {
+
+		// open and check
+		std::ifstream inp(file.c_str());
+		if (inp.bad() || inp.eof() || ! inp.is_open()) { return; }
+
+		// read all entries
+		while (!inp.eof()) {
+
+			// each section starts with [fingerprint]
+			std::string section;
+			inp >> section;
+			if (inp.eof()) {break;}
+			if (section != "[fingerprint]") {throw Exception("error!");}
+
+			// deserialize it
+			WiFiFingerprint wfp;
+			wfp.read(inp);
+			if (wfp.measurements.entries.empty()) {continue;}
+			fingerprints.push_back(wfp);
+
+		}
+
+		inp.close();
+
+	}
+
+
+	/** serialize the model */
+	void save(const std::string& file) {
+
+		// open and check
+		std::ofstream out(file.c_str());
+		if (out.bad()) {throw Exception("error while opening " + file + " for write");}
+
+		// write all entries
+		for (const WiFiFingerprint& wfp : fingerprints) {
+			out << "[fingerprint]\n";
+			wfp.write(out);
+		}
+
+		// done
+		out.close();
+
+	}
+
+	/** get the fingerprint for the given location. if no fingerprint exists, an empty one is created! */
+	WiFiFingerprint& getFingerprint(const Point3 pos_m) {
+
+		// try to find an existing one
+		for (WiFiFingerprint& wfp : fingerprints) {
+			// get within range of floating-point rounding issues
+			if (wfp.pos_m.getDistance(pos_m) < 0.01) {return wfp;}
+		}
+
+		// create a new one and return its reference
+		WiFiFingerprint wfp(pos_m);
+		fingerprints.push_back(wfp);
+		return fingerprints.back();
+
+	}
+
+};
+
+#endif // WIFIFINGERPRINTS_H

sensors/radio/setup/WiFiOptimizer.h

@@ -1,247 +1,68 @@
-#ifndef OPTIMIZER_H
-#define OPTIMIZER_H
-
-#include "../../../floorplan/v2/Floorplan.h"
-#include "../../../floorplan/v2/FloorplanHelper.h"
+#ifndef WIFIOPTIMIZER_H
+#define WIFIOPTIMIZER_H
 
+#include "WiFiFingerprints.h"
+#include "WiFiOptimizerStructs.h"
 #include "../VAPGrouper.h"
-#include "../../../geo/BBox3.h"
-#include "../../../misc/Debug.h"
 
-#include "WiFiFingerprint.h"
-#include "../model/WiFiModel.h"
-#include "../model/WiFiModelLogDistCeiling.h"
+#include <unordered_map>
 
-#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
-#include <KLib/math/optimization/NumOptAlgoGenetic.h>
-#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
+namespace WiFiOptimizer {
 
-#include <string>
-#include <sstream>
+	/** base-class for all WiFiOptimizers */
+	class Base {
 
-struct WiFiOptimizer {
+	protected:
 
-private:
+		/** each MAC-Adress has several position->rssi entries */
+		std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
 
-	/** combine one RSSI measurement with the position the signal was measured at */
-	struct RSSIatPosition {
+		/** how to handle virtual access points [group, not-group, ..] */
+		const VAPGrouper vg;
 
-		/** real-world position (in meter) */
-		const Point3 pos_m;
-
-		/** measured signal strength (for one AP) */
-		const float rssi;
+	public:
 
 		/** ctor */
-		RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
+		Base(const VAPGrouper vg) : vg(vg) {;}
 
-	};
-
-public:
-
-	struct APParams {
-		float x;
-		float y;
-		float z;
-		float txp;
-		float exp;
-		float waf;
-		Point3 getPos() const {return Point3(x,y,z);}
-		APParams() {;}
-		APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
-		std::string asString() {
-			std::stringstream ss;
-			ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
-			return ss.str();
-		}
-	};
-
-	/** add MAC-info to params */
-	struct APParamsMAC {
-		MACAddress mac;
-		APParams params;
-		APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
-	};
-
-private:
-
-	Floorplan::IndoorMap* map;
-	const VAPGrouper vg;
-
-	/** each MAC-Adress has several position->rssi entries */
-	std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
-
-	const char* name = "WiFiOptimizer";
-
-public:
-
-	/** ctor */
-	WiFiOptimizer(Floorplan::IndoorMap* map, const VAPGrouper& vg) : map(map), vg(vg) {
-		;
-	}
-
-
-	/** add a new fingerprint to the optimizers data-source */
-	void addFingerprint(const WiFiFingerprint& fp) {
-
-		// group the fingerprint's measurements by VAP (if configured)
-		const WiFiMeasurements measurements = vg.group(fp.measurements);
-
-		// add each available AP to its slot (lookup map)
-		for (const WiFiMeasurement& m : measurements.entries) {
-			const RSSIatPosition rap(fp.pos_m, m.rssi);
-			apMap[m.getAP().getMAC()].push_back(rap);
+		/** get a list of all to-be-optimized access-points (given by their mac-address) */
+		virtual std::vector<MACAddress> getAllMACs() const {
+			std::vector<MACAddress> res;
+			for (const auto& it : apMap) {res.push_back(it.first);}
+			return res;
 		}
 
-	}
-
-	/** get a list of all to-be-optimized access-points (given by their mac-address) */
-	std::vector<MACAddress> getAllMACs() const {
-		std::vector<MACAddress> res;
-		for (const auto& it : apMap) {res.push_back(it.first);}
-		return res;
-	}
-
-	/** optimize all known APs */
-	std::vector<APParamsMAC> optimizeAll() const {
-
-		// sanity chekc
-		Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!");
-
-		float errSum = 0;
-		std::vector<APParamsMAC> res;
-		for (const MACAddress& mac : getAllMACs()) {
-			float err;
-			const APParams params = optimize(mac, err);
-			res.push_back(APParamsMAC(mac, params));
-			errSum += err;
+		/** get all [VAPGrouped, Averaged] fingerprints for the given mac */
+		virtual const std::vector<RSSIatPosition>& getFingerprintsFor(const MACAddress& mac) {
+			const auto& it = apMap.find(mac);
+			if (it == apMap.end()) {throw Exception("mac not found: " + mac.asString());}
+			return it->second;
 		}
 
-		const float avgErr = errSum / getAllMACs().size();
-		Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
+		/** add a new fingerprint to the optimizer as data-source */
+		virtual void addFingerprint(const WiFiFingerprint& fp) {
 
-		return res;
-
-	}
-
-	/** optimize the given AP */
-	APParams optimize(const MACAddress& mac, float& errResult) const {
-
-		// starting parameters do not matter for the current optimizer!
-		APParams params(0,0,0, -40, 2.5, -4.0);
-		constexpr float hugeError = 1e10;
-
-		// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
-		const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
-
-
-		// signal-strength-prediction-model...
-		WiFiModelLogDistCeiling model(map);
-
-		auto func = [&] (const float* data) {
-
-			const APParams* params = (APParams*) data;
-
-			// some sanity checks
-			if (params->waf > 0)	{return hugeError;}
-
-			if (params->txp < -50)	{return hugeError;}
-			if (params->txp > -30)	{return hugeError;}
-
-			if (params->exp > 4)	{return hugeError;}
-			if (params->exp < 1)	{return hugeError;}
-
-			// current position guess for the AP;
-			const Point3 apPos_m = params->getPos();
-
-			// add the AP [described by the current guess] to the signal-strength-prediction model
-			model.clear();
-			model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
-
-			float err = 0;
-			int cnt = 0;
-
-			// process each measurement
-			for (const RSSIatPosition& reading : entries) {
-
-				// get the model-estimation for the fingerprint's position
-				const float rssiModel = model.getRSSI(mac, reading.pos_m);
-
-				// difference between estimation and measurement
-				const float diff = std::abs(rssiModel - reading.rssi);
-
-				// adjust the error
-				err += diff*diff;
-				++cnt;
-
-				// max distance penality
-				// [unlikely to get a reading for this AP here!]
-				if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
+			// group the fingerprint's measurements by VAP (if configured)
+			const WiFiMeasurements measurements = vg.group(fp.measurements);
 
+			// add each available AP to its slot (lookup map)
+			for (const WiFiMeasurement& m : measurements.entries) {
+				const RSSIatPosition rap(fp.pos_m, m.getRSSI());
+				apMap[m.getAP().getMAC()].push_back(rap);
 			}
 
-			err /= cnt;
-			err = std::sqrt(err);
+		}
 
-			if (params->txp < -50) {err += 999999;}
-			if (params->txp > -35) {err += 999999;}
+		/** add new fingerprints to the optimizer as data-source */
+		virtual void addFingerprints(const WiFiFingerprints& fps) {
+			for (const WiFiFingerprint& fp : fps.getFingerprints()) {
+				addFingerprint(fp);
+			}
+		}
 
-			if (params->exp > 3.5) {err += 999999;}
-			if (params->exp < 1.0) {err += 999999;}
-
-			return err;
-
-		};
+	};
 
 
-		//
-		const BBox3 mapBBox = FloorplanHelper::getBBox(map);
+}
 
-		using LeOpt = K::NumOptAlgoRangeRandom<float>;
-		const std::vector<LeOpt::MinMax> valRegion = {
-			 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
-			 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
-			 LeOpt::MinMax(mapBBox.getMin().z -  5, mapBBox.getMax().z +  5),	// z
-			 LeOpt::MinMax(-50,-30),	// txp
-			 LeOpt::MinMax(1,3),		// exp
-			 LeOpt::MinMax(-10,-4),		// waf
-		 };
-
-
-		// log
-		Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
-		Log::tick();
-
-		LeOpt opt(valRegion);
-		opt.setPopulationSize(500);	// USE MORE FOR PRODUCTION
-		opt.setNumIerations(150);
-		opt.calculateOptimum(func, (float*) &params);
-
-//		using LeOpt = K::NumOptAlgoGenetic<float>;
-//		LeOpt opt(6);
-//		opt.setPopulationSize(750);
-//		opt.setMaxIterations(50);
-//		opt.setElitism(0.05f);
-//		opt.setMutation(0.75f);
-//		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
-//		opt.setValRegion(valRegion);
-
-//		K::NumOptAlgoDownhillSimplex<float, 6> opt;
-//		opt.setMaxIterations(100);
-//		opt.setNumRestarts(10);
-
-		opt.calculateOptimum(func, (float*) &params);
-		errResult = func((float*)&params);
-
-		Log::tock();
-		Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err");
-
-		return params;
-
-	}
-
-
-};
-
-
-#endif // OPTIMIZER_H
+#endif // WIFIOPTIMIZER_H

sensors/radio/setup/WiFiOptimizerLogDistCeiling.h

@@ -0,0 +1,345 @@
+#ifndef WIFI_OPTIMIZER_LOG_DIST_CEILING_H
+#define WIFI_OPTIMIZER_LOG_DIST_CEILING_H
+
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+
+#include "../../../geo/BBox3.h"
+#include "../../../misc/Debug.h"
+
+#include "WiFiFingerprint.h"
+#include "WiFiFingerprints.h"
+
+#include "../model/WiFiModel.h"
+#include "../model/WiFiModelLogDistCeiling.h"
+
+#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
+#include <KLib/math/optimization/NumOptAlgoGenetic.h>
+#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
+
+#include <string>
+#include <sstream>
+
+#include "WiFiOptimizer.h"
+#include "WiFiOptimizerStructs.h"
+
+#include <functional>
+
+namespace WiFiOptimizer {
+
+	/**
+	 * optimize access-point parameters,
+	 * given several fingerprints using the log-dist-ceiling model
+	 */
+	struct LogDistCeiling : public Base {
+
+	public:
+
+		enum class Mode {
+			FAST,
+			MEDIUM,
+			QUALITY,
+		};
+
+		/**
+		 * resulting optimization stats for one AP
+		 */
+		struct Stats {
+
+			/** average model<->scan error after optimzing */
+			float error_db;
+
+			/** number of fingerprints [= locations] that were used for optimzing */
+			int usedFingerprins;
+
+			/** resulting model<->scan error after optimzing for each individual fingerprints [= location] */
+			std::vector<ErrorAtPosition> errors;
+
+			/** get the location where the model estimation reaches the highest negative value [model estimation too low] */
+			ErrorAtPosition getEstErrorMaxNeg() const {
+				auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
+				return *std::min_element(errors.begin(), errors.end(), cmpErrAtPos);
+			}
+
+			/** get the location where the model estimation reaches the highest positive value [model estimation too high] */
+			ErrorAtPosition getEstErrorMaxPos() const {
+				auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
+				return *std::max_element(errors.begin(), errors.end(), cmpErrAtPos);
+			}
+
+		};
+
+		/** parameters for one AP when using the LogDistCeiling model */
+		struct APParams {
+
+			float x;
+			float y;
+			float z;
+
+			float txp;
+			float exp;
+			float waf;
+
+			Point3 getPos() const {return Point3(x,y,z);}
+
+			/** ctor */
+			APParams() {;}
+
+			/** ctor */
+			APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
+
+			std::string asString() const {
+				std::stringstream ss;
+				ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
+				return ss.str();
+			}
+
+			/** we add some constraints to the parameter range */
+			bool outOfRange() const {
+				return  (waf > 0) ||
+				        (txp < -50) ||
+				        (txp > -30) ||
+				        (exp > 4) ||
+				        (exp < 1);
+			}
+
+		};
+
+		/** add MAC-info to params */
+		struct APParamsMAC {
+			MACAddress mac;
+			APParams params;
+			APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
+		};
+
+		class APParamsList {
+
+			std::vector<APParamsMAC> lst;
+
+		public:
+
+			/** ctor */
+			APParamsList(const std::vector<APParamsMAC>& lst) : lst(lst) {
+
+			}
+
+			/** get the list */
+			const std::vector<APParamsMAC>& get() const {
+				return lst;
+			}
+
+			/** get params for the given mac [if known, otherwise nullptr] */
+			const APParamsMAC* get (const MACAddress& mac) const {
+				for (const APParamsMAC& ap : lst) {
+					if (ap.mac == mac) {return &ap;}
+				}
+				return nullptr;
+			}
+
+		};
+
+		using APFilter = std::function<bool(const int numFingerprints, const MACAddress& mac)>;
+
+		const APFilter NONE = [] (const int numFingerprints, const MACAddress& mac) {
+			(void) numFingerprints; (void) mac;
+			return false;
+		};
+
+		const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) {
+			(void) mac;
+			return numFingerprints < 5;
+		};
+
+	private:
+
+		Floorplan::IndoorMap* map;
+
+		Mode mode = Mode::QUALITY;
+
+		const char* name = "WiFiOptLDC";
+
+	public:
+
+		/** ctor */
+		LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) {
+			;
+		}
+
+		/** ctor */
+		LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const WiFiFingerprints& fps, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) {
+			addFingerprints(fps);
+		}
+
+		/** optimize all known APs */
+		APParamsList optimizeAll(APFilter filter) const {
+
+			// sanity check
+			Assert::isFalse(getAllMACs().empty(), "no APs found for optimization! call addFingerprint() first!");
+
+			float errSum = 0; int errCnt = 0;
+			std::vector<APParamsMAC> res;
+			for (const MACAddress& mac : getAllMACs()) {
+				Stats stats;
+				const APParams params = optimize(mac, stats);
+				if (!filter(stats.usedFingerprins, mac)) {
+					res.push_back(APParamsMAC(mac, params));
+					errSum += stats.error_db;
+					++errCnt;
+				} else {
+					std::cout << "ignored due to filter!" << std::endl;
+				}
+			}
+
+			const float avgErr = errSum / errCnt;
+			Log::add(name, "optimized APs: " + std::to_string(errCnt));
+			Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
+
+			// done
+			return APParamsList(res);
+
+		}
+
+
+		/** optimize the given AP */
+		APParams optimize(const MACAddress& mac, Stats& res) const {
+
+			// starting parameters do not matter for the current optimizer!
+			APParams params(0,0,0, -40, 2.5, -4.0);
+
+			// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
+			const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
+
+			// log
+			Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
+			Log::tick();
+
+			// get the map's size
+			const BBox3 mapBBox = FloorplanHelper::getBBox(map);
+
+			using LeOpt = K::NumOptAlgoRangeRandom<float>;
+			const std::vector<LeOpt::MinMax> valRegion = {
+				 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
+				 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
+				 LeOpt::MinMax(mapBBox.getMin().z -  5, mapBBox.getMax().z +  5),	// z
+			     LeOpt::MinMax(-50, -30),		// txp
+			     LeOpt::MinMax(1, 4),			// exp
+			     LeOpt::MinMax(-15, -0),		// waf
+			 };
+
+
+
+
+			LeOpt opt(valRegion);
+
+			switch(mode) {
+			case Mode::FAST:
+				opt.setPopulationSize(100);
+				opt.setNumIerations(50);
+				break;
+			case Mode::MEDIUM:
+				opt.setPopulationSize(200);
+				opt.setNumIerations(100);
+				break;
+			case Mode::QUALITY:
+				opt.setPopulationSize(500);
+				opt.setNumIerations(150);
+				break;
+			}
+
+			// error function
+			auto func = [&] (const float* params) {
+				return getErrorLogDistCeiling(mac, entries, params, nullptr);
+			};
+
+			opt.calculateOptimum(func, (float*) &params);
+
+	//		using LeOpt = K::NumOptAlgoGenetic<float>;
+	//		LeOpt opt(6);
+	//		opt.setPopulationSize(750);
+	//		opt.setMaxIterations(50);
+	//		opt.setElitism(0.05f);
+	//		opt.setMutation(0.75f);
+	//		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
+	//		opt.setValRegion(valRegion);
+
+	//		K::NumOptAlgoDownhillSimplex<float, 6> opt;
+	//		opt.setMaxIterations(100);
+	//		opt.setNumRestarts(10);
+
+			opt.calculateOptimum(func, (float*) &params);
+			res.error_db = getErrorLogDistCeiling(mac, entries, (float*)&params, &res);
+			res.usedFingerprins = entries.size();
+
+			Log::tock();
+			Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(res.error_db) +" dB err");
+
+			return params;
+
+		}
+
+
+	private:
+
+		float getErrorLogDistCeiling(const MACAddress& mac, const std::vector<RSSIatPosition>& entries, const float* data, Stats* stats = nullptr) const {
+
+			const APParams* params = (APParams*) data;
+
+			// some sanity checks
+			if (params->outOfRange()) {return 1e10;}
+
+			// current position guess for the AP;
+			const Point3 apPos_m = params->getPos();
+
+			// add the AP [described by the current guess] to the signal-strength-prediction model
+			// signal-strength-prediction-model...
+			WiFiModelLogDistCeiling model(map);
+			model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
+
+			float err = 0;
+			int cnt = 0;
+
+			// process each measurement
+			for (const RSSIatPosition& reading : entries) {
+
+				// get the model-estimation for the fingerprint's position
+				const float rssiModel = model.getRSSI(mac, reading.pos_m);
+
+				// difference between estimation and measurement
+				const float diff = std::abs(rssiModel - reading.rssi);
+
+				// add error to stats object?
+				if (stats) {
+					stats->errors.push_back(ErrorAtPosition(reading.pos_m, reading.rssi, rssiModel));
+				}
+
+				// adjust the error
+				err += std::pow(std::abs(diff), 2.0);
+				++cnt;
+
+				// max distance penality
+				// [unlikely to get a reading for this AP here!]
+				if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
+
+			}
+
+			err /= cnt;
+			err = std::sqrt(err);
+
+			if (params->txp < -50) {err += 999999;}
+			if (params->txp > -35) {err += 999999;}
+
+			if (params->exp > 3.5) {err += 999999;}
+			if (params->exp < 1.0) {err += 999999;}
+
+			return err;
+
+		}
+
+
+
+	};
+
+}
+
+
+#endif // WIFI_OPTIMIZER_LOG_DIST_CEILING_H

sensors/radio/setup/WiFiOptimizerStructs.h

@@ -0,0 +1,55 @@
+#ifndef WIFIOPTIMIZERSTRUCTS_H
+#define WIFIOPTIMIZERSTRUCTS_H
+
+#include "../../../geo/Point3.h"
+
+namespace WiFiOptimizer {
+
+	/**
+	 * one entry that is used during optimization:
+	 * combine one RSSI measurement with the position the signal was measured at
+	 */
+	struct RSSIatPosition {
+
+		/** real-world position (in meter) */
+		const Point3 pos_m;
+
+		/** measured signal strength (for one AP) */
+		const float rssi;
+
+		/** ctor */
+		RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
+
+	};
+
+	/**
+	 * for statistics
+	 * after optimiziaton
+	 *
+	 * denotes the difference [error] between one fingerprinted rssi
+	 * at location (x,y,z) and the model estimation for this location
+	 */
+	struct ErrorAtPosition {
+
+		/** real-world position (in meter) */
+		const Point3 pos_m;
+
+		/** measured signal strength (for one AP) */
+		const float scan_rssi;
+
+		/** final model's prediction */
+		const float model_rssi;
+
+		/** ctor */
+		ErrorAtPosition(const Point3 pos_m, const float scan_rssi, const float model_rssi) : pos_m(pos_m), scan_rssi(scan_rssi), model_rssi(model_rssi)  {;}
+
+		/** get the difference [error] between model-estimated-rssi and fingerprinted-rssi */
+		float getError_db() const {
+			return model_rssi - scan_rssi;
+		}
+
+	};
+
+}
+
+#endif // WIFIOPTIMIZERSTRUCTS_H