added beacon stuff similiar architecture then wifi \n added activity percentage stuff \n added testcases

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,93 @@
+#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, -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(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