added wifi per-floor optimization

added plot to wifi-quality-analyzer
changes to per-floor wifi models
minor fixes

geo/BBox3.h

@@ -89,11 +89,14 @@ public:
 		p2 += p;	// increase maximum
 	}
 
-		/** set both, min/max z to the same value */
+	/** set both, min/max z to the same value */
-		void setZ(const float z) {
+	void setZ(const float z) {
-			p1.z = z;
+		p1.z = z;
-			p2.z = z;
+		p2.z = z;
-		}
+	}
+
+	void setMinZ(const float z) {this->p1.z = z;}
+	void setMaxZ(const float z) {this->p2.z = z;}
 
 	/** does the bbox contain the given point? */
 	bool contains(const Point3& p) const {

navMesh/walk/NavMeshWalkKLD.h

@@ -81,7 +81,7 @@ namespace NM {
             // to-be-walked distance;
             const float toBeWalkedDist = params.getToBeWalkedDistance();
             const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
-            const float toBeWalkedDistKld = (kld * qualityWifi * 0.5);
+			const float toBeWalkedDistKld = (kld * qualityWifi);
 
             // construct reachable region
             NavMeshSub<Tria> reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD...

sensors/radio/WiFiProbabilityFree.h

@@ -15,6 +15,14 @@
  */
 class WiFiObserverFree : public WiFiProbability {
 
+public:
+
+	enum class EvalDist {
+		NORMAL_DISTIRBUTION,
+		CAPPED_NORMAL_DISTRIBUTION,
+		EXPONENTIAL,
+	};
+
 private:
 
 	const float sigma;
@@ -31,9 +39,11 @@ private:
 
 	bool useError = false;
 
+	EvalDist dist;
+
 public:
 
-	WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) {
+	WiFiObserverFree(const float sigma, WiFiModel& model, EvalDist dist = EvalDist::NORMAL_DISTIRBUTION) : sigma(sigma), model(model), dist(dist) {
 		allAPs = model.getAllAPs();
 	}
 
@@ -76,8 +86,16 @@ public:
 			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
 			// probability for this AP
-			double local = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			double local = NAN;
-			//double local = Distribution::Exponential<double>::getProbability(0.1, std::abs(modelRSSI-scanRSSI));
+			switch (dist) {
+				case EvalDist::NORMAL_DISTIRBUTION:
+					local = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI); break;
+				case EvalDist::CAPPED_NORMAL_DISTRIBUTION:
+					local = Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI); break;
+				case EvalDist::EXPONENTIAL:
+					local = Distribution::Exponential<double>::getProbability(0.05, std::abs(modelRSSI-scanRSSI)); break;
+				default: throw Exception("unsupported distribution");
+			}
 
 			// also add the error value? [location is OK but model is wrong]
 			if (useError) {

sensors/radio/WiFiQualityAnalyzer.h

@@ -4,6 +4,12 @@
 #include "WiFiMeasurements.h"
 #include <unordered_set>
 
+#ifdef WITH_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#endif
+
 class WiFiQualityAnalyzer {
 
 private:
@@ -12,8 +18,25 @@ private:
 	std::vector<WiFiMeasurements> history;
 	float quality = 0;
 
+	#ifdef WITH_DEBUG_PLOT
+		K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines line1;
+		K::GnuplotPlotElementLines line2;
+		int gpX = 0;
+	#endif
+
+
 public:
 
+	WiFiQualityAnalyzer() {
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(&line1);
+			plot.add(&line2);
+			plot.setTitle("WiFi Quality");
+		#endif
+	}
+
 	/** attach the current measurement and infer the quality */
 	void add(const WiFiMeasurements& mes) {
 
@@ -43,6 +66,16 @@ private:
 
 		quality = qAvgdB;
 
+		#ifdef WITH_DEBUG_PLOT
+			line1.add(K::GnuplotPoint2(gpX,qAvgdB)); line1.setTitle("dB"); line1.getStroke().setWidth(2);
+			line2.add(K::GnuplotPoint2(gpX,qCnt)); line2.setTitle("visible");
+			while(line1.size() > 50) {line1.remove(0);}
+			while(line2.size() > 50) {line2.remove(0);}
+			++gpX;
+			gp.draw(plot);
+			gp.flush();
+		#endif
+
 	}
 
 	/** score [0:1] based on the average sig-strength. the higher the better */

sensors/radio/model/WiFiModelPerFloor.h

@@ -14,6 +14,8 @@
  */
 class WiFiModelPerFloor : public WiFiModel {
 
+public:
+
 	struct ModelForFloor {
 
 		float fromZ;
@@ -30,6 +32,8 @@ class WiFiModelPerFloor : public WiFiModel {
 
 	};
 
+private:
+
 	Floorplan::IndoorMap* map;
 
 	/** all contained models [one per floor] */
@@ -46,6 +50,10 @@ public:
 
 	}
 
+	/** get a list of all models for the distinct floors */
+	std::vector<ModelForFloor>& getFloorModels() {
+		return models;
+	}
 
 	/** get a list of all APs known to the model */
 	std::vector<AccessPoint> getAllAPs() const override {
@@ -70,11 +78,70 @@ public:
 
 	float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
 
-		for (const ModelForFloor& mff : models) {
+		#if (1==0)
-			if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);}
-		}
 
-		return -120;
+			float res = -120;
+
+			// find the best matching one
+			for (const ModelForFloor& mff : models) {
+				if (mff.matches(position_m.z)) {
+					const float rssi = mff.mdl->getRSSI(accessPoint, position_m);
+					if (rssi > res) {res = rssi;}
+				}
+			}
+
+			return res;
+
+
+		#elif (1==0)
+
+			// nearest matching model
+			float nearest = 9999;
+			float res = -120;
+
+			for (const ModelForFloor& mff : models) {
+				const float distToFloor = std::abs(position_m.z - mff.fromZ);
+				if (distToFloor < nearest) {
+					const float rssi = mff.mdl->getRSSI(accessPoint, position_m);
+					if (rssi == rssi) {
+						res = rssi;
+						nearest = distToFloor;
+					}
+				}
+			}
+
+			Assert::isNotNaN(res, "detected NaN");
+			return res;
+
+		#elif (1==1)
+
+			// average of all matching models
+
+			float sum = 0;
+			int cnt = 0;
+
+			// find the best matching one
+			for (const ModelForFloor& mff : models) {
+				if (mff.matches(position_m.z)) {
+					const float rssi = mff.mdl->getRSSI(accessPoint, position_m);
+					if (rssi == rssi) {
+						sum += rssi; ++cnt;
+					}
+				}
+			}
+
+			Assert::isNotNaN(sum, "detected NaN");
+
+			return (cnt > 0) ? (sum/cnt) : (-120);
+
+		#endif
+
+
+//		for (const ModelForFloor& mff : models) {
+//			if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);}
+//		}
+
+//		return -120;
 
 	}
 

sensors/radio/model/WiFiModels.h

@@ -1,6 +1,7 @@
 #ifndef WIFIMODELS_H
 #define WIFIMODELS_H
 
+/** umbrella header for WiFiModel and factory */
 #include "WiFiModel.h"
 #include "WiFiModelFactory.h"
 #include "WiFiModelFactoryImpl.h"

sensors/radio/setup/WiFiOptimizerLogDistCeiling.h

@@ -99,7 +99,7 @@ namespace WiFiOptimizer {
 				return  (waf > 0) ||
 				        (txp < -50) ||
 				        (txp > -30) ||
-				        (exp > 4) ||
+						(exp > 4) ||
 				        (exp < 1);
 			}
 
@@ -138,18 +138,29 @@ namespace WiFiOptimizer {
 
 		};
 
-		using APFilter = std::function<bool(const int numFingerprints, const MACAddress& mac)>;
+		using APFilter = std::function<bool(const Stats& stats, const MACAddress& mac)>;
 
-		const APFilter NONE = [] (const int numFingerprints, const MACAddress& mac) {
+		const APFilter NONE = [] (const Stats& stats, const MACAddress& mac) {
-			(void) numFingerprints; (void) mac;
+			(void) stats; (void) mac;
 			return false;
 		};
 
-		const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) {
+		const APFilter MIN_2_FPS = [] (const Stats& stats, const MACAddress& mac) {
 			(void) mac;
-			return numFingerprints < 5;
+			return stats.usedFingerprins < 2;
 		};
 
+		const APFilter MIN_5_FPS = [] (const Stats& stats, const MACAddress& mac) {
+			(void) mac;
+			return stats.usedFingerprins < 5;
+		};
+
+		const APFilter MIN_10_FPS = [] (const Stats& stats, const MACAddress& mac) {
+			(void) mac;
+			return stats.usedFingerprins < 10;
+		};
+
+
 	private:
 
 		Floorplan::IndoorMap* map;
@@ -179,15 +190,21 @@ namespace WiFiOptimizer {
 			float errSum = 0; int errCnt = 0;
 			std::vector<APParamsMAC> res;
 			for (const MACAddress& mac : getAllMACs()) {
+
+				// perform optimization, get resulting parameters and optimization stats
 				Stats stats;
 				const APParams params = optimize(mac, stats);
-				if (!filter(stats.usedFingerprins, mac)) {
+
+				// filter based on stats (option to ignore/filter some access-points)
+				if (!filter(stats, mac)) {
 					res.push_back(APParamsMAC(mac, params));
 					errSum += stats.error_db;
 					++errCnt;
 				} else {
-					std::cout << "ignored due to filter!" << std::endl;
+					Log::add(name, "ignoring opt-result for AP " + mac.asString() + " due to filter");
+					//std::cout << "ignored due to filter!" << std::endl;
 				}
+
 			}
 
 			const float avgErr = errSum / errCnt;
@@ -222,8 +239,8 @@ namespace WiFiOptimizer {
 				 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(1, 4),			// exp
-			     LeOpt::MinMax(-15, -0),		// waf
+				 LeOpt::MinMax(-15, -0),		// waf
 			 };
 
 

sensors/radio/setup/WiFiOptimizerPerFloor.h

@@ -0,0 +1,141 @@
+#ifndef WIFIOPTIMIZERPERFLOOR_H
+#define WIFIOPTIMIZERPERFLOOR_H
+
+#include "WiFiOptimizerLogDistCeiling.h"
+#include "../model/WiFiModelPerFloor.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+#include <mutex>
+
+#define WITH_DEBUG_PLOT
+#ifdef WITH_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#endif
+
+/**
+ * uses the log-distance model, but one per floor.
+ * the model is optimized using all fingerprints that belong to this floor
+ */
+class WiFiOptimizerPerFloor {
+
+	WiFiModelPerFloor* mdl = nullptr;
+
+	WiFiFingerprints fps;
+
+	Floorplan::IndoorMap* map;
+	std::mutex mtx;
+
+#ifdef WITH_DEBUG_PLOT
+	K::Gnuplot gp;
+	K::GnuplotSplot splot;
+	K::GnuplotSplotElementColorPoints pts;
+	K::GnuplotSplotElementLines lines;
+#endif
+
+public:
+
+	WiFiOptimizerPerFloor(Floorplan::IndoorMap* map) : map(map) {
+
+		// the overall model (contains one sub-model per floor)
+		mdl = new WiFiModelPerFloor(map);
+
+		#ifdef WITH_DEBUG_PLOT
+			splot.add(&pts); pts.setPointSize(1); pts.setPointType(7);
+			splot.add(&lines);
+			BBox3 bb = FloorplanHelper::getBBox(map);
+			splot.getAxisX().setRange(bb.getMin().x, bb.getMax().x);
+			splot.getAxisY().setRange(bb.getMin().y, bb.getMax().y);
+			splot.getAxisZ().setRange(bb.getMin().z, bb.getMax().z);
+		#endif
+
+	}
+
+
+	/** make the given fingerprint known to the optimizer */
+	void addFingerprint(const WiFiFingerprint& fp) {
+		fps.add(fp);
+	}
+
+	WiFiModelPerFloor* optimizeAll() {
+
+		const VAPGrouper vg = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1);
+
+		// optimize each floor on its own
+		//for (WiFiModelPerFloor::ModelForFloor& mdlForFloor : mdl->getFloorModels()) {
+		for (size_t i = 0; i < map->floors.size(); ++i) {
+
+			const Floorplan::Floor* floor = map->floors[i];
+
+			// 1) create a new optimizer for the current floor
+			WiFiOptimizer::LogDistCeiling opt(map, vg);
+
+			// 2) create the model for this floor
+			mtx.lock();
+				WiFiModelLogDistCeiling* mdlForFloor = new WiFiModelLogDistCeiling(map);
+				mdl->add(mdlForFloor, floor);
+			mtx.unlock();
+
+			// 3) get the floor's bbox and adjust the z-region (needed for museum in Rothenburg)
+			BBox3 bb = FloorplanHelper::getBBox(floor);
+			bb.setMinZ(floor->atHeight+0.25);
+			bb.setMaxZ(floor->atHeight+2);
+
+			// 4) find all fingerprints that belong to the floor/model and add them to the optimizer
+			for (const WiFiFingerprint& fp : fps.getFingerprints()) {
+				//if (mdlForFloor.matches(fp.pos_m.z)) {
+				//	std::cout << fp.pos_m.z << std::endl;
+				//	opt.addFingerprint(fp);
+				//}
+				if (bb.contains(fp.pos_m)) {
+				//if (fp.pos_m.z >= floor->atHeight && fp.pos_m.z < floor->atHeight+floor->height) {
+					std::cout << fp.pos_m.z << std::endl;
+					opt.addFingerprint(fp);
+
+					#ifdef WITH_DEBUG_PLOT
+						pts.add(K::GnuplotPoint3(fp.pos_m.x, fp.pos_m.y, fp.pos_m.z), i);
+					#endif
+
+				}
+			}
+
+			#ifdef WITH_DEBUG_PLOT
+				for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+					for (const Point2 pt : poly->poly.points) {
+						lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight));
+					}
+					lines.splitFace(); lines.splitFace();
+					for (const Point2 pt : poly->poly.points) {
+						lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight+floor->height));
+					}
+					lines.splitFace(); lines.splitFace();
+				}
+				gp.draw(splot);
+				gp.flush();
+				pts.clear();
+				lines.clear();
+				sleep(1);
+			#endif
+
+			// 5) run the optimizer
+			const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.MIN_2_FPS);
+
+			// 6) add all optimized APs to the floor's model
+			for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
+				// model is per-floor. so model cant optimize waf.. set to VERY HIGH manually
+				const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
+				mdlForFloor->addAP(ap.mac, entry);
+			}
+
+		}
+
+		return mdl;
+
+	}
+
+};
+
+
+#endif // WIFIOPTIMIZERPERFLOOR_H

wifi/estimate/ray3/FloorplanMesh.h

@@ -78,7 +78,7 @@ namespace Ray3D {
 		};
 
 		/** DEBUG: convert to .obj file code for exporting */
-		OBJData toOBJ(const std::string& name) {
+		OBJData toOBJ(const std::string name) {
 
 			bool swapYZ = true;
 			int nVerts = 1;