current state

wifi/EvalWiFiGround.h

@@ -0,0 +1,77 @@
+#ifndef EVALWIFIGROUND_H
+#define EVALWIFIGROUND_H
+
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/model/WiFiModels.h"
+
+#include "Indoor/sensors/radio/VAPGrouper.h"
+#include "Indoor/sensors/offline/FileReader.h"
+
+#include "Indoor/floorplan/v2/Floorplan.h"
+#include "Indoor/floorplan/v2/FloorplanReader.h"
+#include "Indoor/floorplan/v2/FloorplanHelper.h"
+#include "Indoor/floorplan/v2/FloorplanCeilings.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
+
+#include <KLib/math/statistics/Statistics.h>
+
+#include "../Structs.h"
+#include "../plots/Plotty.h"
+#include "../plots/PlotErrTime.h"
+#include "../plots/PlotErrFunc.h"
+#include "../plots/PlotWiFiGroundProb.h"
+//#include "../CSV.h"
+
+#include <unordered_set>
+#include <thread>
+
+class EvalWiFiGround {
+
+private:
+
+	WiFiModel* mdl;
+	WiFiObserverFree* obs;
+	PlotWiFiGroundProb* groundProb;
+
+public:
+
+	EvalWiFiGround(Floorplan::IndoorMap* map, const std::string calibModel) {
+
+		mdl = WiFiModelFactory(map).loadXML(calibModel);
+		obs = new WiFiObserverFree(8.0, *mdl);
+		groundProb = new PlotWiFiGroundProb(map);
+
+	}
+
+	void show(const std::string walkFile) {
+
+		Offline::FileReader reader(walkFile);
+		int cnt = 0;
+
+		for (const auto& entry : reader.getWiFiGroupedByTime()) {
+			const WiFiMeasurements& mes = entry.data;
+			groundProb->show(*obs, mes);
+			groundProb->plotMe();
+			//if (++cnt > 70) {break;}
+			std::this_thread::sleep_for(std::chrono::milliseconds(2));
+		}
+
+		//groundProb->update();
+		groundProb->plotMe();
+		int i = 0; (void) i;
+
+	}
+
+};
+
+#endif // EVALWIFIGROUND_H

wifi/EvalWiFiPathMethods.h

@@ -0,0 +1,555 @@
+#ifndef EVALWIFIPATHMETHODS_H
+#define EVALWIFIPATHMETHODS_H
+
+
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/model/WiFiModels.h"
+
+#include "Indoor/sensors/radio/VAPGrouper.h"
+#include "Indoor/sensors/offline/FileReader.h"
+
+#include "Indoor/floorplan/v2/Floorplan.h"
+#include "Indoor/floorplan/v2/FloorplanReader.h"
+#include "Indoor/floorplan/v2/FloorplanHelper.h"
+#include "Indoor/floorplan/v2/FloorplanCeilings.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
+
+#include <KLib/math/statistics/Statistics.h>
+#include <Indoor/math/MovingAVG.h>
+#include <Indoor/math/MovingMedian.h>
+
+#include "../Structs.h"
+#include "../plots/Plotty.h"
+#include "../plots/PlotErrTime.h"
+#include "../plots/PlotErrFunc.h"
+#include "../plots/PlotWiFiGroundProb.h"
+//#include "CSV.h"
+
+#include <unordered_set>
+#include <thread>
+#include "../Settings.h"
+
+/** evaluate just the wifi error by using the same model but various probability functions */
+class EvalWiFiPathMethods {
+
+private:
+
+	Floorplan::IndoorMap* map;
+	BBox3 mapBBox;
+
+	VAPGrouper* vap = nullptr;
+
+	K::Statistics<float>* statsOrig_m;
+	K::Statistics<float>* statsOther_m;
+	PlotErrFunc* pef_m;
+
+	K::Statistics<float>* statsOrig_p;
+	K::Statistics<float>* statsOther_p;
+	PlotErrFunc* pef_p;
+
+	K::Statistics<float>* statsOrig_c;
+	K::Statistics<float>* statsOther_c;
+	PlotErrFunc* pef_c;
+
+	WiFiModel* wiModel = nullptr;
+
+	std::vector<Point3> randomLoc;
+	std::vector<AccessPoint> allAPs;
+
+	int idx = -2;
+
+public:
+
+	/** ctor with map and fingerprints */
+	EvalWiFiPathMethods(const std::string& mapFile) {
+
+		// load floorplan
+		map = Floorplan::Reader::readFromFile(mapFile);
+
+		// estimate bbox
+		mapBBox = FloorplanHelper::getBBox(map);
+
+		// how to handle VAPs
+		vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+
+		pef_m = new PlotErrFunc("error (meter)", "measurements (%)");
+		pef_m->showMarkers(false);
+
+		pef_p = new PlotErrFunc("-log(p(..))", "measurements (%)");
+		pef_p->showMarkers(false);
+		pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
+
+
+		pef_c = new PlotErrFunc("cross error", "measurements (%)");
+		pef_c->showMarkers(false);
+		pef_c->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
+
+
+
+
+		// generate some random locations within the walkable area
+		std::minstd_rand gen;
+		std::uniform_real_distribution<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
+		std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
+		for (const Floorplan::Floor* f : map->floors) {
+
+			std::vector<Point3> floorLoc;
+
+			while (floorLoc.size() < 100) {
+
+				const Point3 pt(distX(gen), distY(gen), f->atHeight + 1.3);	// human above ground
+
+
+				for (const Floorplan::FloorOutlinePolygon* poly : f->outline) {
+
+					// ensure the random samples are nicely placed along the floor
+					for (const Point3& p : floorLoc) {
+						if (p.getDistance(pt) < 4) {continue;}
+					}
+
+					HelperPoly hp(*poly);
+					if (hp.contains(pt.xy()*100)) {
+						floorLoc.push_back(pt);
+					}
+				}
+			}
+
+			for (const Point3 p : floorLoc) {randomLoc.push_back(p);}
+
+		}
+
+//		Plotty p(map);
+//		p.buildFloorplan();
+//		for (const Point3 pt :randomLoc) {
+//			p.points.add({pt.x, pt.y, pt.z});
+//		}
+//		p.plot();
+//		sleep(1000);
+
+
+//		// generate some random locations within the building
+//		std::minstd_rand gen;
+//		std::uniform_real_distribution<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
+//		std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
+//		std::uniform_real_distribution<float> distZ(mapBBox.getMin().z, mapBBox.getMax().z);
+//		for (int i = 0; i < 250; ++i) {
+//			const Point3 pt(distX(gen), distY(gen), distZ(gen));
+//			for (const Point3& p : randomLoc) {
+//				if (p.getDistance(pt) < 3) {continue;}	// ensure the random samples are nicely placed
+//			}
+//			randomLoc.push_back(pt);
+//		}
+
+	}
+
+
+
+	void loadModel(const std::string& xmlFile, const std::string& modelName, const std::string& modeNameA, const std::string& modeNameB) {
+
+		idx += 2;
+
+		WiFiModelFactory fac(map);
+
+		// setup the model
+		this->wiModel = fac.loadXML(xmlFile);
+
+		statsOrig_m = new K::Statistics<float>();
+		statsOrig_p = new K::Statistics<float>();
+		statsOrig_c = new K::Statistics<float>();
+
+		statsOther_m = new K::Statistics<float>();
+		statsOther_p = new K::Statistics<float>();
+		statsOther_c = new K::Statistics<float>();
+
+		pef_m->add(modeNameA, statsOrig_m);
+		pef_m->add(modeNameB, statsOther_m);
+		pef_m->getPlot().getAxisX().setTicsLabelFormat("%h m");
+
+		pef_p->add(modeNameA, statsOrig_p);
+		pef_p->add(modeNameB, statsOther_p);
+
+		pef_c->add(modeNameA, statsOrig_c);
+		pef_c->add(modeNameB, statsOther_c);
+		pef_c->getPlot().getAxisX().setTicsLabelFormat("%h %%");
+
+
+		this->allAPs = wiModel->getAllAPs();
+		for (const AccessPoint& ap : allAPs) {
+			std::cout << ap.getMAC().asString() << std::endl;
+		}
+		int i = 0; (void) i;
+
+	}
+
+	void walks (const std::vector<std::string> files, const std::vector<std::vector<int>> gtIndicies) {
+		for (size_t i = 0; i < files.size(); ++i) {
+			walk(files[i], gtIndicies[i]);
+		}
+	}
+
+	void writeGP(const std::string& path, const std::string& name) {
+		writeGP(*pef_m, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_meter");
+		writeGP(*pef_c, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_cross");
+		writeGP(*pef_p, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_logprob");
+	}
+
+	void writeGP(PlotErrFunc& pef, K::GnuplotSize size, const std::string& file) {
+		pef.getGP().setTerminal("epslatex", size);
+		pef.getGP().setOutput(file+".tex");
+		pef.getPlot().getKey().setVisible(true);
+		pef.getPlot().getKey().setSampleLength(0.5);
+		pef.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);
+		pef.getPlot().getKey().setWidthIncrement(-4);
+		pef.getPlot().getAxisY().setLabelOffset(2.5, 0);
+		pef.getPlot().getAxisY().setTicsStep(0, 25, 95);
+		pef.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0,95));
+
+		pef.getPlot().getAxisX().setLabel("");
+		pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
+		pef.getGP() << "set lmargin 4.5\n";
+		pef.getGP() << "set tmargin 0.1\n";
+		pef.getGP() << "set rmargin 0.4\n";
+		pef.getGP() << "set bmargin 2.0\n";
+		pef.writeCodeTo(file+".gp");
+		pef.plot();
+		pef.writeCodeTo("");
+	}
+
+	void walk(const std::string& fPath, const std::vector<int> gtIndices) {
+
+		Offline::FileReader reader(fPath);
+
+		const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);
+
+		// process each wifi entry within the offline file
+		for (const auto wifi : reader.wifi) {
+
+			// all seen APs at one timestamp
+			const WiFiMeasurements& _mes = wifi.data;
+
+			// perform vap grouping
+			const WiFiMeasurements mes = vap->group(_mes);
+
+			// error calculation
+			auto funcOrig =		[&] (const float* params) -> double { return errFuncOrig(params, mes); };
+			auto funcOther =	[&] (const float* params) -> double { return errFuncOther(params, mes); };
+
+			// parameters (x,y,z);
+			float paramsOrig[3] = {0,0,0};
+			float paramsOther[3] = {0,0,0};
+
+			// USE RANGE RANDOM WITH COOLING
+			using LeOpt = K::NumOptAlgoRangeRandom<float>;
+			const std::vector<LeOpt::MinMax> valRegion = {
+				 LeOpt::MinMax(mapBBox.getMin().x, mapBBox.getMax().x),	// x
+				 LeOpt::MinMax(mapBBox.getMin().y, mapBBox.getMax().y),	// y
+				 LeOpt::MinMax(mapBBox.getMin().z, mapBBox.getMax().z),	// z
+			 };
+
+			K::NumOptAlgoRangeRandom<float> opt(valRegion);
+			opt.setPopulationSize(400);
+			opt.setNumIerations(30);
+
+			opt.calculateOptimum(funcOrig, paramsOrig);
+			opt.calculateOptimum(funcOther, paramsOther);
+
+
+			// estimation
+			//std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
+			const Point3 curEstOrig(paramsOrig[0], paramsOrig[1], paramsOrig[2]);
+			const Point3 curEstOther(paramsOther[0], paramsOther[1], paramsOther[2]);
+			const Timestamp ts = mes.entries.front().getTimestamp();
+
+			// groud-truth
+			const Point3 gt = gtp.get(ts);
+
+			// error in meter
+			//const float err_m = gt.xy().getDistance(curEst.xy());		// 2D
+			const float errOrig_m = gt.getDistance(curEstOrig);				// 3D
+			const float errOther_m = gt.getDistance(curEstOther);			// 3D
+
+			statsOrig_m->add(errOrig_m);
+			//pet_m.addErr(ts, errOrig_m, idx+0);
+			statsOther_m->add(errOther_m);
+			//pet_m.addErr(ts, errOther_m, idx+1);
+
+//			// error in -log(p)
+			float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3};	// human above ground
+
+			{
+				const double probOnGT_a = -errFuncOrig(gtFloat, mes);
+				const double logProbOnGT_a = -std::log10(probOnGT_a);
+				const double logProbOnGTNorm_a = (logProbOnGT_a/mes.entries.size());
+				statsOrig_p->add(logProbOnGTNorm_a);
+			}
+
+			{
+				const double probOnGT_b = -errFuncOther(gtFloat, mes);
+				const double logProbOnGT_b = -std::log10(probOnGT_b);
+				const double logProbOnGTNorm_b = (logProbOnGT_b/mes.entries.size());
+
+				// break on huge errors
+				if (logProbOnGTNorm_b > 2) {
+//					std::cout << "-------------------------------"<< std::endl;
+//					std::cout << gt.asString() << std::endl;
+//					getVeto(gt, mes);
+//					throw "123";
+				}
+				statsOther_p->add(logProbOnGTNorm_b);
+			}
+
+			checkCross(funcOrig, gt, statsOrig_c);
+			checkCross(funcOther, gt, statsOther_c);
+
+			//pet_p.addErr(ts, logProbOnGTNorm, idx);
+
+			static int xx = 0; ++xx;
+
+			if ( (xx % 20) == 0) {
+				pef_p->plot();
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+				pef_m->plot();
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+				pef_c->plot();
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+			}
+
+
+
+		}
+
+	}
+
+private:
+
+
+
+	void checkCross(std::function<double(const float* params)> func, Point3 gt, K::Statistics<float>* stats) {
+
+		const float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3};	// above ground
+		const double orig_p = -func(gtFloat);
+		float bigger = 0;
+		int cnt = 0;
+
+		float errSum = 0;
+		int errCnt = 0;
+
+		for (const Point3 pt : randomLoc) {
+
+			if (pt.getDistance(gt) > 20) {continue;}
+			float params[3] = {pt.x, pt.y, pt.z};
+			const double other_p = -func(params);
+
+			if (pt.getDistance(gt) >= 3) {			// at least 3 meter away
+				if (other_p > orig_p*0.75) {++bigger;}
+			}
+
+			if (pt.getDistance(gt) >=5) {			// at least 5 meter away
+				if (other_p > orig_p) {errSum += pt.getDistance(gt); ++errCnt;}
+			}
+			++cnt;
+		}
+
+
+//		for (float rad = 0; rad < 2*M_PI; rad += 0.1) {
+//			Point3 p = gt + Point3(std::cos(rad), std::sin(rad), 0) * 5;	// 5 meters around gt
+//			float params[3] = {p.x, p.y, p.z};
+//			const double other_p = func(params);
+//			if (other_p >orig_p) {++bigger;}
+//			++cnt;
+//		}
+
+		const float factor = bigger / (float)cnt;
+		//const float factor = errSum / (errCnt+0.00001f);
+
+		// break on huge errors
+		if (factor > 0.12) {
+//			std::cout << "-------------------------------"<< std::endl;
+//			std::cout << gt.asString() << std::endl;
+//			func(gtFloat);
+//			const float gtFloat2[3] = {gt.x, gt.y, gt.z-1};
+//			func(gtFloat2);
+//			throw "123";
+		}
+
+		stats->add(factor * 100);	// scale to "%"
+
+	}
+
+	double errFuncOrig(const float* params, const WiFiMeasurements& mes) {
+
+		// crop z to 1 meter
+		//params[2] = std::round(params[2]);
+
+		// suggested position
+		const Point3 pos_m(params[0], params[1], params[2]);
+
+		const float sigma = 8.0;
+		double prob = 1.0;
+
+		// calculate error for above position using the currently available measurements
+		for (const WiFiMeasurement& m : mes.entries) {
+
+			// skip non-FHWS APs
+			if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;}
+
+			// get model's rssi for the given location
+			const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);
+
+			// skip APs unknown to the model
+			if (rssi_model != rssi_model) {
+				std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
+				continue;
+			}
+
+			// get scan's rssi
+			const float rssi_scan = m.getRSSI();
+
+			// likelyhood
+			const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
+			//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);
+
+			// adjust
+			prob *= p;
+
+		}
+
+		const double err = -prob;
+		return err;
+
+	}
+
+	double errFuncOther(const float* params, const WiFiMeasurements& mes) {
+
+		// crop z to 1 meter
+		//params[2] = std::round(params[2]);
+
+		// suggested position
+		const Point3 pos_m(params[0], params[1], params[2]);
+
+		const float sigma = 8.0;
+		double prob = 1.0;
+		double error = 0;
+		int cnt = 0;
+
+		//const auto comp = [] (const WiFiMeasurement& m1, const WiFiMeasurement& m2) {return m1.getRSSI() < m2.getRSSI();};
+		//const auto& min = std::min_element(mes.entries.begin(), mes.entries.end(), comp);
+
+		// calculate error for above position using the currently available measurements
+		for (const WiFiMeasurement& m : mes.entries) {
+
+			// skip non-FHWS APs
+			if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;}
+
+			// TESTING
+			//if (m.getAP().getMAC() == min->getAP().getMAC()) {continue;}
+			//if (m.getRSSI() < -90) {continue;}
+			//if (m.getRSSI() > -55) {continue;}
+
+			const float rssi = m.getRSSI();
+//			const volatile float min = -100;
+//			const volatile float max = -40;
+//			const volatile float val = (m.getRSSI() - min) / (max-min);
+
+			// get model's rssi for the given location
+			const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);
+
+			// skip APs unknown to the model
+			if (rssi_model != rssi_model) {
+				std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
+				continue;
+			}
+
+			// get scan's rssi
+			const float rssi_scan = m.getRSSI();
+
+			// likelyhood
+			//double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
+			//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);
+			//const double p = Distribution::Triangle<double>::getProbability(rssi_model, sigma*2, rssi_scan);
+			//double p = Distribution::Exponential<double>::getProbability(1, std::abs(rssi_model-rssi_scan));
+
+			double p = Distribution::Exponential<double>::getProbability(1.0, std::abs(rssi_model-rssi_scan));
+
+			//p = 0.85 * p + 0.15 * (1.0-p);
+			//p = 0.95 * p + 0.05;
+
+//			const double diff = std::abs(rssi_model - rssi_scan);
+////			error += diff;
+//			++cnt;
+//			if		(diff < 3)	{error += 0.001;}
+//			else if (diff < 5)	{error += 0.001;}
+//			else if (diff < 8)	{error += 0.001;}
+//			else if (diff < 10)	{error += 3.0;}
+//			else if (diff < 15)	{error += 5.0;}
+//			else				{error += 15.0;}
+//			error += std::pow((diff / 10.0f), 5) / 10.0f;
+
+			// adjust
+			prob *= p;
+
+		}
+
+		const double err = -prob;
+		return err;
+
+	}
+
+	double getVeto(const Point3& pos_m, const WiFiMeasurements& obs) const {
+
+		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 = wiModel->getRSSI(ap.getMAC(), pos_m);
+			if (rssi != rssi) {throw Exception("should not happen?!");}	// 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];
+			if (apr.rssi < -80) {continue;}
+			const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
+			if (!mes) {
+				++numVetos;
+				continue;
+				//std::cout << apr.ap.getMAC().asString() << ":" << apr.rssi << std::endl;
+			}
+			const float rssiScan = mes->getRSSI();
+			const float rssiModel = apr.rssi;
+			const float diff = std::abs(rssiScan - rssiModel);
+			if (diff > 20) {++numVetos;}
+		}
+
+		return (numVetos < 1) ? (0.999) : (0.001);
+
+//		if (numVetos == 0)	{return 0.70;}
+//		if (numVetos == 1)	{return 0.70;}
+//		else				{return 0.01;}
+
+	}
+
+};
+
+
+#endif // EVALWIFIPATHMETHODS_H

wifi/EvalWiFiPaths.h

@@ -0,0 +1,226 @@
+#ifndef EVALWIFIPATHS_H
+#define EVALWIFIPATHS_H
+
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
+#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
+#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
+#include "Indoor/sensors/radio/model/WiFiModels.h"
+
+#include "Indoor/sensors/radio/VAPGrouper.h"
+#include "Indoor/sensors/offline/FileReader.h"
+
+#include "Indoor/floorplan/v2/Floorplan.h"
+#include "Indoor/floorplan/v2/FloorplanReader.h"
+#include "Indoor/floorplan/v2/FloorplanHelper.h"
+#include "Indoor/floorplan/v2/FloorplanCeilings.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
+
+#include <KLib/math/statistics/Statistics.h>
+#include <Indoor/math/MovingAVG.h>
+#include <Indoor/math/MovingMedian.h>
+
+#include "../Structs.h"
+#include "../plots/Plotty.h"
+#include "../plots/PlotErrTime.h"
+#include "../plots/PlotErrFunc.h"
+#include "../plots/PlotWiFiGroundProb.h"
+//#include "CSV.h"
+
+#include <unordered_set>
+#include <thread>
+#include "../Settings.h"
+
+/** evaluate just the wifi error for several given paths */
+class EvalWiFiPaths {
+
+private:
+
+	Floorplan::IndoorMap* map;
+	BBox3 mapBBox;
+
+	VAPGrouper* vap = nullptr;
+
+	K::Statistics<float>* stats_m;
+	PlotErrFunc* pef_m;
+
+	K::Statistics<float>* stats_p;
+	PlotErrFunc* pef_p;
+
+	WiFiModel* wiModel = nullptr;
+
+	int idx = -1;
+
+public:
+
+	/** ctor with map and fingerprints */
+	EvalWiFiPaths(const std::string& mapFile) {
+
+		// load floorplan
+		map = Floorplan::Reader::readFromFile(mapFile);
+
+		// estimate bbox
+		mapBBox = FloorplanHelper::getBBox(map);
+
+		// how to handle VAPs
+		vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
+
+		pef_m = new PlotErrFunc("\\small{error (m)}", "\\small{measurements (\\%)}");
+		pef_m->showMarkers(false);
+
+		pef_p = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}");
+		pef_p->showMarkers(false);
+		pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
+
+	}
+
+
+
+	void loadModel(const std::string& xmlFile, const std::string& name) {
+
+		++idx;
+
+		WiFiModelFactory fac(map);
+
+		// setup the model
+		this->wiModel = fac.loadXML(xmlFile);
+
+		stats_m = new K::Statistics<float>();
+		stats_p = new K::Statistics<float>();
+
+		pef_m->add(name, stats_m);
+		pef_p->add(name, stats_p);
+
+	}
+
+	void walks (const std::vector<std::string> files, const std::vector<std::vector<int>> gtIndicies) {
+		for (size_t i = 0; i < files.size(); ++i) {
+			walk(files[i], gtIndicies[i]);
+		}
+	}
+
+	void walk(const std::string& fPath, const std::vector<int> gtIndices) {
+
+		static PlotErrTime pet_m("","",""); pet_m.clear();
+		static PlotErrTime pet_p("","",""); pet_p.clear();
+		Offline::FileReader reader(fPath);
+
+		const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);
+
+		// process each wifi entry within the offline file
+		for (const auto wifi : reader.wifi) {
+
+			// all seen APs at one timestamp
+			const WiFiMeasurements& _mes = wifi.data;
+
+			// perform vap grouping
+			const WiFiMeasurements mes = vap->group(_mes);
+
+			// error calculation
+			auto func = [&] (const float* params) -> double {
+
+				// crop z to 1 meter
+				//params[2] = std::round(params[2]);
+
+				// suggested position
+				const Point3 pos_m(params[0], params[1], params[2]);
+
+				const float sigma = 8.0;
+				double prob = 1.0;
+
+				// calculate error for above position using the currently available measurements
+				for (const WiFiMeasurement& m : mes.entries) {
+
+					// skip non-FHWS APs
+					if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;}
+
+					// get model's rssi for the given location
+					const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);
+
+					// skip APs unknown to the model
+					if (rssi_model != rssi_model) {
+						std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
+						continue;
+					}
+
+					// get scan's rssi
+					const float rssi_scan = m.getRSSI();
+
+					// likelyhood
+					const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
+					//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);
+
+					// adjust
+					prob *= p;
+
+				}
+
+				const double err = -prob;
+				return err;
+
+			};
+
+			// parameters (x,y,z);
+			float params[3];
+
+			// USE RANGE RANDOM WITH COOLING
+			using LeOpt = K::NumOptAlgoRangeRandom<float>;
+			const std::vector<LeOpt::MinMax> valRegion = {
+				 LeOpt::MinMax(mapBBox.getMin().x, mapBBox.getMax().x),	// x
+				 LeOpt::MinMax(mapBBox.getMin().y, mapBBox.getMax().y),	// y
+				 LeOpt::MinMax(mapBBox.getMin().z, mapBBox.getMax().z),	// z
+			 };
+
+			K::NumOptAlgoRangeRandom<float> opt(valRegion);
+			opt.setPopulationSize(200);
+			opt.setNumIerations(50);
+			opt.calculateOptimum(func, params);
+
+			// estimation
+			std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
+			const Point3 curEst(params[0], params[1], params[2]);
+			const Timestamp ts = mes.entries.front().getTimestamp();
+
+			// groud-truth
+			const Point3 gt = gtp.get(ts);
+
+			// error in meter
+			//const float err_m = gt.xy().getDistance(curEst.xy());		// 2D
+			const float err_m = gt.getDistance(curEst);				// 3D
+			stats_m->add(err_m);
+			pet_m.addErr(ts, err_m, idx);
+
+			// error in -log(p)
+			float gtFloat[3] = {gt.x, gt.y, gt.z};
+			const double probOnGT = -func(gtFloat);
+			const double logProbOnGT = -std::log10(probOnGT);
+			const double logProbOnGTNorm = (logProbOnGT/mes.entries.size());
+			stats_p->add(logProbOnGTNorm);
+			pet_p.addErr(ts, logProbOnGTNorm, idx);
+
+			static int xx = 0; ++xx;
+
+			if (xx % 10 == 0) {
+				pet_p.plot();
+				pet_m.plot();
+
+				pef_p->plot();
+				pef_m->plot();
+			}
+
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+		}
+
+	}
+
+};
+
+#endif // EVALWIFIPATHS_H