From b2adb16b49216031c1e852f7ebca336166c4cf24 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 5 Apr 2017 18:45:00 +0200 Subject: [PATCH] next commit --- CMakeLists.txt | 2 +- EvalApOpt.h | 2 +- EvalCompareOpt.h | 212 ++++++++++++---- EvalCompareOpt2.h | 458 +++++++++++++++++++++++++++++++++++ EvalWiFi.h | 224 +++++++++++++---- EvalWiFiSigStrength.h | 2 +- Helper.h | 144 +++++++---- PlotErrFunc.h | 100 -------- Settings.h | 21 +- main.cpp | 301 ++++++++++++++++++----- pf/EvalWalk.h | 130 +++++++--- pf/PF.h | 40 ++- plots/PlotErrFunc.h | 141 +++++++++++ plots/PlotErrTime.h | 71 ++++++ Plotty.h => plots/Plotty.h | 16 +- tex/bare_conf.tex | 2 + tex/chapters/experiments.tex | 64 ++++- tex/chapters/work.tex | 32 +++ 18 files changed, 1602 insertions(+), 360 deletions(-) create mode 100644 EvalCompareOpt2.h delete mode 100644 PlotErrFunc.h create mode 100644 plots/PlotErrFunc.h create mode 100644 plots/PlotErrTime.h rename Plotty.h => plots/Plotty.h (95%) create mode 100644 tex/chapters/work.tex diff --git a/CMakeLists.txt b/CMakeLists.txt index 607fcf6..7c3f02c 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,7 +58,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O0 + -O2 -march=native -DWITH_TESTS diff --git a/EvalApOpt.h b/EvalApOpt.h index 105065f..b5db32a 100644 --- a/EvalApOpt.h +++ b/EvalApOpt.h @@ -25,7 +25,7 @@ #include #include "Structs.h" -#include "Plotty.h" +#include "plots/Plotty.h" #include "CSV.h" #include "Helper.h" diff --git a/EvalCompareOpt.h b/EvalCompareOpt.h index 1f0948b..1238396 100644 --- a/EvalCompareOpt.h +++ b/EvalCompareOpt.h @@ -16,6 +16,8 @@ #include "Indoor/floorplan/v2/FloorplanCeilings.h" #include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h" +#include "Indoor/sensors/offline/FileReader.h" + #include "Helper.h" using APAtFloor = std::pair; @@ -39,28 +41,71 @@ protected: std::vector mapAPs; WiFiOptimizer::Base* base; +public: /** ctor with map and fingerprints */ - EvalCompareOpt(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor) { + EvalCompareOpt(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor, const bool ignoreIndoor) { + + setup(mapFile); + + // load fingerprints + calib = WiFiFingerprints(fpFile); + if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);} + if (ignoreStaircases) {calib = LeHelper::removeStaircases(calib);} + if (ignoreIndoor) {calib = LeHelper::removeIndoor(calib);} + + cleanup(); + + } + + /** ctor with map and live-walk [ground-truth] */ + EvalCompareOpt(const std::string& mapFile, std::vector>> walks) { + + setup(mapFile); + + + // ensure each AP is present at least once + WiFiMeasurements mes; + for (const APAtFloor& apf : mapAPs) { + const Floorplan::AccessPoint* ap = apf.first; + WiFiMeasurement m(AccessPoint(MACAddress(ap->mac)), -120); + mes.entries.push_back(m); + } + WiFiFingerprint fpDummy(Point3(-99, -99, -99), mes); + calib.add(fpDummy); + + for (const auto& it : walks) { + + const std::string walkFile = it.first; + const std::vector gtPathIndices = it.second; + + // load "fingerprints" by matching ground-truth against live walk + Offline::FileReader reader(walkFile); + Offline::FileReader::GroundTruth gt = reader.getGroundTruth(map, gtPathIndices); + + for (const auto& it : reader.getWiFiGroupedByTime()) { + const Timestamp ts = Timestamp::fromMS(it.ts); // time of the measurement + const WiFiMeasurements& mes = it.data; // wifi measurement + const Point3 gtPos_m = gt.get(ts); // location on the ground-truth during time of measuring + const WiFiFingerprint fp(gtPos_m, mes); // location + measurement = fingerprint + calib.add(fp); + } + + } + + cleanup(); + + } + +private: + + void setup(const std::string& mapFile) { // load floorplan map = Floorplan::Reader::readFromFile(mapFile); // how to group VAPs - vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); - - // load fingerprints - calib = WiFiFingerprints(fpFile); - - LeHelper::removeNonFHWS(calib); - if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);} - if (ignoreStaircases) {calib = LeHelper::removeStaircases(calib);} - //LeHelper::plot(map, calib); - WiFiFingerprints calib2 = calib; - for (WiFiFingerprint& fp : calib2.getFingerprints()) { - fp.measurements = vap->group(fp.measurements); - } - LeHelper::plot(map, calib2); + vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); // some ceiling calculations ceilings = Floorplan::Ceilings(map); @@ -68,15 +113,34 @@ protected: // all APs within the map mapAPs = FloorplanHelper::getAPs(map); + + } + + void cleanup() { + + LeHelper::removeNonFHWS(calib); + //LeHelper::plot(map, calib); + // used to aggreagate fingerprints base = new WiFiOptimizer::Base(*vap); base->addFingerprints(calib); + // VAP-group fingerprints + for (WiFiFingerprint& fp : calib.getFingerprints()) { + fp.measurements = vap->group(fp.measurements); + } + + // plot + LeHelper::plot(map, calib); + } - /** get the error for the given AP at the provided location */ +protected: + + /** get the error for the given APs */ K::Statistics analyzeErrorForAPs(const std::vector& aps) { + // overall error K::Statistics statsAbs; // process each AP @@ -85,7 +149,6 @@ protected: } // done - //std::cout << "overall error: " << std::endl << statsAbs.asString() << std::endl; return statsAbs; } @@ -114,17 +177,25 @@ protected: const WiFiOptimizer::LogDistCeiling::APParams& params = ap.params; const MACAddress& mac = ap.mac; - // always using the same model + // empty model WiFiModelLogDistCeiling model(map); + + // add configured AP model.addAP(mac, WiFiModelLogDistCeiling::APEntry(params.getPos(), params.txp, params.exp, params.waf), false); // get all fingerprints for the given AP const std::vector entries = base->getFingerprintsFor(mac); + // sanity check. seems ok +// const std::vector entries2 = calib.getFingerprintsFor(mac); +// if (entries.size() != entries2.size()) { +// throw Exception("detected size mismatch"); +// } + // stats K::Statistics stats; - // process each fingerprint for this ap + // process each fingerprint for this ap to estimate the error for (const WiFiOptimizer::RSSIatPosition& reading : entries) { // get the model-estimation for the fingerprint's position @@ -144,15 +215,6 @@ protected: } - - -}; - - - -/** fixed ap pos, fixed ap params */ -class EvalCompareOptAllFixed : public EvalCompareOpt { - private: // looks good @@ -162,15 +224,37 @@ private: public: - EvalCompareOptAllFixed(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor) : - EvalCompareOpt(mapFile, fpFile, ignoreStaircases, ignoreOutdoor) { - ; +// EvalCompareOptAllFixed(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor) : +// EvalCompareOpt(mapFile, fpFile, ignoreStaircases, ignoreOutdoor) { +// ; +// } + + + struct Result { + + // the configured model + WiFiModelLogDistCeiling model; + + // the resulting error + K::Statistics errAbs; + + Result(const WiFiModelLogDistCeiling& model, const K::Statistics& errAbs) : model(model), errAbs(errAbs) {;} + + }; + + WiFiModelLogDistCeiling::APEntry toModel(const WiFiOptimizer::LogDistCeiling::APParams& p) { + return WiFiModelLogDistCeiling::APEntry( + Point3(p.x, p.y, p.z), + p.txp, + p.exp, + p.waf + ); } - - /** get the error when using the given 3 params for ALL aps */ - K::Statistics getStatsAll(const float txp, const float exp, const float waf) { + Result getStatsAll(const float txp, const float exp, const float waf) { + + WiFiModelLogDistCeiling model(map); // all access points with params std::vector aps; @@ -190,31 +274,33 @@ public: WiFiOptimizer::LogDistCeiling::APParamsMAC ap(mac, params); aps.push_back(ap); + model.addAP(mac, toModel(params), false); } - return analyzeErrorForAPs(aps); + K::Statistics errAbs = analyzeErrorForAPs(aps); + return Result(model, errAbs); } /** calculate error for fixed positions and fixed constants */ - K::Statistics fixedPosFixedParamsForAll() { + Result fixedPosFixedParamsForAll() { // fire - K::Statistics stats = getStatsAll(txp, exp, waf); + Result res = getStatsAll(txp, exp, waf); std::cout << "----------------------------------------------------" << std::endl; std::cout << "AP POS FROM MAP, FIXED TXP/EXP/WAF FOR ALL APS" << std::endl; - std::cout << stats.asString() << std::endl; + std::cout << res.errAbs.asString() << std::endl; std::cout << std::endl; - return stats; + return res; } /** calculate error for fixed positions and optimized constants, but the same 3 for all APs */ - K::Statistics fixedPosOptParamsForAll() { + Result fixedPosOptParamsForAll() { auto func = [&] (const float* params) { - return getStatsAll(params[0], params[1], params[2]).getAvg(); + return getStatsAll(params[0], params[1], params[2]).errAbs.getAvg(); }; auto callback = [&] (const int run, const int iteration, const float , const float* ) { @@ -241,18 +327,20 @@ public: // opt.setMutation(0.25); // opt.calculateOptimum(func, params); - K::Statistics stats = getStatsAll(params[0], params[1], params[2]); + Result res = getStatsAll(params[0], params[1], params[2]); std::cout << "----------------------------------------------------" << std::endl; std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF: THE SAME FOR ALL APS" << std::endl; std::cout << "params: " << params[0] << "," << params[1] << "," << params[2] << std::endl; - std::cout << stats.asString() << std::endl; + std::cout << res.errAbs.asString() << std::endl; std::cout << std::endl; - return stats; + return res; } /** calculate error for fixed positions and optimized constants, each AP on its own */ - K::Statistics fixedPosOptParamsForEach() { + Result fixedPosOptParamsForEach() { + + WiFiModelLogDistCeiling model(map); K::Statistics _dstAbs; @@ -294,18 +382,25 @@ public: std::cout << "--" << mac.asString() << " params: " << params[0] << ",\t" << params[1] << ",\t" << params[2] << "\terr: " << tmp.getAvg() << std::endl; analyzeErrorForAP(mac, pos, params[0], params[1], params[2], _dstAbs); + // add the optimized AP to the final model + model.addAP(mac, pos, params[0], params[1], params[2], false); + } + std::cout << "----------------------------------------------------" << std::endl; std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl; std::cout << _dstAbs.asString() << std::endl; std::cout << std::endl; - return _dstAbs; + return Result(model, _dstAbs); } /** calculate error for fixed positions and optimized constants, each AP on its own */ - K::Statistics optPosOptParamsForEach() { + Result optPosOptParamsForEach() { + + // output only + WiFiModelLogDistCeiling model(map); K::Statistics _dstAbs; @@ -318,7 +413,12 @@ public: // fixed const MACAddress mac(mapAP.first->mac); - const Point3 pos = mapAP.first->getPos(mapAP.second); + //const Point3 pos = mapAP.first->getPos(mapAP.second); + + if (calib.getFingerprintsFor(mac).empty()) { + std::cout << "skipping AP " << mac.asString() << "!!! not seen during calibration" << std::endl; + continue; + } // opt-func for one AP auto func = [&] (const float* params) { @@ -359,19 +459,33 @@ public: std::cout << "--" << mac.asString() << " params: " << params[0] << ",\t" << params[1] << ",\t" << params[2] << ",\t" << params[3] << ",\t" << params[4] << ",\t" << params[5] << "\terr: " << tmp.getAvg() << std::endl; analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], _dstAbs); + // add to model for later reuse + model.addAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], false); + } std::cout << "----------------------------------------------------" << std::endl; std::cout << "OPTIMIZING POS/TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl; std::cout << _dstAbs.asString() << std::endl; std::cout << std::endl; - return _dstAbs; + return Result(model, _dstAbs); } + + }; +///** fixed ap pos, fixed ap params */ +//class EvalCompareOptAllFixed : public EvalCompareOpt { + + + +//}; + + + #endif // EVALCOMPAREOPT_H diff --git a/EvalCompareOpt2.h b/EvalCompareOpt2.h new file mode 100644 index 0000000..63e4c74 --- /dev/null +++ b/EvalCompareOpt2.h @@ -0,0 +1,458 @@ +#ifndef EVALCOMPAREOPT2_H +#define EVALCOMPAREOPT2_H + +#include + +#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/setup/WiFiOptimizerLogDistCeiling.h" + +#include "Indoor/sensors/radio/VAPGrouper.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 "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h" +#include "Indoor/sensors/offline/FileReader.h" + +#include "Helper.h" + +#include + +class EvalCompareOpt2 { + + int power = 1; + Floorplan::IndoorMap* map; + WiFiFingerprints calib; + VAPGrouper* vap; + Floorplan::Ceilings ceilings; + std::vector mapAPs; + +public: + + /** ctor with map and fingerprints */ + EvalCompareOpt2(const std::string& mapFile, const std::string& fpFile, std::function remove) { + + setup(mapFile); + + // load fingerprints + calib = WiFiFingerprints(fpFile); +// if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);} +// if (ignoreStaircases) {calib = LeHelper::removeStaircases(calib);} +// if (ignoreIndoor) {calib = LeHelper::removeIndoor(calib);} + calib = LeHelper::removeIf(calib, remove); + + cleanup(); + + } + + /** ctor with map and live-walk [ground-truth] */ + EvalCompareOpt2(const std::string& mapFile, std::vector>> walks) { + + setup(mapFile); + + + // ensure each AP is present at least once + WiFiMeasurements mes; + for (const APAtFloor& apf : mapAPs) { + const Floorplan::AccessPoint* ap = apf.first; + WiFiMeasurement m(AccessPoint(MACAddress(ap->mac)), -100); + mes.entries.push_back(m); + } + WiFiFingerprint fpDummy(Point3(-50, -50, -50), mes); + calib.add(fpDummy); + + for (const auto& it : walks) { + + const std::string walkFile = it.first; + const std::vector gtPathIndices = it.second; + + // load "fingerprints" by matching ground-truth against live walk + Offline::FileReader reader(walkFile); + Offline::FileReader::GroundTruth gt = reader.getGroundTruth(map, gtPathIndices); + + for (const auto& it : reader.getWiFiGroupedByTime()) { + const Timestamp ts = Timestamp::fromMS(it.ts); // time of the measurement + const WiFiMeasurements& mes = it.data; // wifi measurement + const Point3 gtPos_m = gt.get(ts); // location on the ground-truth during time of measuring + const WiFiFingerprint fp(gtPos_m, mes); // location + measurement = fingerprint + calib.add(fp); + } + + } + + cleanup(); + + } + + +private: + + void setup(const std::string& mapFile) { + + // load floorplan + map = Floorplan::Reader::readFromFile(mapFile); + + // how to group VAPs + vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); + + // some ceiling calculations + ceilings = Floorplan::Ceilings(map); + + // all APs within the map + mapAPs = FloorplanHelper::getAPs(map); + + + } + + void cleanup() { + + LeHelper::removeNonFHWS(calib); + + // VAP-group fingerprints + for (WiFiFingerprint& fp : calib.getFingerprints()) { + fp.measurements = vap->group(fp.measurements); + } + + // plot + LeHelper::plot(map, calib); + + } + +public: + + struct Result { + + // the configured model + WiFiModelLogDistCeiling model; + + // the resulting error + K::Statistics errAvg; + K::Statistics errSingle; + + Result(Floorplan::IndoorMap* map) : model(map) {;} + + }; + + + Result fixedPosFixedParamsForAll() { + + const float txp = -40; + const float exp = 2.65; + const float waf = -6.5; + + Result res(map); + + // process each AP + for (const auto _ap : mapAPs) { + const Floorplan::AccessPoint* ap = _ap.first; + const Floorplan::Floor* floor = _ap.second; + const MACAddress mac(ap->mac); + const Point3 pos_m = ap->getPos(floor); + res.model.addAP(mac, pos_m, txp, exp, waf, true); + K::Statistics err = getError(mac, res.model); + res.errAvg.add(err.getAvg()); + res.errSingle.add(err); + } + + dump(res); + return res; + + } + + Result fixedPosOptParamsForAll() { + + // resulting error using the given txp,exp,waf for ALL APs + auto errFunc = [&] (const float txp, const float exp, const float waf) -> Result { + + Result res(map); + + // process each AP + for (const auto _ap : mapAPs) { + const Floorplan::AccessPoint* ap = _ap.first; + const Floorplan::Floor* floor = _ap.second; + const MACAddress mac(ap->mac); + const Point3 pos_m = ap->getPos(floor); + res.model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params + K::Statistics err = getError(mac, res.model); + res.errAvg.add(err.getAvg()); + res.errSingle.add(err); + } + + return res; + + }; + + // to-be-optimized function: reduce average error among all fingerprints/AP's + auto optFunc = [&] (const float* params) { + return errFunc(params[0], params[1], params[2]).errSingle.getAvg(); + }; + + // use simplex + float params[3] = {-40, 2, -8}; + K::NumOptAlgoDownhillSimplex opt(3); + opt.setMaxIterations(40); + opt.setNumRestarts(20); + opt.calculateOptimum(optFunc, params); + +// // use genetic +// K::NumOptAlgoGenetic opt(3); +// opt.setPopulationSize(100); +// opt.setMaxIterations(100); +// opt.setValRange({1, 0.1, 0.2}); +// opt.setElitism(0.05f); +// opt.setMutation(0.25); +// opt.calculateOptimum(optFunc, params); + + // get the error result for all APs + Result res = errFunc(params[0], params[1], params[2]); + + // done + dump(res); + return res; + + } + + Result fixedPosOptParamsForEach() { + + Result res(map); + + // process each AP + for (const auto _ap : mapAPs) { + + // fixed AP params + const Floorplan::AccessPoint* ap = _ap.first; + const Floorplan::Floor* floor = _ap.second; + const MACAddress mac(ap->mac); + const Point3 pos_m = ap->getPos(floor); + const std::vector fps = getFingerprints(mac); + + // resulting error using the given txp,exp,waf for ALL APs + auto errFunc = [&] (const float txp, const float exp, const float waf) -> K::Statistics { + WiFiModelLogDistCeiling model(map); // new, empty model for this AP + model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params + K::Statistics err = getError(mac, model, fps); // calculate error among all fingerprints + return err; + }; + + // to-be-optimized function: reduce average error among all fingerprints/AP's + auto optFunc = [&] (const float* params) { + return errFunc(params[0], params[1], params[2]).getAvg(); + }; + + // use simplex + float params[3] = {-40, 2, -8}; + K::NumOptAlgoDownhillSimplex opt(3); + opt.setMaxIterations(40); + opt.setNumRestarts(20); + opt.calculateOptimum(optFunc, params); + +// // use genetic +// K::NumOptAlgoGenetic opt(3); +// opt.setPopulationSize(100); +// opt.setMaxIterations(100); +// opt.setValRange({1, 0.1, 0.2}); +// opt.setElitism(0.05f); +// opt.setMutation(0.25); +// opt.calculateOptimum(optFunc, params); + + // get the error result for the current AP + K::Statistics err = errFunc(params[0], params[1], params[2]); + res.errAvg.add(err.getAvg()); + res.errSingle.add(err); + + // add finalized params to the model + res.model.addAP(mac, pos_m, params[0], params[1], params[2], false); + + } + + // done + dump(res); + return res; + + } + + Result optPosOptParamsForEach() { + + Result res(map); + + // process each AP + for (const auto _ap : mapAPs) { + + // fixed AP params + const Floorplan::AccessPoint* ap = _ap.first; + const MACAddress mac(ap->mac); + const std::vector fps = getFingerprints(mac); + + // resulting error using the given txp,exp,waf for ALL APs + auto errFunc = [&] (const Point3 pos_m, const float txp, const float exp, const float waf) -> K::Statistics { + WiFiModelLogDistCeiling model(map); // new, empty model for this AP + model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params + K::Statistics err = getError(mac, model, fps); // calculate error among all fingerprints + return err; + }; + + // to-be-optimized function: reduce average error among all fingerprints/AP's + auto optFunc = [&] (const float* params) { + const Point3 pos_m(params[0], params[1], params[2]); + return errFunc(pos_m, params[3], params[4], params[5]).getAvg(); // AVG + }; + + // params + float params[6] = {0}; + + std::minstd_rand gen; + BBox3 mapBBox = FloorplanHelper::getBBox(map); + std::uniform_real_distribution distX(mapBBox.getMin().x, mapBBox.getMax().x); + std::uniform_real_distribution distY(mapBBox.getMin().y, mapBBox.getMax().y); + std::uniform_real_distribution distZ(mapBBox.getMin().z, mapBBox.getMax().z); + + // initializer for the optimizer: random position within the map's bbox + auto init = [&] (const int childIdx, float* params) { + (void) childIdx; + params[0] = distX(gen); + params[1] = distY(gen); + params[2] = distZ(gen); + params[3] = -40; + params[4] = 2; + params[5] = -8; + }; + +// // use genetic +// K::NumOptAlgoGenetic opt(6); +// opt.setPopulationSize(300); +// opt.setMaxIterations(50); +// opt.setValRange({1, 1, 1, 1, 0.1, 0.2}); +// opt.setElitism(0.05f); +// opt.setMutation(0.25); +// opt.calculateOptimum(optFunc, params, init); + + using LeOpt = K::NumOptAlgoRangeRandom; + const std::vector 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 + LeOpt::MinMax(-50, -30), // txp + LeOpt::MinMax( 1, 4), // exp + LeOpt::MinMax(-15, -0), // waf + }; + + K::NumOptAlgoRangeRandom opt(valRegion); + opt.setPopulationSize(300); + opt.setNumIerations(75); + opt.calculateOptimum(optFunc, params); + + + +// K::NumOptAlgoDownhillSimplex opt(6); +// opt.setMaxIterations(40); +// opt.setNumRestarts(20); +// opt.calculateOptimum(optFunc, params); + + // get the error result for the current AP + const Point3 pos_m(params[0], params[1], params[2]); + K::Statistics err = errFunc(pos_m, params[3], params[4], params[5]); + res.errAvg.add(err.getAvg()); + res.errSingle.add(err); + + // add finalized params to the model + res.model.addAP(mac, pos_m, params[3], params[4], params[5], false); + + } + + // done + dump(res); + return res; + + } + + + + +private: + + std::vector getFingerprints(const MACAddress& mac) { + + // get all fingerprints where the given mac was seen + const std::vector fps = calib.getFingerprintsFor(mac); + + // sanity check +// if (fps.size() < 5) {throw Exception("not enought fingerprints for AP " + mac.asString());} + + return fps; + + } + + + + void dump(const Result& res) { + std::cout << res.errSingle.asString() << std::endl; + std::cout << res.errAvg.asString() << std::endl; + std::cout << "------------------------------------------------------" << std::endl; + } + + /** calculate the error for the given AP [mac + configured model] for all fingerprints for this mac */ + K::Statistics getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model) { + + // get all fingerprints where the given mac was seen + const std::vector fps = getFingerprints(mac); + + // fire + return getError(mac, model, fps); + + } + + /** calculate the error for the given AP [mac + configured model] for all of the given FPS */ + K::Statistics getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model, const std::vector& fps) { + + K::Statistics res; + + // calculate the model error for each fingerprint + for (const WiFiFingerprint& fp : fps) { + + // sanity checks + if (fp.measurements.entries.size() != 1) {throw Exception("invalid fingerprint");} + if (fp.measurements.entries.front().getAP().getMAC() != mac) {throw Exception("invalid fingerprint");} + + // rssi prediction from the configured model + const float model_rssi = model.getRSSI(mac, fp.pos_m); + + // sanity check + if (model_rssi != model_rssi) {throw Exception("model rssi not available for " + mac.asString());} + + // rssi measured during scan at this location + const float scan_rssi = fp.measurements.entries.front().getRSSI(); + + // sanity check + if (scan_rssi < -100) {throw Exception("scan rssi out of range for " + mac.asString());} + if (scan_rssi > -35) {throw Exception("scan rssi out of range for " + mac.asString());} + + // difference + //const float err = model_rssi - scan_rssi; + const float err = -std::log(Distribution::Normal::getProbability(model_rssi, 7, scan_rssi)); + float aErr = std::abs(err); + + // penalty +// if (model.getAP(mac).waf > -2) {aErr += 9999;} +// if (model.getAP(mac).txp > -30) {aErr += 9999;} +// if (model.getAP(mac).txp < -50) {aErr += 9999;} + + + res.add(aErr); + + } + + // done + return res; + + } + + +}; + +#endif // EVALCOMPAREOPT2_H diff --git a/EvalWiFi.h b/EvalWiFi.h index f078a52..1eb9ed4 100644 --- a/EvalWiFi.h +++ b/EvalWiFi.h @@ -25,7 +25,10 @@ #include #include "Structs.h" -#include "Plotty.h" +#include "plots/Plotty.h" +#include "plots/PlotErrTime.h" +#include "plots/PlotErrFunc.h" + #include "CSV.h" #include @@ -89,6 +92,16 @@ private: WiFiModel* wiModel = nullptr; std::vector gtIndices; + // error in meter + PlotErrFunc* pef; + PlotErrTime* pet; + + // error in probability + PlotErrFunc* pef2; + PlotErrTime* pet2; + + Plotty* plot; + public: /** ctor with map and fingerprints */ @@ -108,30 +121,70 @@ public: // the optimizer // opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM); + // how to handle VAPs + vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); + + pef = new PlotErrFunc("\\small{error (m)}", "\\small{measurements (\\%)}"); + pef->showMarkers(false); + + pef2 = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}"); + pef2->showMarkers(false); + + pet = new PlotErrTime("2", "3", ""); + pet->getPlot().setRangeY(K::GnuplotAxisRange(0, 40)); + + pet2 = new PlotErrTime("2", "3", ""); + + plot = new Plotty(map); + plot->buildFloorplan(); + plot->setGroundTruth(gtIndices); + } - void fixedParams(const float txp, const float exp, const float waf) { - - // how to handle VAPs - vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + void load(const std::string& xmlFile, const std::string& name) { // setup the model WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map); - wiModel->loadAPs(map, *vap, txp, exp, waf, false); + wiModel->loadXML(xmlFile); this->wiModel = wiModel; // fire - run(); + build(name); } +// void fixedParams(const float txp, const float exp, const float waf) { + +// // how to handle VAPs +// vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + +// // setup the model +// WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map); +// wiModel->loadAPs(map, *vap, txp, exp, waf, false); +// this->wiModel = wiModel; + +// // fire +// run(); + +// } + private: - void run() { + void build(const std::string& name) { + + static int idx = -1; ++idx; + + + const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices); - Plotty* plot = new Plotty(map); Line path; - plot->setGroundTruth(gtIndices); + K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines(); + plot->splot.add(gpPath); + + K::Statistics* stats = new K::Statistics(); + K::Statistics* statsProbOnGT = new K::Statistics(); + pef->add(name, stats); + pef2->add(name, statsProbOnGT); // process each wifi entry within the offline file for (const auto wifi : reader.wifi) { @@ -146,42 +199,71 @@ private: const WiFiMeasurements mes = vap->group(_mes); + // error calculation - auto func = [&] (float* params) -> double { + auto func = [&] (const float* params) -> double { // crop z to 1 meter - params[2] = std::round(params[2]); + //params[2] = std::round(params[2]); // suggested position const Point3 pos_m(params[0], params[1], params[2]); - const float sigma = 6.0; + 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) { + if (1 == 1) { - // skip non-FHWS APs - if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;} + // calculate error for above position using the currently available measurements + for (const WiFiMeasurement& m : mes.entries) { - // get model's rssi for the given location - const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m); + // 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::getProbability(rssi_model, sigma, rssi_scan); + //const double p = Distribution::Region::getProbability(rssi_model, sigma, rssi_scan); + + // adjust + prob *= p; - // 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(); + } else { - // likelyhood - const double p = Distribution::Normal::getProbability(rssi_model, sigma, rssi_scan); +// //const float limit = -85; - // adjust - prob *= p; +// for (const AccessPoint& ap : wiModel->getAllAPs()) { + +// // get model's rssi for the given location +// float rssi_model = wiModel->getRSSI(ap.getMAC(), pos_m); +// if (rssi_model < limit) {rssi_model = limit;} + +// // get scan's rssi +// const WiFiMeasurement* mesModel = mes.getForMac(ap.getMAC()); +// float rssi_scan = (mesModel) ? (mesModel->getRSSI()) : (limit); +// if (rssi_scan < limit) {rssi_scan = limit;} + +// // likelyhood +// const double p = Distribution::Normal::getProbability(rssi_model, sigma, rssi_scan); + +// // adjust +// prob *= p; + +// } } @@ -190,36 +272,80 @@ private: }; - std::minstd_rand gen; - std::uniform_real_distribution distX(mapBBox.getMin().x, mapBBox.getMax().x); - std::uniform_real_distribution distY(mapBBox.getMin().y, mapBBox.getMax().y); - std::uniform_real_distribution distZ(mapBBox.getMin().z, mapBBox.getMax().z); - - // initializer for the optimizer: random position within the map's bbox - auto init = [&] (const int childIdx, float* params) { - params[0] = distX(gen); - params[1] = distY(gen); - params[2] = distZ(gen); - }; - - + // parameters float params[3]; - K::NumOptAlgoGenetic opt(3); + +// std::minstd_rand gen; +// std::uniform_real_distribution distX(mapBBox.getMin().x, mapBBox.getMax().x); +// std::uniform_real_distribution distY(mapBBox.getMin().y, mapBBox.getMax().y); +// std::uniform_real_distribution distZ(mapBBox.getMin().z, mapBBox.getMax().z); + +// // initializer for the optimizer: random position within the map's bbox +// auto init = [&] (const int childIdx, float* params) { +// (void) childIdx; +// params[0] = distX(gen); +// params[1] = distY(gen); +// params[2] = distZ(gen); +// }; + +// K::NumOptAlgoGenetic opt(3); +// opt.setPopulationSize(400); +// opt.setMaxIterations(20); +// opt.calculateOptimum(func, params, init); + + + using LeOpt = K::NumOptAlgoRangeRandom; + const std::vector 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 opt(valRegion); opt.setPopulationSize(200); - opt.setMaxIterations(20); - opt.calculateOptimum(func, params, init); + opt.setNumIerations(50); + //opt.calculateOptimum(func, params); + std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl; - path.add(Point3(params[0], params[1], params[2])); + const Point3 curEst(params[0], params[1], params[2]); + path.add(curEst); - // draw a smoothed version of th epath - plot->pathEst.clear(); + const Timestamp ts = mes.entries.front().getTimestamp(); + + // draw a smoothed version of the path + gpPath->clear(); for (const Point3 p : path.getAverage(2)) { const K::GnuplotPoint3 gp3(p.x, p.y, p.z); - plot->pathEst.add(gp3); + gpPath->add(gp3); } + + // groud-truth + const Point3 gt = gtp.get(ts); + plot->gp << "set arrow 1 at " << gt.x << "," << gt.y << "," << gt.z << " to " << gt.x << "," << gt.y << "," << (gt.z+1) << "\n"; + + // error + //const float err_m = gt.xy().getDistance(curEst.xy()); + const float err_m = gt.getDistance(curEst); + stats->add(err_m); + + float gtFloat[3] = {gt.x, gt.y, gt.z}; + const double probOnGT = -func(gtFloat); + const double logProbOnGT = -log(probOnGT); + statsProbOnGT->add(logProbOnGT); + + // plot err + pet->addErr(ts, err_m, idx); + pet2->addErr(ts, logProbOnGT/mes.entries.size(), idx); + + // fire plot->plot(); +// pet->plot(); +// pef->plot(); + + pet2->plot(); + pef2->plot(); } diff --git a/EvalWiFiSigStrength.h b/EvalWiFiSigStrength.h index 730665d..16ba02b 100644 --- a/EvalWiFiSigStrength.h +++ b/EvalWiFiSigStrength.h @@ -26,7 +26,7 @@ #include #include "Structs.h" -#include "Plotty.h" +#include "plots/Plotty.h" #include "CSV.h" #include "Helper.h" diff --git a/Helper.h b/Helper.h index c559980..c628a32 100644 --- a/Helper.h +++ b/Helper.h @@ -6,7 +6,7 @@ #include #include -#include "Plotty.h" +#include "plots/Plotty.h" class LeHelper { @@ -28,31 +28,69 @@ public: } + static inline bool isOutdoor(const WiFiFingerprint& fp) { + + const Point3 pos = fp.pos_m; + + // garden + if (pos == Point3(21, 12, 1.3)) {return true;} + if (pos == Point3(21, 27, 1.3)) {return true;} + if (pos == Point3(39, 12, 1.3)) {return true;} + if (pos == Point3(39, 27, 1.3)) {return true;} + if (pos == Point3(67, 12, 1.3)) {return true;} + if (pos == Point3(67, 27, 1.3)) {return true;} + if (pos == Point3(88, 12, 1.3)) {return true;} + if (pos == Point3(88, 27, 1.3)) {return true;} + + // front door + if (pos == Point3(94, 43, 4+1.3)) {return true;} + if (pos == Point3(110, 43, 4+1.3)) {return true;} + if (pos == Point3(94, 55, 4+1.3)) {return true;} + if (pos == Point3(110, 55, 4+1.3)) {return true;} + if (pos == Point3(67.2, 55, 4+1.3)) {return true;} + + return false; + + } + + static inline bool isStaircase(const WiFiFingerprint& fp) { + + const Point3 pos = fp.pos_m; + + // floor 0 + if (pos == Point3(67.2, 48.5, 1.3)) {return true;} + if (pos == Point3(11.2, 48.3, 1.3)) {return true;} + if (pos == Point3(16.5, 11.9, 1.3)) {return true;} + if (pos == Point3(57.4, 2.6, 1.3)) {return true;} + if (pos == Point3(57.4, -3.2, 3.0)) {return true;} + + // floor 1 + if (pos == Point3(67.2, 48.3, 4+1.3)) {return true;} + if (pos == Point3(11.4, 48.3, 4+1.3)) {return true;} + if (pos == Point3(16.1, 11.9, 4+1.3)) {return true;} + if (pos == Point3(57.4, 2.4, 4+1.3)) {return true;} + + // floor 2 + if (pos == Point3(67.0, 48.5, 3.4+4+1.3)) {return true;} + if (pos == Point3(11.6, 48.5, 3.4+4+1.3)) {return true;} + if (pos == Point3(16.3, 11.9, 3.4+4+1.3)) {return true;} + + // floor 3 + if (pos == Point3(67.0, 48.3, 3.4+3.4+4+1.3)) {return true;} + if (pos == Point3(11.4, 48.5, 3.4+3.4+4+1.3)) {return true;} + if (pos == Point3(16.3, 11.9, 3.4+3.4+4+1.3)) {return true;} + + return false; + + } + /** remove all outdoor fingerprints */ static WiFiFingerprints removeOutdoor(const WiFiFingerprints& inp) { WiFiFingerprints res; for (const WiFiFingerprint& fp : inp.getFingerprints()) { - const Point3 pos = fp.pos_m; - - // garden - if (pos == Point3(21, 12, 1.3)) {continue;} - if (pos == Point3(21, 27, 1.3)) {continue;} - if (pos == Point3(39, 12, 1.3)) {continue;} - if (pos == Point3(39, 27, 1.3)) {continue;} - if (pos == Point3(67, 12, 1.3)) {continue;} - if (pos == Point3(67, 27, 1.3)) {continue;} - if (pos == Point3(88, 12, 1.3)) {continue;} - if (pos == Point3(88, 27, 1.3)) {continue;} - - // front door - if (pos == Point3(94, 43, 4+1.3)) {continue;} - if (pos == Point3(110, 43, 4+1.3)) {continue;} - if (pos == Point3(94, 55, 4+1.3)) {continue;} - if (pos == Point3(110, 55, 4+1.3)) {continue;} - if (pos == Point3(67.2, 55, 4+1.3)) {continue;} - + if (isOutdoor(fp)) {continue;} res.add(fp); } @@ -65,38 +103,52 @@ public: WiFiFingerprints res; for (const WiFiFingerprint& fp : inp.getFingerprints()) { - const Point3 pos = fp.pos_m; - - // floor 0 - if (pos == Point3(67.2, 48.5, 1.3)) {continue;} - if (pos == Point3(11.2, 48.3, 1.3)) {continue;} - if (pos == Point3(16.5, 11.9, 1.3)) {continue;} - if (pos == Point3(57.4, 2.6, 1.3)) {continue;} - if (pos == Point3(57.4, -3.2, 3.0)) {continue;} - - // floor 1 - if (pos == Point3(67.2, 48.3, 4+1.3)) {continue;} - if (pos == Point3(11.4, 48.3, 4+1.3)) {continue;} - if (pos == Point3(16.1, 11.9, 4+1.3)) {continue;} - if (pos == Point3(57.4, 2.4, 4+1.3)) {continue;} - - // floor 2 - if (pos == Point3(67.0, 48.5, 3.4+4+1.3)) {continue;} - if (pos == Point3(11.6, 48.5, 3.4+4+1.3)) {continue;} - if (pos == Point3(16.3, 11.9, 3.4+4+1.3)) {continue;} - - // floor 3 - if (pos == Point3(67.0, 48.3, 3.4+3.4+4+1.3)) {continue;} - if (pos == Point3(11.4, 48.5, 3.4+3.4+4+1.3)) {continue;} - if (pos == Point3(16.3, 11.9, 3.4+3.4+4+1.3)) {continue;} - + if (isStaircase(fp)) {continue;} res.add(fp); - } return res; } + static WiFiFingerprints removeIndoor(const WiFiFingerprints& inp) { + + WiFiFingerprints res; + + for (const WiFiFingerprint& fp : inp.getFingerprints()) { + if (!isStaircase(fp) && !isOutdoor(fp)) {continue;} + res.add(fp); + } + + return res; + } + + + static WiFiFingerprints removeIf(const WiFiFingerprints& inp, std::function remove) { + + WiFiFingerprints res; + + for (const WiFiFingerprint& fp : inp.getFingerprints()) { + if (remove(fp)) {continue;} + res.add(fp); + } + + return res; + } + + + static WiFiFingerprints onlyOutdoor(const WiFiFingerprints& inp) { + + WiFiFingerprints res; + + for (const WiFiFingerprint& fp : inp.getFingerprints()) { + if (!isOutdoor(fp)) {continue;} + res.add(fp); + } + + return res; + + } + static void plot(const Floorplan::IndoorMap* map, const WiFiFingerprints& inp, const std::string& title = "Fingerprints") { Plotty* p = new Plotty(map); diff --git a/PlotErrFunc.h b/PlotErrFunc.h deleted file mode 100644 index 4a926e4..0000000 --- a/PlotErrFunc.h +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef PLOTERRFUNC_H -#define PLOTERRFUNC_H - -#include - -#include -#include -#include - - -class PlotErrFunc { - - - - struct Entry { - std::string name; - K::Statistics stats; - Entry(const std::string& name, const K::Statistics& stats) : name(name), stats(stats) {;} - }; - - std::vector entries; - std::vector lines; - - K::Gnuplot gp; - K::GnuplotPlot gplot; - - //std::vector colors = {"#000000", "#ff0000", "#00bb00", "#0000ff"}; - std::vector colors = {"#000000", "#999999", "#0000ff", "#9999ff"}; - - std::string codeFile; - -public: - - /** ctor with x-axis label */ - PlotErrFunc(const std::string& xLabel, const std::string& yLabel) { - gplot.setLabelX(xLabel); - gplot.setLabelY(yLabel); - } - - /** add one curve */ - void add(const std::string name, const K::Statistics stats) { - entries.push_back(Entry(name, stats)); - K::GnuplotPlotElementLines* gpel = new K::GnuplotPlotElementLines(); - gpel->setTitle(name); - gpel->setLineWidth(2); - lines.push_back(gpel); - gplot.add(gpel); - } - - void clear() { - entries.clear(); - } - - K::Gnuplot& getGP() { - return gp; - } - - void writeCodeTo(const std::string& file) { - this->codeFile = file; - } - - /** plot all curves */ - void plot() { - - gp << "set grid\n"; - - for (size_t i = 0; i < entries.size(); ++i) { - - const Entry e = entries[i]; - K::GnuplotPlotElementLines* line = lines[i]; - - line->clear(); - //line.setTitle(e.name); - line->setColorHex(colors[i]); - - // 0 - 80% - for (int i = 0; i <= 85; i+= 5) { - const float q = i / 100.0f; - const float y = e.stats.getQuantile(q); - K::GnuplotPoint2 gp(y, q*100); - line->add(gp); - } - - } - - gp.draw(gplot); - - if (codeFile != "") { - std::ofstream out(codeFile); - out << gp.getBuffer(); - out.close(); - } - - gp.flush(); - - } - -}; - -#endif // PLOTERRFUNC_H diff --git a/Settings.h b/Settings.h index 8776682..845bbe2 100644 --- a/Settings.h +++ b/Settings.h @@ -20,6 +20,7 @@ namespace Settings { const std::string wifiAllOptPar = pathWiFi + "allOptPar.xml"; const std::string wifiEachOptPar = pathWiFi + "eachOptPar.xml"; const std::string wifiEachOptParPos = pathWiFi + "eachOptParPos.xml"; + const std::string wifiEachOptParPosNoStairNoOut = pathWiFi + "eachOptParPosNoStairNoOut.xml"; @@ -33,10 +34,10 @@ namespace Settings { float smartphoneAboveGround = 1.3; namespace IMU { - const float turnSigma = 1.5 + 0.5;//1.5; - const float stepLength = 0.85; - const float stepSigma = 0.40; - const float absHeadSigma = 90; + const float turnSigma = 1.25;//1.5; + const float stepLength = 0.70; + const float stepSigma = 0.45; + const float absHeadSigma = 80; } namespace Grid { @@ -57,19 +58,19 @@ namespace Settings { namespace WiFiModel { - constexpr float sigma = 8.0; + constexpr float sigma = 11.0; /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ - constexpr float TXP = -44; - constexpr float EXP = 2.5; - constexpr float WAF = -8.0; +// constexpr float TXP = -44; +// constexpr float EXP = 2.5; +// constexpr float WAF = -8.0; // constexpr float TXP = -64.5896; // constexpr float EXP = 1.25986; // constexpr float WAF = -2.47467; // how to perform VAP grouping - const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); - const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); } diff --git a/main.cpp b/main.cpp index e264848..3b0cfa1 100644 --- a/main.cpp +++ b/main.cpp @@ -21,20 +21,23 @@ #include "Settings.h" #include "EvalCompareOpt.h" +#include "EvalCompareOpt2.h" + #include "EvalApOpt.h" #include "EvalData.h" #include "EvalWiFi.h" #include "EvalWiFiSigStrength.h" #include "pf/EvalWalk.h" -#include "PlotErrFunc.h" +#include "plots/PlotErrFunc.h" // build plots for the paper void paperOutputs() { Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap); - if (1==1){ + // show fingerprints as plot + if (1==0){ EvalWiFiSigStrength sig(Settings::fMap, Settings::fCalib); Plotty* p = new Plotty(map); p->writeCodeTo(Settings::fPathGFX + "compare-wifi-in-out.gp"); @@ -52,22 +55,48 @@ void paperOutputs() { delete p; } - if (1==1) { + // perform varios AP-param optimizations + // generate error plot showing the performance of each + // save the resulting wifi-models to XML for later re-use during the walk-eval <<<<<< !!!! + if (1 == 1) { - const bool ignoreStaircases = false; - const bool ignoreOutdoor = false; - EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor); +// // use walks? +// std::vector>> calibWalks = { +// std::make_pair(Settings::path1a, Settings::GroundTruth::path1), +// std::make_pair(Settings::path1b, Settings::GroundTruth::path1), +// std::make_pair(Settings::path2a, Settings::GroundTruth::path2), +// std::make_pair(Settings::path2b, Settings::GroundTruth::path2), +// }; +// EvalCompareOpt2 opt1(Settings::fMap, calibWalks); - K::Statistics s1 = allFixed.fixedPosFixedParamsForAll(); //BREAK; - K::Statistics s2 = allFixed.fixedPosOptParamsForAll(); //BREAK; - K::Statistics s3 = allFixed.fixedPosOptParamsForEach(); //BREAK; - K::Statistics s4 = allFixed.optPosOptParamsForEach(); //BREAK; + auto remove = [] (const WiFiFingerprint& fp) -> bool { + return fp.pos_m.z != 4; + }; + + // use fingerprints? + bool ignoreStaircases = false; + bool ignoreOutdoor = false; + bool ignoreIndoor = false; + EvalCompareOpt2 opt1(Settings::fMap, Settings::fCalib, remove); + + + EvalCompareOpt2::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK; + EvalCompareOpt2::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK; + EvalCompareOpt2::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK; + EvalCompareOpt2::Result s4 = opt1.optPosOptParamsForEach(); //BREAK; + + + // save models to file + s1.model.saveXML(Settings::wifiAllFixed); + s2.model.saveXML(Settings::wifiAllOptPar); + s3.model.saveXML(Settings::wifiEachOptPar); + s4.model.saveXML(Settings::wifiEachOptParPos); PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}"); - pef.add("\\small{empiric}", s1); - pef.add("\\small{real pos, opt params}", s2); - pef.add("\\small{real pos, opt params [per AP]}", s3); - pef.add("\\small{opt pos, opt params [per AP]}", s4); + pef.add("\\small{empiric}", &s1.errSingle); + pef.add("\\small{real pos, opt params}", &s2.errSingle); + pef.add("\\small{real pos, opt params [per AP]}", &s3.errSingle); + pef.add("\\small{opt pos, opt params [per AP]}", &s4.errSingle); pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5)); pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-methods.tex"); @@ -77,25 +106,87 @@ void paperOutputs() { pef.getGP() << "set tmargin 0.4\n"; pef.plot(); + return; + //sleep(1000); + + + } - // error histogram all pos, all params, between in/out/stair, in/out, in/stair, in - if(1==1){ - EvalCompareOptAllFixed e1(Settings::fMap, Settings::fCalib, false, false); - EvalCompareOptAllFixed e2(Settings::fMap, Settings::fCalib, true, false); - EvalCompareOptAllFixed e3(Settings::fMap, Settings::fCalib, false, true); - EvalCompareOptAllFixed e4(Settings::fMap, Settings::fCalib, true, true); + // REPLACED BY EVAL OPT 2 +// // perform varios AP-param optimizations +// // generate error plot showing the performance of each +// // save the resulting wifi-models to XML for later re-use during the walk-eval <<<<<< !!!! +// if (1 == 0) { - K::Statistics s1 = e1.optPosOptParamsForEach(); - K::Statistics s2 = e2.optPosOptParamsForEach(); - K::Statistics s3 = e3.optPosOptParamsForEach(); - K::Statistics s4 = e4.optPosOptParamsForEach(); +// bool ignoreStaircases = false; +// bool ignoreOutdoor = false; +// bool ignoreIndoor = false; + +// // use walks? +//// std::vector>> calibWalks = { +//// std::make_pair(Settings::path1a, Settings::GroundTruth::path1), +//// std::make_pair(Settings::path1b, Settings::GroundTruth::path1), +//// std::make_pair(Settings::path2a, Settings::GroundTruth::path2), +//// std::make_pair(Settings::path2b, Settings::GroundTruth::path2), +//// }; +//// EvalCompareOpt opt1(Settings::fMap, calibWalks); + +// // use fingerprints? +// EvalCompareOpt opt1(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); + +// EvalCompareOpt::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK; +// EvalCompareOpt::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK; +// EvalCompareOpt::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK; +// EvalCompareOpt::Result s4 = opt1.optPosOptParamsForEach(); //BREAK; + +// ignoreStaircases = true; +// ignoreOutdoor = true; +// EvalCompareOpt opt2(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); +// EvalCompareOpt::Result s4b = opt2.optPosOptParamsForEach(); //BREAK; + +// // save models to file +// s1.model.saveXML(Settings::wifiAllFixed); +// s2.model.saveXML(Settings::wifiAllOptPar); +// s3.model.saveXML(Settings::wifiEachOptPar); +// s4.model.saveXML(Settings::wifiEachOptParPos); +// s4b.model.saveXML(Settings::wifiEachOptParPosNoStairNoOut); + +// PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}"); +// pef.add("\\small{empiric}", s1.errAbs); +// pef.add("\\small{real pos, opt params}", s2.errAbs); +// pef.add("\\small{real pos, opt params [per AP]}", s3.errAbs); +// pef.add("\\small{opt pos, opt params [per AP]}", s4.errAbs); +// pef.add("\\small{opt pos, opt params [per AP, no stair, no out]}", s4b.errAbs); + +// pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5)); +// pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-methods.tex"); +// pef.writeCodeTo(Settings::fPathGFX + "wifi-opt-error-hist-methods.gp"); +// pef.getGP() << "set key right bottom width -4 samplen 0.5\n"; +// pef.getGP() << "set rmargin 0.4\n"; +// pef.getGP() << "set tmargin 0.4\n"; +// pef.plot(); + +// } + + // error histogram all pos, all params, between in/out/stair, in/out, in/stair, in + if(1==0){ + + EvalCompareOpt e1(Settings::fMap, Settings::fCalib, false, false, false); + EvalCompareOpt e2(Settings::fMap, Settings::fCalib, true, false, false); + EvalCompareOpt e3(Settings::fMap, Settings::fCalib, false, true, false); + EvalCompareOpt e4(Settings::fMap, Settings::fCalib, true, true, false); + + K::Statistics s1 = e1.optPosOptParamsForEach().errAbs; + K::Statistics s2 = e2.optPosOptParamsForEach().errAbs; + K::Statistics s3 = e3.optPosOptParamsForEach().errAbs; + K::Statistics s4 = e4.optPosOptParamsForEach().errAbs; PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}"); - pef.add("\\small{floor + stairs + out}", s1); - pef.add("\\small{floor + out}", s2); - pef.add("\\small{floor + stairs}", s3); - pef.add("\\small{floor}", s4); + pef.add("\\small{floor + stairs + out}", &s1); + pef.add("\\small{floor + out}", &s2); + pef.add("\\small{floor + stairs}", &s3); + pef.add("\\small{floor}", &s4); pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5)); pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-stair-outdoor.tex"); @@ -107,14 +198,106 @@ void paperOutputs() { } + // wifi issue for path1 + if (1 == 0) { + + Offline::FileReader reader(Settings::path1a); + PlotWifiMeasurements plot; + + for (int i = 0; i < 60; ++i) { + const WiFiMeasurements mes = reader.getWiFiGroupedByTime().at(i).data; + const WiFiMeasurements mes2 = Settings::WiFiModel::vg_eval.group(mes); + plot.add(mes2); + } + + K::GnuplotObjectRectangle rect( + K::GnuplotCoordinate2(0, K::GnuplotCoordinateSystem::FIRST, 0, K::GnuplotCoordinateSystem::GRAPH), + K::GnuplotCoordinate2(10, K::GnuplotCoordinateSystem::FIRST, 1, K::GnuplotCoordinateSystem::GRAPH), + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromRGB(128,128,128), 0.5), + K::GnuplotStroke() + ); + + plot.getPlot().setGrid(true); + plot.getPlot().setLegend(false); + plot.getPlot().getObjects().add(&rect); + plot.plot(); + + sleep(100); + + } + +} + +void testWAF() { + + Floorplan::Ceilings ceilings; + ceilings.addCeiling(3); + ceilings.addCeiling(6); + ceilings.addCeiling(9); + ceilings.addCeiling(12); + + K::Gnuplot gp; + K::GnuplotPlot gplot; + K::GnuplotPlotElementLines lines; gplot.add(&lines); + + const Point3 posAP(0, 0, 8); + + for (float z = 0; z < 15; z += 0.1) { + + const Point3 posMe(0, 0, z); + float factor = ceilings.numCeilingsBetweenFloat(posAP, posMe); + lines.add({z, factor}); + + } + + gp.draw(gplot); + gp.flush(); + + sleep(1000); + + } int main(void) { -// paperOutputs(); return 0; + //testWAF(); - if (1 == 1) { + //paperOutputs(); // return 0; + + // calib error in/out + if (1 == 0) { + + // outdoor only + bool ignoreStaircases = true; + bool ignoreOutdoor = false; + bool ignoreIndoor = true; + EvalCompareOpt opt1(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); + EvalCompareOpt::Result s1 = opt1.optPosOptParamsForEach(); + std::cout << s1.errAbs.asString() << std::endl; + + // stairs only + ignoreStaircases = false; + ignoreOutdoor = true; + ignoreIndoor = true; + EvalCompareOpt opt3(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); + EvalCompareOpt::Result s3 = opt3.optPosOptParamsForEach(); + std::cout << s3.errAbs.asString() << std::endl; + + // indoor only + ignoreStaircases = true; + ignoreOutdoor = true; + ignoreIndoor = false; + EvalCompareOpt opt2(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); + EvalCompareOpt::Result s2 = opt2.optPosOptParamsForEach(); + std::cout << s2.errAbs.asString() << std::endl; + + + + } + + // walks + if (1 == 0) { Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);; EvalWalk walk(map); walk.walk1(); @@ -139,12 +322,19 @@ int main(void) { // test wifi within data files - if (1 == 0) { + if (1 == 1) { + EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1); //EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2); - EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2); //ew1.fixedParams(-40, 2.5, -8); BREAK; //ew1.fixedParams(-64.5905, 1.25988, -2.47863); BREAK; - ew1.fixedParams(-59.4903,1.52411,-3.25077); BREAK; + //ew1.fixedParams(-59.4903,1.52411,-3.25077); BREAK; + //ew1.load(Settings::wifiEachOptParPos); + ew1.load(Settings::wifiAllFixed, "empirc"); + ew1.load(Settings::wifiAllOptPar, "opt params all APs"); + ew1.load(Settings::wifiEachOptPar, "opt params each AP"); + ew1.load(Settings::wifiEachOptParPos, "everything opt"); + + sleep(1000); } @@ -161,22 +351,23 @@ int main(void) { // compare wifi opt methods - if (1 == 1) { + if (1 == 0) { const bool ignoreStaircases = false; const bool ignoreOutdoor = false; - EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor); + const bool ignoreIndoor = false; + EvalCompareOpt opt(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor); - K::Statistics s1 = allFixed.fixedPosFixedParamsForAll(); //BREAK; - K::Statistics s2 = allFixed.fixedPosOptParamsForAll(); //BREAK; - K::Statistics s3 = allFixed.fixedPosOptParamsForEach(); //BREAK; - K::Statistics s4 = allFixed.optPosOptParamsForEach(); //BREAK; + EvalCompareOpt::Result s1 = opt.fixedPosFixedParamsForAll(); //BREAK; + EvalCompareOpt::Result s2 = opt.fixedPosOptParamsForAll(); //BREAK; + EvalCompareOpt::Result s3 = opt.fixedPosOptParamsForEach(); //BREAK; + EvalCompareOpt::Result s4 = opt.optPosOptParamsForEach(); //BREAK; PlotErrFunc pef("error (dB)", "fingerprints (%)"); - pef.add("empiric", s1); - pef.add("real pos, opt params [same for all]", s2); - pef.add("real pos, opt params [for each]", s3); - pef.add("opt pos, opt params [for each]", s4); + pef.add("empiric", &s1.errAbs); + pef.add("real pos, opt params [same for all]", &s2.errAbs); + pef.add("real pos, opt params [for each]", &s3.errAbs); + pef.add("opt pos, opt params [for each]", &s4.errAbs); pef.plot(); } @@ -184,21 +375,21 @@ int main(void) { // compare leaving out fingerprints if (1 == 0) { - EvalCompareOptAllFixed e1(Settings::fMap, Settings::fCalib, false, false); - EvalCompareOptAllFixed e2(Settings::fMap, Settings::fCalib, true, false); - EvalCompareOptAllFixed e3(Settings::fMap, Settings::fCalib, false, true); - EvalCompareOptAllFixed e4(Settings::fMap, Settings::fCalib, true, true); + EvalCompareOpt e1(Settings::fMap, Settings::fCalib, false, false, false); + EvalCompareOpt e2(Settings::fMap, Settings::fCalib, true, false, false); + EvalCompareOpt e3(Settings::fMap, Settings::fCalib, false, true, false); + EvalCompareOpt e4(Settings::fMap, Settings::fCalib, true, true, false); - K::Statistics s1 = e1.optPosOptParamsForEach(); - K::Statistics s2 = e2.optPosOptParamsForEach(); - K::Statistics s3 = e3.optPosOptParamsForEach(); - K::Statistics s4 = e4.optPosOptParamsForEach(); + K::Statistics s1 = e1.optPosOptParamsForEach().errAbs; + K::Statistics s2 = e2.optPosOptParamsForEach().errAbs; + K::Statistics s3 = e3.optPosOptParamsForEach().errAbs; + K::Statistics s4 = e4.optPosOptParamsForEach().errAbs; PlotErrFunc pef("error (dB)", "fingerprints (%)"); - pef.add("floor + stairs + out", s1); - pef.add("floor + out", s2); - pef.add("floor + stairs", s3); - pef.add("floor", s4); + pef.add("floor + stairs + out", &s1); + pef.add("floor + out", &s2); + pef.add("floor + stairs", &s3); + pef.add("floor", &s4); pef.plot(); } diff --git a/pf/EvalWalk.h b/pf/EvalWalk.h index 8848e55..d036457 100644 --- a/pf/EvalWalk.h +++ b/pf/EvalWalk.h @@ -8,8 +8,9 @@ #include #include #include +#include -#include "../PlotErrFunc.h" +#include "../plots/PlotErrFunc.h" #include @@ -23,6 +24,7 @@ #include "Indoor/sensors/radio/VAPGrouper.h" #include "Indoor/sensors/imu/StepDetection.h" #include "Indoor/sensors/imu/TurnDetection.h" +#include "Indoor/sensors/activity/ActivityDetector.h" #include "Indoor/floorplan/v2/Floorplan.h" #include "Indoor/floorplan/v2/FloorplanReader.h" @@ -34,6 +36,9 @@ #include "Indoor/sensors/offline/FileReader.h" #include "../Helper.h" #include "PF.h" +#include "../plots/PlotErrTime.h" + +#include #include #include @@ -48,6 +53,7 @@ class EvalWalk : public Offline::Listener { WiFiModelLogDistCeiling wifiModel; Plotty plotty; + PlotWifiMeasurements plotWifi; Offline::FileReader reader; Offline::FilePlayer player; @@ -61,18 +67,24 @@ class EvalWalk : public Offline::Listener { StepDetection stepDetect; TurnDetection turnDetect; + ActivityDetector actDetect; std::vector groundTruth; Floorplan::IndoorMap* map; + EarthMapping em; + + float absHead = 0; + public: - EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map) { + EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map), em(map) { const std::string saveFile = Settings::pathData + "/grid.dat"; grid = new Grid(Settings::Grid::gridSize_cm); + // once plotty.buildFloorplan(); // deserialize grid @@ -93,17 +105,24 @@ public: pf = new K::ParticleFilter( Settings::numParticles, std::unique_ptr(new PFInit(grid)) ); // TODO: flexible model - wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false); - std::unique_ptr eval = std::unique_ptr( new PFEval(grid, wifiModel) ); - pf->setEvaluation( std::move(eval) ); + //wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false); - wifiModel.saveXML("/tmp/test.xml"); - wifiModel.loadXML("/tmp/test.xml"); + // transition + pf->setTransition( std::unique_ptr( new PFTrans(grid)) ); // resampling step? - pf->setNEffThreshold(0.5); + //pf->setNEffThreshold(0.5); //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.10)) ); + + //pf->setNEffThreshold(0.75); + //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.10)) ); + + //pf->setNEffThreshold(0.75); + //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.05)) ); + + pf->setNEffThreshold(1.0); + pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingNEff(0.50, 0.05)) ); + // state estimation step pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); @@ -117,15 +136,32 @@ public: void walk1() { - runName = "path2_forward_simple"; - std::string path = Settings::path1a; - groundTruth = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1); + // path1 + absHead = M_PI/2; + const std::string path = Settings::path1b; + const std::vector pathPoints = Settings::GroundTruth::path1; - //GridWalkSimpleControl* walk = new GridWalkSimpleControl(); - pf->setTransition( std::unique_ptr( new PFTrans(grid)) ); + // path2 +// absHead = 0; +// const std::string path = Settings::path2a; +// const std::vector pathPoints = Settings::GroundTruth::path2; + runName = ""; + + // get ground-truth + groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints); + + // wifi model + WiFiModelLogDistCeiling wifiModel(map); + wifiModel.loadXML(Settings::wifiEachOptParPos); + + // eval + std::unique_ptr eval = std::unique_ptr( new PFEval(grid, wifiModel, em) ); + pf->setEvaluation( std::move(eval) ); + + // data-file reader.open(path); - groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1); + groundTruthLive = reader.getGroundTruth(map, pathPoints); player.setReader(&reader); player.setListener(this); player.start(); @@ -152,6 +188,7 @@ public: ++curCtrl.numStepsSinceLastTransition; } gotSensorData(ts); + curCtrl.activityNew = actDetect.add(ts, data); } virtual void onGravity(const Timestamp ts, const GravityData data) override { @@ -161,6 +198,8 @@ public: virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override { std::cout << "WIFI" << std::endl; curObs.wifi = data; + plotWifi.add(Settings::WiFiModel::vg_eval.group(data)); + plotWifi.plot(); } virtual void onBarometer(const Timestamp ts, const BarometerData data) override { @@ -225,17 +264,31 @@ private: K::Statistics statsErr; + int updateCount = 0; + int getNumFHWSAPs(const WiFiMeasurements& mes) { + std::unordered_set set; + for (const WiFiMeasurement& m : mes.entries) { + std::string mac = m.getAP().getMAC().asString(); + mac.back() = '0'; + set.insert(mac); + } + return set.size(); + } /** perform a filter-update (called from a background-loop) */ void filterUpdate() { + ++updateCount; + static PlotErrTime pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}"); static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}"); + pef.showMarkers(true); std::cout << "update" << std::endl; MyControl ctrlCopy = curCtrl; - static float absHead = M_PI/2; absHead += ctrlCopy.turnSinceLastTransition_rad; + //static float absHead = relHeadingOffset; + absHead += ctrlCopy.turnSinceLastTransition_rad; //lastEst = curEst; curEst = pf->update(&curCtrl, curObs); @@ -245,25 +298,41 @@ private: plotty.setCurEst(curEst.position.inMeter()); plotty.setGroundTruth(curGT); - // error between ground-truth and estimation - const float estRealErr = curEst.position.inMeter().getDistance(curGT); - statsErr.add(estRealErr); - pef.clear(); - pef.add("", statsErr); - pef.plot(); + if (updateCount > 4) { + + // error between ground-truth and estimation + const float estRealErr = curEst.position.inMeter().getDistance(curGT); + statsErr.add(estRealErr); + pef.clear(); + pef.add("", &statsErr); + pef.plot(); + + // timed error + pet.addErr(lastTransition, estRealErr); + pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi)); + pet.plot(); + + // update estimated path + const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm); + plotty.pathEst.add(p3/100); + + } std::cout << statsErr.asString() << std::endl; - const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm); - plotty.pathEst.add(p3/100); - + // show particles + float maxWeight = 0; + float minWeight = 99; plotty.particles.clear(); for (const auto p : pf->getParticles()) { const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); - plotty.particles.add(p3/100); + plotty.particles.add(p3/100, p.weight); + if (p.weight > maxWeight) {maxWeight = p.weight;} + if (p.weight < minWeight) {minWeight = p.weight;} } + plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n"; - // GT + // show ground-truth plotty.pathReal.clear(); for (const Point3 pt : groundTruth) { plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z)); @@ -272,9 +341,12 @@ private: std::string title = " time " + std::to_string(curObs.currentTime.sec()) + " steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) + - " turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad); + " turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) + + " APs: " + std::to_string(curObs.wifi.entries.size()) + + " Act: " + std::to_string((int)curCtrl.activityNew); plotty.setTitle(title); + // relative heading and compass { Point2 cen(0.1, 0.9); Point2 dir(std::cos(absHead), std::sin(absHead)); @@ -289,7 +361,7 @@ private: // plot plotty.plot(); - std::this_thread::sleep_for(std::chrono::milliseconds(30)); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); curCtrl.resetAfterTransition(); diff --git a/pf/PF.h b/pf/PF.h index 43fae3b..47e4cad 100644 --- a/pf/PF.h +++ b/pf/PF.h @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include #include @@ -113,7 +115,9 @@ struct MyControl { // TODO: switch to a general activity enum/detector using barometer + accelerometer? /** currently detected activity */ - ActivityButterPressure::Activity activity; + ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY; + + Activity activityNew; /** reset the control-data after each transition */ void resetAfterTransition() { @@ -180,10 +184,10 @@ public: walker.addModule(&modRelHead); walker.addModule(&modAbsHead); + walker.addModule(&modActivity); //walker.addModule(&modFavorZ); //walker.addModule(&modImportance); - //walker.addModule(&modActivity); // if (Settings::destination != GridPoint(0,0,0)) { // //walker.addModule(&modDestination); @@ -215,6 +219,8 @@ public: K::Particle& p = particles[i]; + p.weight = std::pow(p.weight, 0.8); + double prob; p.state = walker.getDestination(*grid, p.state, dist_m, prob); //p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15); @@ -223,11 +229,16 @@ public: //p.weight = 1.0; // reset //p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict] //p.weight *= std::pow(prob, 0.1); // add grid-walk-probability - p.weight = prob; // grid-walk-probability + p.weight *= prob; // grid-walk-probability if (p.weight != p.weight) {throw Exception("nan");} } + // normalize + double weightSum = 0; + for (const auto& p : particles) {weightSum += p.weight;} + for (auto& p : particles) {p.weight /= weightSum;} + } }; @@ -238,6 +249,8 @@ class PFEval : public K::ParticleFilterEvaluation { WiFiModelLogDistCeiling& wifiModel; + EarthMapping& em; + WiFiObserverFree wiFiProbability; // free-calculation //WiFiObserverGrid wiFiProbability; // grid-calculation @@ -247,8 +260,8 @@ class PFEval : public K::ParticleFilterEvaluation { public: - PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : - grid(grid), wifiModel(wifiModel), + PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel, EarthMapping& em) : + grid(grid), wifiModel(wifiModel), em(em), wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free //wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid @@ -280,6 +293,7 @@ public: } + double evaluation(std::vector>& particles, const MyObservation& _observation) override { double sum = 0; @@ -292,6 +306,9 @@ public: const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi); const int numAP2 = wifiObs.entries.size(); + // GPS + const GPSProbability gpsProb(em); + Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2)); // sanity check @@ -301,6 +318,8 @@ public: for (int i = 0; i < Settings::numParticles; ++i) { K::Particle& p = particles[i]; + const MyGridNode& node = grid->getNodeFor(p.state.position); + // WiFi free const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs); @@ -309,12 +328,13 @@ public: //const MyGridNode& node = grid->getNodeFor(p.state.position); //const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs); + //const double pStair = 1;//getStairProb(p, observation.activity); + const double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps); + const double prob = pWiFi * pGPS; - //Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms())); - - const double pStair = 1;//getStairProb(p, observation.activity); - const double pGPS = 1; - const double prob = pWiFi * pGPS * pStair; + if (pGPS != 1) { + int i = 0; (void) i; + } p.weight *= prob; // NOTE: keeps the weight returned by the transition step! //p.weight = prob; // does NOT keep the weights returned by the transition step diff --git a/plots/PlotErrFunc.h b/plots/PlotErrFunc.h new file mode 100644 index 0000000..9edb94c --- /dev/null +++ b/plots/PlotErrFunc.h @@ -0,0 +1,141 @@ +#ifndef PLOTERRFUNC_H +#define PLOTERRFUNC_H + +#include + +#include +#include +#include + + +/** + * helper class to plot error development stored within Statistics. + * statistics are given as pointers and may be altered outside. + * when plot() is called, everything is rebuild using the current contents + * of each Statistics object + */ +class PlotErrFunc { + + /** group name and statistics together */ + struct Entry { + std::string name; + const K::Statistics* stats; + K::GnuplotPlotElementLines* line = nullptr; + Entry(const std::string& name, const K::Statistics* stats) : name(name), stats(stats) {;} + }; + + + std::vector entries; + + K::Gnuplot gp; + K::GnuplotPlot gplot; + + //std::vector colors = {"#000000", "#ff0000", "#00bb00", "#0000ff"}; + std::vector colors = {"#000000", "#999999", "#0000ff", "#9999ff", "#ff0000"}; + + bool markers = false; + + std::string codeFile; + + +public: + + /** ctor with x-axis label */ + PlotErrFunc(const std::string& xLabel, const std::string& yLabel) { + gplot.setLabelX(xLabel); + gplot.setLabelY(yLabel); + gplot.setRangeX(K::GnuplotAxisRange(0, K::GnuplotAxisRange::AUTO)); + gplot.setRangeY(K::GnuplotAxisRange(0, K::GnuplotAxisRange::AUTO)); + } + + /** add one curve. Statistics are allowed to be altered outside afterwards! */ + void add(const std::string name, const K::Statistics* stats) { + Entry entry(name, stats); + entry.line = new K::GnuplotPlotElementLines(); + entry.line->setTitle(name); + entry.line->setLineWidth(2); + gplot.add(entry.line); + entries.push_back(entry); + } + + /** remove all previously added entries */ + void clear() { + for (Entry& e : entries) { + gplot.remove(e.line); + delete e.line; + } + entries.clear(); + } + + K::Gnuplot& getGP() { + return gp; + } + + void writeCodeTo(const std::string& file) { + this->codeFile = file; + } + + void showMarkers(const bool show) { + this->markers = show; + } + + /** plot all curves */ + void plot() { + + gp << "set grid\n"; + + for (size_t i = 0; i < entries.size(); ++i) { + + const Entry& e = entries[i]; + + e.line->clear(); + e.line->setColorHex(colors[i]); + + // distancen between min and max + const float minX = e.stats->getQuantile(0); + const float range = e.stats->getQuantile(0.85) - minX; + const float space = range * 0.07; + + // 0 - 80% + for (int j = 0; j <= 85; j+= 5) { + const float y = j / 100.0f; + const float x = e.stats->getQuantile(y); + K::GnuplotPoint2 gp2(x, y*100); + e.line->add(gp2); + + + + if (markers) { + int id = (i+1) * 100; + if (j == 50) { + gp << "set object " << id+1 << " circle at " << x << "," << j << " size screen 0.02,0.02\n"; + gp << "set label " << id+2 << " at " << x+space << "," << j << " '" << x << "'\n"; + gp << "set arrow " << id+3 << " from " << x << "," << 0 << " to " << x << "," << j << " nohead\n"; + gp << "set arrow " << id+4 << " from " << 0 << "," << j << " to " << x << "," << j << " nohead\n"; + } else if (j == 75) { + gp << "set object " << id+5 << " circle at " << x << "," << j << " size screen 0.02,0.02\n"; + gp << "set label " << id+6 << " at " << x+space << "," << j << " '" << x << "'\n"; + gp << "set arrow " << id+7 << " from " << x << "," << 0 << " to " << x << "," << j << " nohead\n"; + gp << "set arrow " << id+8 << " from " << 0 << "," << j << " to " << x << "," << j << " nohead\n"; + } + } + + } + + } + + gp.draw(gplot); + + if (codeFile != "") { + std::ofstream out(codeFile); + out << gp.getBuffer(); + out.close(); + } + + gp.flush(); + + } + +}; + +#endif // PLOTERRFUNC_H diff --git a/plots/PlotErrTime.h b/plots/PlotErrTime.h new file mode 100644 index 0000000..31b6278 --- /dev/null +++ b/plots/PlotErrTime.h @@ -0,0 +1,71 @@ +#ifndef PLOTERRTIME_H +#define PLOTERRTIME_H + +#include +#include +#include + +#include + +/** + * plot error behaviour over time + */ +class PlotErrTime { + +private: + + K::Gnuplot gp; + K::GnuplotPlot gpplot; + K::GnuplotPlotElementLines lineErr[8]; + K::GnuplotPlotElementLines lineB; + K::GnuplotPlotElementLines lineC; + +public: + + /** ctor */ + PlotErrTime(const std::string& xLabel, const std::string& yLabel, const std::string& y2Label) { + + gpplot.setLabelX(xLabel); + gpplot.setLabelY(yLabel); + gpplot.setLabelY2(y2Label); + + gpplot.add(&lineErr[0]); lineErr[0].setLineWidth(1.5); lineErr[0].setColorHex("#000000"); + gpplot.add(&lineErr[1]); lineErr[1].setLineWidth(1.5); lineErr[1].setColorHex("#FF0000"); + gpplot.add(&lineErr[2]); lineErr[2].setLineWidth(1.5); lineErr[2].setColorHex("#00FF00"); + gpplot.add(&lineErr[3]); lineErr[3].setLineWidth(1.5); lineErr[3].setColorHex("#0000FF"); + + gpplot.add(&lineB); lineB.setLineWidth(1); lineB.setColorHex("#0000ff"); + gpplot.add(&lineC); + gpplot.setGrid(true); + + } + + K::GnuplotPlot& getPlot() { + return gpplot; + } + + /** add the give error value to the idx-th error line */ + void addErr(const Timestamp t, const float err, const int idx = 0) { + K::GnuplotPoint2 pt(t.sec(), err); + lineErr[idx].add(pt); + } + + + void addB(const Timestamp t, const float something) { + K::GnuplotPoint2 pt(t.sec(), something); + lineB.add(pt); + } + + void addC(const Timestamp t, const float something) { + K::GnuplotPoint2 pt(t.sec(), something); + lineC.add(pt); + } + + void plot() { + gp.draw(gpplot); + gp.flush(); + } + +}; + +#endif // PLOTERRTIME_H diff --git a/Plotty.h b/plots/Plotty.h similarity index 95% rename from Plotty.h rename to plots/Plotty.h index 4478011..da99fb9 100644 --- a/Plotty.h +++ b/plots/Plotty.h @@ -94,7 +94,7 @@ public: K::GnuplotSplotElementLines pathReal; K::GnuplotSplotElementLines pathEst; - K::GnuplotSplotElementPoints particles; + K::GnuplotSplotElementColorPoints particles; K::GnuplotSplotElementLines mapOutlineGlass; K::GnuplotSplotElementLines mapOutlineDrywall; @@ -117,14 +117,14 @@ public: //gp << "set view equal xy\n"; - gp << "set palette model RGB\n"; - //gp << "set palette defined (0 '#0000ff', 1 '#ff0000')\n"; - gp << "r(x) = (x < 0) ? 0 : (x/2)\n"; - gp << "g(x) = 0\n"; - gp << "b(x) = (x > 0) ? 0 : (-x/2)\n"; - gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; + //gp << "set palette model RGB\n"; + //gp << "r(x) = (x < 0) ? 0 : (x/2)\n"; + //gp << "g(x) = 0\n"; + //gp << "b(x) = (x > 0) ? 0 : (-x/2)\n"; + //gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; gp << "set ticslevel 0\n"; + // how to draw the floorplan mapOutlineConcrete.setColorHex("#888888"); mapOutlineConcrete.setLineWidth(2); mapOutlineDrywall.setColorHex("#888888"); @@ -133,7 +133,7 @@ public: splot.add(&mapOutlineDrywall); splot.add(&mapOutlineGlass); - splot.add(&particles); particles.setPointSize(0.20); particles.setColorHex("#777777"); + splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777"); splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#000000"); splot.add(&pathEst); pathEst.setLineWidth(2); pathEst.setColorHex("#0000ff"); diff --git a/tex/bare_conf.tex b/tex/bare_conf.tex index b2969ee..7215112 100644 --- a/tex/bare_conf.tex +++ b/tex/bare_conf.tex @@ -195,6 +195,8 @@ \input{chapters/relatedwork} +\input{chapters/work} + \input{chapters/experiments} \input{chapters/conclusion} diff --git a/tex/chapters/experiments.tex b/tex/chapters/experiments.tex index 51dfdc5..34c4553 100644 --- a/tex/chapters/experiments.tex +++ b/tex/chapters/experiments.tex @@ -1,10 +1,72 @@ experiments -Lorem ipsum dolor sit amet, consetetur sadipscing elitr, sed diam nonumy eirmod tempor invidunt ut labore et dolore magna aliquyam erat, sed diam voluptua. At vero eos et accusam et justo duo dolores et ea rebum. Stet clita kasd gubergren, no sea takimata sanctus est Lorem ipsum dolor sit amet. Lorem ipsum dolor sit amet, consetetur sadipscing elitr, sed diam nonumy eirmod tempor invidunt ut labore et dolore magna aliquyam erat, sed diam voluptua. At vero eos et accusam et justo duo dolores et ea rebum. Stet clita kasd gubergren, no sea takimata sanctus est Lorem ipsum dolor sit amet. + +wir betrachten nur die fest-installierten APs die man meist anhand einer bestimmten mac-range ausmachen kann +portable geraete von studenten, beamer, aehnliches werden ignoriert + +modell direkt fuer den gelaufenen pfad optimiert (also wirklich jede wifi messung direkt auf den ground-truth) +der fehler wird zwar kleiner, ist aber immernoch deutlich spürbar. das spricht dafür, dass das modell einfach nicht +gut geeignet ist. + +optimierungs input: alle 4 walks samt ground-truth +dann kommt fuer die 4 typen [fixed, all same par, each par, each par pos] +log probability 50 75, meter 50, 75 + +path1 +31.8|38.9 7.8|11.6 +27.3|36.8 7.2|9.8 +24.0|30.3 5.8|10.24 +22.9|29.9 5.0|7.6 + +hoherer fehler weil mehr outdoor anteil +path2 +32.0|42.4 12.6|20.9 +28.4|35.2 10.1|16.1 +27.0|34.0 7.0|10.1 +25.4|33.3 8.0|17.2 + + +je mehr outdoor, desto schlechter wird es. +outdoor schadet auch der optimierung +outdoor schadet mehr als indoor, weil das wifi modell fuer indoor noch halbwegs passt +aber fuer outdoor so garned + + +fenster sind metallbedampft und schirmen stark ab +siehe beispielgrafik + +gps wird so schnell nicht warm, versagt denn auf dem hof als hilfestellung + +reines wifi eval mittels num-opt springt stark durch die gegend +d.h. das bewegungsmodell rettet uns +kann man auch testen wenn man beim particle-filter das resampling ganz aus macht + \input{gfx/compare-wifi-in-out.tex} starker einfluss der glasscheiben.. 3 meter nach dem AP ist nur noch sehr wenig uebrig +ware das grid-model nicht da, wuerde der outdoor teil richtig schlecht laufen, +weil das wlan hier absolut ungenau ist.. da die partikel aber aufgrund des vorherigen +walks schon recht dicht beisamen sind, kittet das das ganze sehr gut. +kann man testen, indem man z.B. weniger resampling macht und mehr alte partikel aufhebt. +geht sofort kaputt sobald man aus dem gebäude raus kommt + +signalstaerke limitieren, wie : alles was im model oder scan < -90 ist, wird auf -90 abgeschnitten hilft +zwar an manchen stellen, im groben und ganzen führt es aber eher zu fehlern als zu verbesserungen. +zudem ist zu erwarten, dass diese zahl stark vom geraet/hardware abhaengt + +jeweils beim weighting die niedrigste wifi probability weglassen [je nach particle also ein anderer AP] +bringt auch nicht immer was.. killt gelegentlich floor-changes. zudem stehen am ende nur sehr wenige +APs zur verfügung. da einen zu ignorieren, macht noch mehr kaputt + +auch ein versuch wie werfe alle APs aus dem handy-scan weg, die kleiner -90 sind, birgt die selben risiken +es scheint wirklich am sinnvollsten, die scan-daten einfach 1:1 zu nehmen wie sie sind + + +kurz vor ende von path 2 will die estimation nicht in die cafeteria, weil ein paar particle +die treppe richtung h.1.5 hochgehen und durch das wlan sehr sehr hoch gewichtet werden. +die mittelwert-estimation versagt hier + \input{gfx/wifi-opt-error-hist-methods.tex} diff --git a/tex/chapters/work.tex b/tex/chapters/work.tex new file mode 100644 index 0000000..fd5375e --- /dev/null +++ b/tex/chapters/work.tex @@ -0,0 +1,32 @@ +log normal shadowing model +so wie log-dist modell nur mit waf. hier: fuer decken. waende werden aktuell nicht betrachtet + + + +wie wird optimiert +a) bekannte pos + empirische params +b) bekannte pos + opt params (fur alle APs gleich) [simplex] +c) bekannte pos + opt params (eigene je AP) [simplex] +d) alles opt: pos und params (je ap) [range-random] + +probleme bei der optimierung beschreiben. convex usw.. +wo geht simplex gut, wo eher nicht + +harte WAF übergänge scheinen beim optimieren als auch beim matchen nicht so gut +gleitende übergänge mittels sigmoid wirken besser +war eine wichtige erkenntnis + +die vom AP bekannte position wird NICHT als input fuer die alles-OPT funktion benutzt +die ist wirklich 'irgendwo' + +range-random algo +domain bekannt [map groesse, txp/exp/waf in etwa] +genetic refinement mit cooling [= erst grob, dann fein] + +optimierung ist tricky. auch wegen dem WAF der ja sprunghaft dazu kommt, sobald messung und AP in zwei unterschiedlichen +stockwerken liegen.. und das selbst wenn hier vlt sichtkontakt möglich wäre, da der test 2D ist und nicht 3D + +aps sind (statistisch) unaebhaengig. d.h., jeder AP kann fuer sich optimiert werden. +optimierung des gesamtsystems ist nicht notwendig. + +pro AP also 6 params. pos x/y/z, txp, exp, waf