From 11cb93975861e514fe2bf6c51dc11cf72d1a128b Mon Sep 17 00:00:00 2001 From: kazu Date: Tue, 18 Apr 2017 18:03:31 +0200 Subject: [PATCH] current state --- CMakeLists.txt | 2 +- EvalWiFi.h | 391 ------------------------ EvalWiFiSigStrength.h | 106 ++++++- Settings.h | 38 ++- bboxes.h | 44 +++ main.cpp | 296 ++++++++++++++++--- pf/EvalWalk.h | 54 ++-- pf/PF.h | 144 ++++++--- plots/PlotErrFunc.h | 4 + plots/PlotErrTime.h | 25 ++ plots/PlotWiFiGroundProb.h | 163 ++++++++++ plots/Plotty.h | 104 ++++++- tex/chapters/experiments.tex | 67 ++++- tex/chapters/system.tex | 12 +- tex/chapters/work.tex | 17 ++ wifi/EvalWiFiGround.h | 77 +++++ wifi/EvalWiFiPathMethods.h | 555 +++++++++++++++++++++++++++++++++++ wifi/EvalWiFiPaths.h | 226 ++++++++++++++ 18 files changed, 1809 insertions(+), 516 deletions(-) delete mode 100644 EvalWiFi.h create mode 100644 bboxes.h create mode 100644 plots/PlotWiFiGroundProb.h create mode 100644 wifi/EvalWiFiGround.h create mode 100644 wifi/EvalWiFiPathMethods.h create mode 100644 wifi/EvalWiFiPaths.h 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/EvalWiFi.h b/EvalWiFi.h deleted file mode 100644 index 94ec152..0000000 --- a/EvalWiFi.h +++ /dev/null @@ -1,391 +0,0 @@ -#ifndef EVALWIFI_H -#define EVALWIFI_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 -#include -#include -#include -#include -#include -#include - -#include - -#include "Structs.h" -#include "plots/Plotty.h" -#include "plots/PlotErrTime.h" -#include "plots/PlotErrFunc.h" - -#include "CSV.h" - -#include -#include - -template class Line { - -private: - - std::vector elements; - -public: - - void add(const T& elem) { - elements.push_back(elem); - } - - std::vector getAverage(const int size) { - std::vector res; - for (int i = 0; i < (int)elements.size(); ++i) { - - T sum; - int cnt = 0; - - // calculate sume of all elements around i - for (int j = -size; j <= +size; ++j) { - int idx = i+j; - if (idx < 0) {continue;} - if (idx >= elements.size()) {continue;} - sum += elements[idx]; - ++cnt; - } - - // calculate average - T avg = sum / cnt; - res.push_back(avg); - - } - return res; - } - -}; - -/** - * read path - * fetch wifi - * use given model to estimate the most likely location - * -> WIFI ONLY - */ -class EvalWiFi { - -private: - - Floorplan::IndoorMap* map; - BBox3 mapBBox; - - //WiFiFingerprints* calib; - VAPGrouper* vap = nullptr; - //WiFiOptimizer::LogDistCeiling* opt; - - Offline::FileReader reader; - 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 */ - EvalWiFi(const std::string& mapFile, const std::string& fPath, const std::vector gtIndices) : reader(fPath), gtIndices(gtIndices) { - - std::cout << "EvalWiFi for " << fPath << std::endl; - - // 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); - - // 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::AVERAGE); - - 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("walktime (seconds)", "\\small{error (m)}", ""); - pet->getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 40)); - - pet2 = new PlotErrTime("walktime (seconds)", "\\small{-log(p(groundTruth | measurements))", ""); - - plot = new Plotty(map); - plot->buildFloorplan(); - plot->setGroundTruth(gtIndices); - - } - - void load(const std::string& xmlFile, const std::string& name) { - - WiFiModelFactory fac(map); - - // setup the model - this->wiModel = fac.loadXML(xmlFile); - - // fire - build(name); - - } - - -private: - - void build(const std::string& name) { - - K::GnuplotStroke stroke( - K::GnuplotDashtype::SOLID, 1.5, - K::GnuplotColor::AUTO() - ); - static int idx = -1; ++idx; - - - const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices); - - Line path; - K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines(); - gpPath->setStroke(stroke); - 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) { - - // all seen APs at one timestamp - const WiFiMeasurements& _mes = wifi.data; - - // debug output - std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl; - - // 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; - - if (1 == 1) { - - // 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::getProbability(rssi_model, sigma, rssi_scan); - //const double p = Distribution::Region::getProbability(rssi_model, sigma, rssi_scan); - - // adjust - prob *= p; - - } - - } else { - -// //const float limit = -85; - -// 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; - -// } - - } - - const double err = -prob; - return err; - - }; - - // parameters - float params[3]; - - // USE GENETIC -// 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); - - - // USE RANGE RANDOM WITH COOLING - 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.setNumIerations(50); - opt.calculateOptimum(func, params); - - - std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl; - const Point3 curEst(params[0], params[1], params[2]); - path.add(curEst); - - 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); - 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()); // 2D - //const float err_m = gt.getDistance(curEst); // 3D - stats->add(err_m); - - 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()); - - statsProbOnGT->add(logProbOnGTNorm); - - static int xxx = 0; ++xxx; - - // plot err - pet->addErr(ts, err_m, idx); - pet2->addErr(ts, logProbOnGTNorm, idx); - - if (xxx%2 == 0) { - - plot->plot(); - - pet->plot(); - pef->plot(); - - pet2->plot(); - pef2->plot(); - - } else { - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - - - - } - - } - -// // TODO -// void abc(const std::string& fpFile) { - -// // load fingerprints -// calib = new WiFiFingerprints(fpFile); - -// // how to handle VAPs -// VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); - -// // the optimizer -// opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM); - -// } - - -// static void dumpWiFiCenterForPath(coconst std::string& fPath) { - -// std::cout << "dump WiFi for " << fPath << std::endl; - -// Offline::FileReader fr(fPath); - -// WiFiModel logDistC - -// for (const auto wifi : fr.wifi) { -// std::cout << wifi.ts << ":" << wifi.data.entries.size() << std::endl; -// } - - -// } - - -}; - -#endif // EVALWIFI_H diff --git a/EvalWiFiSigStrength.h b/EvalWiFiSigStrength.h index 16ba02b..0876f9a 100644 --- a/EvalWiFiSigStrength.h +++ b/EvalWiFiSigStrength.h @@ -103,18 +103,118 @@ public: std::vector mes = fp.getAllForMAC(mac); if (!mes.empty()) { - const float rssi = mes.front().getRSSI(); const float s = scale(rssi, -100, -40); const Color c = Color::fromHSV(s*100, 255, 255); + p->addFloorRect(fp.pos_m, 1, c); + } - // fingerprint onto the floor - p->addFloorRect(fp.pos_m - Point3(0,0,1.3), 1, c); + } + + } + + /** SPECIAL PAPER_PLOT OUTPUT */ + void forPaperNN(Plotty* p, const MACAddress& mac) { + + VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + + // get AP/floor + std::pair _ap = FloorplanHelper::getAP(map, mac); + const Floorplan::Floor* floor = _ap.second; + const Floorplan::AccessPoint* ap = _ap.first; + + // copy + WiFiFingerprints calib = this->calib; + for (WiFiFingerprint& fp : calib.getFingerprints()) { + fp.measurements = vap.group(fp.measurements); + } + + const Point3 apPos3 = ap->getPos(floor); + K::GnuplotPoint3 apPos(apPos3.x, apPos3.y, apPos3.z); + p->points.add(apPos); + p->points.setPointSize(1.5); // The AP + + // get all positions + struct Entry { + Point3 pos; + float rssi; + Entry(Point3 pos, float rssi) : pos(pos), rssi(rssi) {;} + }; + std::vector entries; + + // process every fingerprint location + for (const WiFiFingerprint& fp : calib.getFingerprints()) { + + // either 0 entries [ap not seen here] or 1 entry due to vap grouping [ap seen here] + std::vector mes = fp.getAllForMAC(mac); + + if (!mes.empty()) { + + const float rssi = mes.front().getRSSI(); + Entry e(fp.pos_m - Point3(0,0,1.3), rssi); + + // only floor 0!!!!! + if (e.pos.z != 0) {continue;} + + entries.push_back(e); } } + const float ss = 1.0; + + for (const Floorplan::Floor* f : map->floors) { + + // only floor 0!!!!! + if (f->atHeight != 0) {continue;} + + BBox3 bb = FloorplanHelper::getBBox(f); + for (float y = bb.getMin().y; y < bb.getMax().y; y += ss) { + for (float x = bb.getMin().x; x < bb.getMax().x; x += ss) { + + + const Point3 pos(x,y,f->atHeight); +// float rssi = 0; float distSum = 0.01; +// for (const Entry& e : entries) { +// const float dist = e.pos.getDistance(pos); +// const float imp = 1.0 / std::pow((dist), 4); +// rssi += e.rssi * imp; +// distSum += imp; +// if (dist < 5) {rssi = e.rssi; distSum = 1;} +// } +// rssi /= distSum; + + auto comp = [&] (const Entry& e1, const Entry& e2) {return e1.pos.getDistance(pos) < e2.pos.getDistance(pos);}; + const auto& it = std::min_element(entries.begin(), entries.end(), comp); + + if (it->pos.getDistance(pos) > 2.99) {continue;} + +// const float rssi = it->rssi; +// const float s = scale(rssi, -100, -40); +// const Color c = Color::fromHSV(s*100, 255, 255); +// p->addFloorRect(pos, ss/2, c); + + } + } + } + + for (Entry e : entries) { + + // FIX ONE FP NEAR THE AP + if (e.pos.xy().getDistance(Point2(67.2,32.1)) < 0.1) { + e.pos.y += 0.75; + } + + p->gp << "set label '\\scriptsize{" << std::round(e.rssi) << "}' front at " << (e.pos.x-2) << "," << e.pos.y << "\n"; + + const float rssi = e.rssi; + const float s = scale(rssi, -100, -40); + const Color c = Color::fromHSV(s*100, 255, 255); + p->addFloorRect(e.pos, 2.6, c, 1.4); + + } + } void perAP_avg() { diff --git a/Settings.h b/Settings.h index 180e2c5..cb72e5e 100644 --- a/Settings.h +++ b/Settings.h @@ -8,14 +8,34 @@ namespace Settings { const std::string pathData = "/apps/android/workspace/OTHER2017/data/"; const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/"; + const std::string pathWalksToni = "/apps/android/workspace/OTHER2017/data/toni/"; const std::string pathWiFi = "/apps/android/workspace/OTHER2017/data/wifi/"; + // GPS walks const std::string path1a = pathWalks + "path1/1490208103510.csv"; const std::string path1b = pathWalks + "path1/1490208519267.csv"; const std::string path2a = pathWalks + "path2/1490208940015.csv"; const std::string path2b = pathWalks + "path2/1490209320343.csv"; + // nonGPS walks + const std::string path_toni_all_1a = pathWalksToni + "all/path1/1490031549543.csv"; + const std::string path_toni_all_1b = pathWalksToni + "all/path1/1490031883742.csv"; + + const std::string path_toni_all_2a = pathWalksToni + "all/path2/1490032575999.csv"; + const std::string path_toni_all_2b = pathWalksToni + "all/path2/1490032861864.csv"; + + + const std::string path_toni_inst_1a = pathWalksToni + "inst/path1/1489769326868.csv"; + const std::string path_toni_inst_1b = pathWalksToni + "inst/path1/1489769510080.csv"; + + const std::string path_toni_inst_2a = pathWalksToni + "inst/path2/1489774173022.csv"; + const std::string path_toni_inst_2b = pathWalksToni + "inst/path2/1489774361865.csv"; + + const std::string path_toni_inst_3a = pathWalksToni + "inst/path3/1489776812891.csv"; + const std::string path_toni_inst_3b = pathWalksToni + "inst/path3/1489776979143.csv"; + + const std::string wifiAllFixed = pathWiFi + "allFixed.xml"; const std::string wifiAllOptPar = pathWiFi + "allOptPar.xml"; const std::string wifiEachOptPar = pathWiFi + "eachOptPar.xml"; @@ -28,22 +48,23 @@ namespace Settings { const std::string wifiEachOptParPos_multimodel = pathWiFi + "eachOptParPos_multimodel.xml"; + const std::string wifiEachOptParPos_perBBox = pathWiFi + "eachOptParPos_perBBox.xml"; - const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38.xml"; + const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38_no_elev.xml"; const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat"; const std::string fPathGFX = "/apps/android/workspace/OTHER2017/tex/gfx/"; - int numParticles = 5000; + int numParticles = 2500; float smartphoneAboveGround = 1.3; namespace IMU { const float turnSigma = 1.25;//1.5; - const float stepLength = 0.70; - const float stepSigma = 0.45; - const float absHeadSigma = 80; + const float stepLength = 0.70+0.3; + const float stepSigma = 0.10; + const float absHeadSigma = 20; } namespace Grid { @@ -92,6 +113,13 @@ namespace Settings { const std::vector path2 = {53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66}; const std::vector path3 = {52, 51, 67, 68, 50, 49, 69, 70, 71, 72, 73, 74, 75, 76, 77, 73, 78, 79, 80, 81, 61, 82, 83, 84, 42, 41, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95}; + const std::vector path_toni_all_1 = {40,41,42,43,44,45, 1, 0, 46,47,48,49,50,51,52}; + const std::vector path_toni_all_2 = {53,54,55,56,57,58,59,60,61,62,63,64,65,66}; + + const std::vector path_toni_inst_1 = {0,1,2,3,4,5,6,7,8,9,10}; + const std::vector path_toni_inst_2 = {11, 12, 3, 2, 1, 0, 13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30, 11}; + const std::vector path_toni_inst_3 = {31,32,33,34,35,36,37,38}; + } } diff --git a/bboxes.h b/bboxes.h new file mode 100644 index 0000000..55e19c1 --- /dev/null +++ b/bboxes.h @@ -0,0 +1,44 @@ +// dummy +const BBox3 dummy(Point3( 0, 0, -200), Point3( 1, 1, -199)); + +// floor 0 +const BBox3 floor0Ha(Point3( 0.0, 0.0, 0.0), Point3( 18.6, 31.3, 3.99)); +const BBox3 floor0Hb(Point3( 0.0, 31.3, 0.0), Point3( 77.5, 50.7, 3.99)); +const BBox3 floor0Hc(Point3(77.5, 41.0, 0.0), Point3(111.6, 50.7, 3.99)); +BBoxes3 bboxes0H({floor0Ha, floor0Hb, floor0Hc}); + +const BBox3 floor0Oa(Point3(18.6, 6.5, 0.0), Point3( 91.0, 31.3, 3.99)); +const BBox3 floor0Ob(Point3(77.5, 31.3, 0.0), Point3( 91.0, 41.0, 3.99)); +BBoxes3 bboxes0O({floor0Oa, floor0Ob}); + +const BBox3 floor0Ia(Point3(43.7, -8.5, 0.0), Point3( 91.0, 6.5, 3.99)); +const BBox3 floor0Ib(Point3(91.0, -8.5, 0.0), Point3(111.6, 41.0, 3.99)); +BBoxes3 bboxes0I({floor0Ia, floor0Ib}); + +// floor 1 +const BBox3 floor1Ha(Point3( 0.0, 0.0, 4.0), Point3( 18.6, 31.3, 7.39)); +const BBox3 floor1Hb(Point3( 0.0, 31.3, 4.0), Point3( 77.5, 50.7, 7.39)); +BBoxes3 bboxes1H({floor1Ha, floor1Hb}); + + + const BBox3 floor1Oa(Point3(77.5, 41.0, 4.0), Point3(111.6, 50.7, 7.39)); + const BBox3 floor1Ob(Point3(61.2, 50.7, 4.0), Point3(111.6, 58.0, 7.39)); + //BBoxes3 bboxes1O({floor1Oa, floor1Ob}); + + const BBox3 floor1Ia(Point3(43.7, -8.5, 4.0), Point3( 91.0, 6.5, 7.39)); + const BBox3 floor1Ib(Point3(91.0, -8.5, 4.0), Point3(111.6, 41.0, 7.39)); + //BBoxes3 bboxes1I({floor1Ia, floor1Ib}); + + // or + + BBoxes3 bboxes1O({dummy}); + BBoxes3 bboxes1I({floor1Ia, floor1Ib, floor1Oa, floor1Ob}); + + +// floor 2 +const BBox3 floor2Ha(Point3( 0.0, 0.0, 7.4), Point3(111.6, 50.7, 10.79)); +BBoxes3 bboxes2H({floor2Ha}); + +// floor 3 +const BBox3 floor3Ha(Point3( 0.0, 0.0, 10.8), Point3(111.6, 50.7, 14.2)); +BBoxes3 bboxes3H({floor3Ha}); diff --git a/main.cpp b/main.cpp index 872d2e1..1f8a5ae 100644 --- a/main.cpp +++ b/main.cpp @@ -26,11 +26,15 @@ #include "EvalApOpt.h" #include "EvalData.h" -#include "EvalWiFi.h" #include "EvalWiFiSigStrength.h" #include "pf/EvalWalk.h" #include "EvalWifiOptResult.h" #include "wifi/EvalWiFiConvex.h" +#include "wifi/EvalWiFiGround.h" +#include "wifi/EvalWiFi.h" +#include "wifi/EvalWiFiPaths.h" +#include "wifi/EvalWiFiPathMethods.h" + #include "plots/PlotErrFunc.h" @@ -42,7 +46,7 @@ void paperOutputs() { // show optimization behaviour - if (1 == 1) { + if (1 == 0) { EvalWiFiConvex eval(map, Settings::fCalib); eval.showParams(); @@ -52,29 +56,72 @@ void paperOutputs() { } + // show fingerprints as plot - if (1==0){ + if (1 == 0){ EvalWiFiSigStrength sig(Settings::fMap, Settings::fCalib); Plotty* p = new Plotty(map); p->writeCodeTo(Settings::fPathGFX + "compare-wifi-in-out.gp"); - p->writeEpsTex(Settings::fPathGFX + "compare-wifi-in-out.tex"); + p->writeEpsTex(Settings::fPathGFX + "compare-wifi-in-out.tex", K::GnuplotSize(8.6, 4.7)); p->settings.floors = {0}; p->settings.maxZ = 1; + p->settings.outlineColor = K::GnuplotColor::fromRGB(170,170,170); p->buildFloorplan(); - sig.forAP_avg(p, MACAddress("d8:84:66:4a:23:d0")); + sig.forPaperNN(p, MACAddress("d8:84:66:4a:23:d0")); p->equalXY(); p->setView(0,0); - p->setScale(3.1, 3.1); - p->addRectangle(Point3(62, 24, 0), Point3(72, 34, 0), Color::fromRGB(0,0,0), false, false); + p->setScale(3.45, 3.45, -0.005, -0.04); + p->addRectangleW(Point3(62.5, 24, 0), Point3(72, 35, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true); // small + p->addRectangleW(Point3(8.0, 39.75, 0), Point3(72, 43.75, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true); // big p->noFrame(); p->plot(); delete p; } + // 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) { + if (1 == 0) { + + #include "bboxes.h" + + Plotty pt(map); + pt.settings.outline = false; + pt.buildFloorplan(); + + // coloring + K::GnuplotColor cH = K::GnuplotColor::fromRGB(220,220,220); + K::GnuplotColor cO = K::GnuplotColor::fromRGB(0,128,0); + K::GnuplotColor cI = K::GnuplotColor::fromRGB(190,190,190); + + // floor 0 + pt.addBBoxes(bboxes0H, cH); + pt.addBBoxes(bboxes0O, cO); + pt.addBBoxes(bboxes0I, cI); + + // floor 1 + pt.addBBoxes(bboxes1H, cH); + pt.addBBoxes(bboxes1O, cO); + pt.addBBoxes(bboxes1I, cI); + + // floor 2 + pt.addBBoxes(bboxes2H, cH); + + // floor 3 + pt.addBBoxes(bboxes3H, cH); + + pt.plot(); + + int i = 0; (void) i; + + } + + + // 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) { // // use walks? // std::vector>> calibWalks = { @@ -85,36 +132,42 @@ void paperOutputs() { // }; // EvalCompareOpt2 opt1(Settings::fMap, calibWalks); - auto removeNone = [] (const WiFiFingerprint& fp) -> bool { - return false; - }; + // all combined + auto removeNone = [] (const WiFiFingerprint& fp) -> bool {return false;}; - auto only0th = [] (const WiFiFingerprint& fp) -> bool { - return std::abs(fp.pos_m.z - (1.3)) > 0.1; - }; + // per floor + auto only0th = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (1.3)) > 0.1;}; + auto only1st = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+1.3)) > 0.1;}; + auto only2nd = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+3.4+1.3)) > 0.1;}; + auto only3rd = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+3.4+3.4+1.3)) > 0.1;}; - auto only1st = [] (const WiFiFingerprint& fp) -> bool { - return std::abs(fp.pos_m.z - (4+1.3)) > 0.1; - }; + // per bbox + #include "bboxes.h" + auto only0H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0H.contains(fp.pos_m);}; + auto only0O = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0O.contains(fp.pos_m);}; + auto only0I = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0I.contains(fp.pos_m);}; - auto only2nd = [] (const WiFiFingerprint& fp) -> bool { - return std::abs(fp.pos_m.z - (4+3.4+1.3)) > 0.1; - }; + auto only1H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1H.contains(fp.pos_m);}; + auto only1O = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1O.contains(fp.pos_m);}; + auto only1I = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1I.contains(fp.pos_m);}; + + auto only2H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes2H.contains(fp.pos_m);}; + + auto only3H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes3H.contains(fp.pos_m);}; - auto only3rd = [] (const WiFiFingerprint& fp) -> bool { - return std::abs(fp.pos_m.z - (4+3.4+3.4+1.3)) > 0.1; - }; // use fingerprints? EvalCompareOpt2 opt1(Settings::fMap, Settings::fCalib, removeNone); + // optimize using all floors EvalCompareOpt2::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK; EvalCompareOpt2::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK; EvalCompareOpt2::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK; EvalCompareOpt2::Result s4 = opt1.optPosOptParamsForEach(); //BREAK; - // optimize only for the 0th floor + + // optimize per floor EvalCompareOpt2 opt_f0(Settings::fMap, Settings::fCalib, only0th); EvalCompareOpt2::Result sf0 = opt_f0.optPosOptParamsForEach(); EvalCompareOpt2 opt_f1(Settings::fMap, Settings::fCalib, only1st); @@ -124,6 +177,30 @@ void paperOutputs() { EvalCompareOpt2 opt_f3(Settings::fMap, Settings::fCalib, only3rd); EvalCompareOpt2::Result sf3 = opt_f3.optPosOptParamsForEach(); + + // optimize per bbox + EvalCompareOpt2 opt_0H(Settings::fMap, Settings::fCalib, only0H); + EvalCompareOpt2::Result sf0H = opt_0H.optPosOptParamsForEach(); + EvalCompareOpt2 opt_0O(Settings::fMap, Settings::fCalib, only0O); + EvalCompareOpt2::Result sf0O = opt_0O.optPosOptParamsForEach(); + EvalCompareOpt2 opt_0I(Settings::fMap, Settings::fCalib, only0I); + EvalCompareOpt2::Result sf0I = opt_0I.optPosOptParamsForEach(); + + EvalCompareOpt2 opt_1H(Settings::fMap, Settings::fCalib, only1H); + EvalCompareOpt2::Result sf1H = opt_1H.optPosOptParamsForEach(); + EvalCompareOpt2 opt_1O(Settings::fMap, Settings::fCalib, only1O); + EvalCompareOpt2::Result sf1O = opt_1O.optPosOptParamsForEach(); + EvalCompareOpt2 opt_1I(Settings::fMap, Settings::fCalib, only1I); + EvalCompareOpt2::Result sf1I = opt_1I.optPosOptParamsForEach(); + + EvalCompareOpt2 opt_2H(Settings::fMap, Settings::fCalib, only2H); + EvalCompareOpt2::Result sf2H = opt_2H.optPosOptParamsForEach(); + + EvalCompareOpt2 opt_3H(Settings::fMap, Settings::fCalib, only3H); + EvalCompareOpt2::Result sf3H = opt_3H.optPosOptParamsForEach(); + + + // save models to file s1.model.saveXML(Settings::wifiAllFixed); s2.model.saveXML(Settings::wifiAllOptPar); @@ -142,6 +219,18 @@ void paperOutputs() { wmpf.add(&sf3.model, map->floors[3]); wmpf.saveXML(Settings::wifiEachOptParPos_multimodel); + // ultra fancy combined model + WiFiModelPerBBox wmbb(map); + wmbb.add(&sf0H.model, bboxes0H); + wmbb.add(&sf0O.model, bboxes0O); + wmbb.add(&sf0I.model, bboxes0I); + wmbb.add(&sf1H.model, bboxes1H); + wmbb.add(&sf1O.model, bboxes1O); + wmbb.add(&sf1I.model, bboxes1I); + wmbb.add(&sf2H.model, bboxes2H); + wmbb.add(&sf3H.model, bboxes3H); + wmbb.saveXML(Settings::wifiEachOptParPos_perBBox); + PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}"); pef.add("\\small{empiric}", &s1.errSingle); pef.add("\\small{real pos, opt params}", &s2.errSingle); @@ -160,12 +249,10 @@ void paperOutputs() { //return; //sleep(1000); - - } /** plot wifi eval results */ - if (1 == 1) { + if (1 == 0) { WiFiFingerprints fps; fps.load(Settings::fCalib); @@ -176,7 +263,7 @@ void paperOutputs() { EvalWiFiOptResult eval2(Settings::fMap); eval2.showErrorPerFingerprint(Settings::wifiAllFixed, fps); - + // advanced model [1 model per floor] EvalWiFiOptResult evalfloor(Settings::fMap); evalfloor.showErrorPerFingerprint(Settings::wifiEachOptParPos_only0th, fps); evalfloor.showErrorPerFingerprint(Settings::wifiEachOptParPos_only1st, fps); @@ -184,6 +271,10 @@ void paperOutputs() { evalfloor.showErrorPerFingerprint(Settings::wifiEachOptParPos_only3rd, fps); evalfloor.showErrorPerFingerprint(Settings::wifiEachOptParPos_multimodel, fps); + // more advanved model [1 model per bbox-region] + EvalWiFiOptResult evalBBox(Settings::fMap); + evalBBox.showErrorPerFingerprint(Settings::wifiEachOptParPos_perBBox, fps); + int i = 0; (void) i; } @@ -279,12 +370,95 @@ void testWAF() { } +void showFingerprintsFor(const std::string& mapFile, const std::string& fpFile, const std::string& smac) { + + WiFiFingerprints calib(fpFile); + VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + for (WiFiFingerprint& fp : calib.getFingerprints()) { + fp.measurements = vap.group(fp.measurements); + } + + const MACAddress mac(smac); + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile); + + + Plotty p(map); + p.buildFloorplan(); + + std::vector fps = calib.getFingerprintsFor(mac); + + for (const WiFiFingerprint& fp : fps) { + if (fp.measurements.entries.size() != 1) {throw "123";} + const float rssi = fp.measurements.entries[0].getRSSI(); + const float s = (rssi-100) / (-40 - -100); + const Color c = Color::fromHSV(s*100, 255, 255); + p.addFloorRect(fp.pos_m, 3, c); + } + + p.plot(); + sleep(1); + +} + +void showModelFor(const std::string& mapFile, const std::string& modelXml, const std::string& smac) { + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile); + WiFiModelFactory fac(map); + + WiFiModel* model = fac.loadXML(modelXml); + + const MACAddress mac(smac); + + Plotty p(map); + p.buildFloorplan(); + + const float ss = 2.0; + for (Floorplan::Floor* floor : map->floors) { + for (float y = -20; y < 70; y+=ss) { + for (float x = -10; x < 130; x+=ss) { + + const Point3 pos(x,y,floor->atHeight+1.3); + + // limit estimations to the floorplan's outline + bool contained = false; + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + HelperPoly hp(*poly); + if (hp.contains(pos.xy()*100)) { + if (poly->method == Floorplan::OutlineMethod::ADD) { + contained = true; + } + } + } + + if (!contained) {continue;} + + const float rssi = model->getRSSI(mac, pos); + const float s = (rssi-100) / (-40 - -100); + const Color c = Color::fromHSV(s*100, 255, 255); + p.addFloorRect(pos, 3, c); + + } + } + } + + p.plot(); + sleep(1); + +} int main(void) { + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap); + //testWAF(); - paperOutputs(); // return 0; + //paperOutputs(); return 0; + + //showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:D0"); + //showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:D0"); + //showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:E0"); + //showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:E0"); // calib error in/out if (1 == 0) { @@ -313,12 +487,21 @@ int main(void) { EvalCompareOpt::Result s2 = opt2.optPosOptParamsForEach(); std::cout << s2.errAbs.asString() << std::endl; + } + // prob on ground + if (1 == 0) { + + EvalWiFiGround eval(map, Settings::wifiEachOptParPos_multimodel); + //eval.show(Settings::path1a); + eval.show(Settings::path2a); + + int i = 0; (void) i; } // walks - if (1 == 1) { + if (1 == 0) { Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);; EvalWalk walk(map); walk.walk1(); @@ -343,24 +526,69 @@ int main(void) { // test wifi within data files - if (1 == 1) { + if (1 == 0) { + //EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1); EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1); - //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.load(Settings::wifiEachOptParPos); - //ew1.load(Settings::wifiAllFixed, "empirc"); + 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"); + ew1.load(Settings::wifiEachOptParPos, "normal model"); ew1.load(Settings::wifiEachOptParPos_multimodel, "model per floor"); + ew1.load(Settings::wifiEachOptParPos_perBBox, "model per region"); //ew1.load(Settings::wifiEachOptParPos_only1st, "everything opt"); + //ew1.writeTeX("path1"); sleep(1000); } + if (1 == 1) { + + std::vector files = { + Settings::path1a, //Settings::path1b, + //Settings::path2a, Settings::path2b, + //Settings::path_toni_all_1a, Settings::path_toni_all_1b, + //Settings::path_toni_all_2a, Settings::path_toni_all_2b, + //Settings::path_toni_inst_1a, Settings::path_toni_inst_1b, + //Settings::path_toni_inst_2a, Settings::path_toni_inst_2b, + //Settings::path_toni_inst_3a, Settings::path_toni_inst_3b, + }; + std::vector> gtIndices = { + Settings::GroundTruth::path1, //Settings::GroundTruth::path1, + //Settings::GroundTruth::path2, Settings::GroundTruth::path2, + //Settings::GroundTruth::path_toni_all_1, Settings::GroundTruth::path_toni_all_1, + //Settings::GroundTruth::path_toni_all_2, Settings::GroundTruth::path_toni_all_2, + //Settings::GroundTruth::path_toni_inst_1, Settings::GroundTruth::path_toni_inst_1, + //Settings::GroundTruth::path_toni_inst_2, Settings::GroundTruth::path_toni_inst_2, + //Settings::GroundTruth::path_toni_inst_3, Settings::GroundTruth::path_toni_inst_3, + }; + +// EvalWiFiPaths ewp(Settings::fMap); +// ewp.loadModel(Settings::wifiAllFixed, "empirc"); +// ewp.walks(files, gtIndices); + +// ewp.loadModel(Settings::wifiEachOptParPos, "normal model"); +// ewp.walks(files, gtIndices); + +// ewp.loadModel(Settings::wifiEachOptParPos_multimodel, "model per floor"); +// ewp.walks(files, gtIndices); + +// ewp.loadModel(Settings::wifiEachOptParPos_perBBox, "model per region"); +// ewp.walks(files, gtIndices); + + EvalWiFiPathMethods ewpm(Settings::fMap); + ewpm.loadModel(Settings::wifiEachOptParPos_perBBox, "model per region", "original", "alternative"); + ewpm.walks(files, gtIndices); + ewpm.writeGP(Settings::fPathGFX, "normalVsExp"); + + + sleep(10000); + + } diff --git a/pf/EvalWalk.h b/pf/EvalWalk.h index 1714ab5..7ff47a3 100644 --- a/pf/EvalWalk.h +++ b/pf/EvalWalk.h @@ -108,7 +108,7 @@ public: pf->setTransition( std::unique_ptr( new PFTrans(grid)) ); // resampling step? - //pf->setNEffThreshold(0.35); + //pf->setNEffThreshold(0.15); //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); //pf->setNEffThreshold(0.75); @@ -117,22 +117,26 @@ public: //pf->setNEffThreshold(0.75); //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.05)) ); - K::ParticleFilterResamplingNEff* res = new K::ParticleFilterResamplingNEff(0.50, 0.05); + K::ParticleFilterResamplingNEff* res = new K::ParticleFilterResamplingNEff(0.75, 0.05); pf->setNEffThreshold(1.0); pf->setResampling( std::unique_ptr>(res) ); - res->setDrawCallback([&] (K::Particle& p) { - static std::minstd_rand gen; - const MyGridNode* n = grid->getNodePtrFor(p.state.position); - std::normal_distribution distTurn(-0.3, +0.3); - for (int j = 0; j < 2; ++j) { - std::uniform_int_distribution distIdx(0, n->getNumNeighbors()-1); - const int idx = distIdx(gen); - n = &(grid->getNeighbor(*n, idx)); - } - p.state.position = *n; - p.state.heading.direction += distTurn(gen); - }); + // move during resampling. NOT ALLOWED! +// res->setDrawCallback([&] (K::Particle& p) { +// static std::minstd_rand gen; +// static int cnt = 0; ++cnt; +// bool init = cnt < pf->getParticles().size() * 50; + +// const MyGridNode* n = grid->getNodePtrFor(p.state.position); +// std::normal_distribution distTurn(0, (init) ? (0.5) : (0.05)); +// for (int j = 0; j < 2; ++j) { +// std::uniform_int_distribution distIdx(0, n->getNumNeighbors()-1); +// const int idx = distIdx(gen); +// n = &(grid->getNeighbor(*n, idx)); +// } +// p.state.position = *n; +// p.state.heading.direction += distTurn(gen); +// }); // state estimation step //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); @@ -144,15 +148,20 @@ public: void walk1() { // path1 - absHead = M_PI/2; - const std::string path = Settings::path1b; - const std::vector pathPoints = Settings::GroundTruth::path1; +// absHead = M_PI/2; +// const std::string path = Settings::path1b; +// const std::vector pathPoints = Settings::GroundTruth::path1; // path2 // absHead = 0; -// const std::string path = Settings::path2a; +// const std::string path = Settings::path2b; // const std::vector pathPoints = Settings::GroundTruth::path2; + // path_toni_inst_2 + absHead = M_PI; + const std::string path = Settings::path_toni_inst_2b; + const std::vector pathPoints = Settings::GroundTruth::path_toni_inst_2; + runName = ""; // get ground-truth @@ -162,8 +171,12 @@ public: //WiFiModelLogDistCeiling wifiModel(map); //wifiModel.loadXML(Settings::wifiAllFixed); //wifiModel.loadXML(Settings::wifiEachOptParPos); - WiFiModelPerFloor wifiModel(map); - wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel); + + //WiFiModelPerFloor wifiModel(map); + //wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel); + + WiFiModelPerBBox wifiModel(map); + wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox); // eval std::unique_ptr eval = std::unique_ptr( new PFEval(grid, wifiModel, em) ); @@ -226,6 +239,7 @@ public: const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe); curCtrl.compassAzimuth_rad += diff * 0.01; curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad); + curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad; //curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01; } diff --git a/pf/PF.h b/pf/PF.h index 61def65..04c6756 100644 --- a/pf/PF.h +++ b/pf/PF.h @@ -12,7 +12,8 @@ #include #include #include -#include +#include + #include #include #include @@ -92,6 +93,9 @@ struct MyObservation { /** gps measurements */ GPSData gps; + /** absolute heading in radians */ + float compassAzimuth_rad = 0; + // TODO: switch to a general activity enum/detector for barometer + accelerometer + ...? /** detected activity */ ActivityButterPressure::Activity activity; @@ -181,9 +185,8 @@ public: PFTrans(Grid* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) { - walker.addModule(&modRelHead); - walker.addModule(&modAbsHead); + //walker.addModule(&modAbsHead); //walker.addModule(&modActivity); //walker.addModule(&modFavorZ); @@ -197,40 +200,77 @@ public: } + int numTrans = 0; + void transition(std::vector>& particles, const MyControl* _ctrl) override { + ++numTrans; + // local copy!! observation might be changed async outside!! (will really produces crashes!) this->ctrl = *_ctrl; ((MyControl*)_ctrl)->resetAfterTransition(); std::normal_distribution noise(0, Settings::IMU::stepSigma); + +// std::normal_distribution turnNoise (0, 0.2); +// std::uniform_real_distribution initTurnNoise(-0.3, +0.3); +// std::uniform_real_distribution initDistNoise(0.0, 0.5); + // sanity check Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + + //for (K::Particle& p : particles) { //#pragma omp parallel for num_threads(3) for (int i = 0; i < Settings::numParticles; ++i) { //#pragma omp atomic - const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); + float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); K::Particle& p = particles[i]; - p.weight = std::pow(p.weight, 0.5); + // first transitions: more variation as the state is unknown + if (numTrans < 50 || numTrans % 10 == 0) { + + const MyGridNode* n = grid->getNodePtrFor(p.state.position); + std::normal_distribution distTurn(0, 0.4); + + for (int j = 0; j < 5; ++j) { + std::uniform_int_distribution distIdx(0, n->getNumNeighbors()-1); + const int idx = distIdx(gen); + n = &(grid->getNeighbor(*n, idx)); + } + p.state.position = *n; + + if (numTrans < 50) { + p.state.heading.direction += distTurn(gen); + dist_m += 0.5; + } + + } + + //p.state.heading.direction += turnNoise(gen); + + + // "blur" the density by adjusting the weights + //p.weight = std::pow(p.weight, 0.4); + //p.weight = 1; - double prob; + double prob = 1.0; p.state = walker.getDestination(*grid, p.state, dist_m, prob); - //p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15); - //p.weight = (prob > 0.01) ? (1.0) : (0.15); - //p.weight = prob; - //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.7); // add grid-walk-probability p.weight *= prob; // grid-walk-probability - if (p.weight != p.weight) {throw Exception("nan");} + + if (p.weight != p.weight) { + throw Exception("nan"); + } + if (p.weight == 0) { + std::cout << "one particle weight is 0.0!" << std::endl; + //throw Exception("weight = 0.0"); + } } @@ -253,7 +293,6 @@ class PFEval : public K::ParticleFilterEvaluation { WiFiObserverFree wiFiProbability; // free-calculation - //WiFiObserverGrid wiFiProbability; // grid-calculation // smartphone is 1.3 meter above ground const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); @@ -264,88 +303,109 @@ public: grid(grid), wifiModel(wifiModel), em(em), wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free - //wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid + } - double getStairProb(const K::Particle& p, const ActivityButterPressure::Activity act) { - const float kappa = 0.75; + double getAbsHead(const MyState& s, const MyObservation& obs) { - const MyGridNode& gn = grid->getNodeFor(p.state.position); - switch (act) { + // compare the heading against the state's heading - the last error + const Heading stateHead = s.heading.direction; + const Heading obsHead(obs.compassAzimuth_rad); - case ActivityButterPressure::Activity::STAY: - if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} - if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} - {return 1-kappa;} + // get the difference + const float angularDiff = obsHead.getDiffHalfRAD(stateHead); - case ActivityButterPressure::Activity::UP: - case ActivityButterPressure::Activity::DOWN: - if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} - if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} - {return 1-kappa;} + double res = 0; - } + if (angularDiff > Angle::degToRad(90)) {res = 0.05;} + else if (angularDiff > Angle::degToRad(45)) {res = 0.25;} + else {res = 0.70;} - return 1.0; + return res; } + WiFiQualityAnalyzer wqa; double evaluation(std::vector>& particles, const MyObservation& _observation) override { - double sum = 0; - // local copy!! observation might be changed async outside!! (will really produces crashes!) const MyObservation observation = _observation; // vap-grouping const int numAP1 = observation.wifi.entries.size(); - const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi); + WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi); const int numAP2 = wifiObs.entries.size(); + Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2)); + + + wqa.add(wifiObs); + const float quality = wqa.getQuality(); // GPS const GPSProbability gpsProb(em); - Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2)); // sanity check Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + // use (0.9 * p) + (0.1 * (1-p)) for error cases + wiFiProbability.setUseError(false); + //#pragma omp parallel for num_threads(3) 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); + double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs); + double pWiFiMod = Distribution::Exponential::getProbability(0.20, -std::log(pWiFi)); + double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs); + + + + if (quality < 0.2) { + pWiFi = 1; + pWiFiVeto = 1; + std::cout << "disabling WiFi" << std::endl; + } // WiFi grid //const MyGridNode& node = grid->getNodeFor(p.state.position); + //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; + const double pAbsHead = getAbsHead(p.state, observation); + const double prob = pWiFi * pWiFiVeto;// * pAbsHead; + + //GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled? if (pGPS != 1) { int i = 0; (void) i; } + // TESTING + //p.weight = std::pow(p.weight, 0.5); + 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 - if (p.weight != p.weight) {throw Exception("nan");} - #pragma omp atomic - sum += p.weight; + if (p.weight != p.weight) { + throw Exception("nan"); + } + if (p.weight == 0) { + std::cout << "one particle weight is 0.0!" << std::endl; + // throw Exception("weight = 0.0"); + } } - return sum; + return 0; } diff --git a/plots/PlotErrFunc.h b/plots/PlotErrFunc.h index 3bca967..6a42b57 100644 --- a/plots/PlotErrFunc.h +++ b/plots/PlotErrFunc.h @@ -71,6 +71,10 @@ public: return gp; } + K::GnuplotPlot& getPlot() { + return gplot; + } + void writeCodeTo(const std::string& file) { this->codeFile = file; } diff --git a/plots/PlotErrTime.h b/plots/PlotErrTime.h index 380185c..77d253a 100644 --- a/plots/PlotErrTime.h +++ b/plots/PlotErrTime.h @@ -19,6 +19,7 @@ private: K::GnuplotPlotElementLines lineErr[8]; K::GnuplotPlotElementLines lineB; K::GnuplotPlotElementLines lineC; + std::string codeFile; public: @@ -40,6 +41,25 @@ public: } + void clear() { + for (int i = 0; i < 8; ++i) {lineErr[i].clear();} + lineB.clear(); + lineC.clear(); + } + + void writeCodeTo(const std::string& file) { + this->codeFile = file; + } + + void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) { + gp.setTerminal("epslatex", size); + gp.setOutput(file); + gp << "set key\n"; + gp << "set rmargin 0.2\n"; + gp << "set tmargin 0.2\n"; + gp << "set bmargin 1.2\n"; + } + K::GnuplotPlot& getPlot() { return gpplot; } @@ -63,6 +83,11 @@ public: void plot() { gp.draw(gpplot); + if (codeFile != "") { + std::ofstream out(codeFile); + out << gp.getBuffer(); + out.close(); + } gp.flush(); } diff --git a/plots/PlotWiFiGroundProb.h b/plots/PlotWiFiGroundProb.h new file mode 100644 index 0000000..ca3f3ca --- /dev/null +++ b/plots/PlotWiFiGroundProb.h @@ -0,0 +1,163 @@ +#ifndef EVALWIFIGROUNDPROB_H +#define EVALWIFIGROUNDPROB_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 "Indoor/sensors/radio/WiFiProbabilityFree.h" + +#include "../Helper.h" + +#include +#include "../plots/Plotty.h" + +class PlotWiFiGroundProb { + +private: + + const Floorplan::IndoorMap* map; + + const float stepSize_m = 3.0; + //std::vector pos; + + struct Entry { + Point3 pos; + K::GnuplotObjectPolygon* poly; + float max = 0; + float sum = 0; + int cnt = 0; + }; + + std::vector pos; + + Plotty plot; + +public: + + /** ctor */ + PlotWiFiGroundProb(const Floorplan::IndoorMap* map) : map(map), plot(map) { + buildEvalPoints(); + + plot.settings.outline = false; + plot.buildFloorplan(); + } + + void show(const WiFiObserverFree& prob, const WiFiMeasurements& _mes) { + + VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + + const WiFiMeasurements mes = vap.group(_mes); + + double min = +99999; + double max = -99999; + for (Entry& e : pos) { + const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes); + if (p < min) {min = p;} + if (p > max) {max = p;} + } + double diff = max-min; + + + for (Entry& e : pos) { + const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes); + const double f = (p-min) / diff * 255; + e.sum += p; + e.cnt++; + + //if (f > e.max) { + const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255); + e.poly->getFill().setColor(c); + e.max = f; + //} + + //plot.cpoints.add(K::GnuplotPoint3(pt.x, pt.y, pt.z), p); + } + + + + } + + void update() { + + double min = +99999; + double max = -99999; + for (Entry& e : pos) { + const double p = e.sum / e.cnt; + if (p < min) {min = p;} + if (p > max) {max = p;} + } + double diff = max-min; + + for (Entry& e : pos) { + const double p = e.sum / e.cnt; + const double f = (p-min) / diff * 255; + const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255); + e.poly->getFill().setColor(c); + } + + } + + void plotMe() { + plot.plot(); + } + +private: + + /** get a list of all points among the outline */ + void buildEvalPoints() { + + BBox3 bbox = FloorplanHelper::getBBox(map); + + for (Floorplan::Floor* f : map->floors) { + for (float y = bbox.getMin().y; y < bbox.getMax().y; y += stepSize_m) { + for (float x = bbox.getMin().x; x < bbox.getMax().x; x += stepSize_m) { + + GridFactory::PartOfOutline part = GridFactory::isPartOfFloorOutline(x*100, y*100, f->outline); + if (part != GridFactory::PartOfOutline::NO) { + + const float s = stepSize_m/2.0f; + + Entry e; + + e.poly = new K::GnuplotObjectPolygon(); + e.poly->add(K::GnuplotCoordinate3(x-s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST)); + e.poly->add(K::GnuplotCoordinate3(x+s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST)); + e.poly->add(K::GnuplotCoordinate3(x+s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST)); + e.poly->add(K::GnuplotCoordinate3(x-s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST)); + e.poly->close(); + e.poly->getFill().setStyle(K::GnuplotFillStyle::SOLID); + e.poly->setStroke(K::GnuplotStroke::NONE()); + + e.pos = Point3(x, y, f->atHeight); + + pos.push_back(e); + plot.splot.getObjects().add(e.poly); + + } + + } + } + } + + } + + +}; + +#endif // EVALWIFIGROUNDPROB_H diff --git a/plots/Plotty.h b/plots/Plotty.h index f3920fe..b5ab00e 100644 --- a/plots/Plotty.h +++ b/plots/Plotty.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include struct Color { @@ -99,6 +101,7 @@ public: K::GnuplotSplotElementLines mapOutlineGlass; K::GnuplotSplotElementLines mapOutlineDrywall; K::GnuplotSplotElementLines mapOutlineConcrete; + K::GnuplotSplotElementLines mapBBoxes; std::string codeFile; @@ -130,9 +133,11 @@ public: mapOutlineConcrete.getStroke().getColor().setHexStr("#888888"); mapOutlineConcrete.getStroke().setWidth(2); mapOutlineDrywall.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().setType(K::GnuplotDashtype::DASHED); + mapBBoxes.getStroke().setWidth(2); splot.add(&mapOutlineConcrete); splot.add(&mapOutlineDrywall); splot.add(&mapOutlineGlass); + splot.add(&mapBBoxes); splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777"); @@ -149,6 +154,52 @@ public: } + void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { + for (const BBox3& bb : boxes.get()) { + addBBoxPoly(bb, c); + } + } + + void addBBox(const BBox3& bb) { + + // floor + mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z}); + mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMin().z}); + mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMin().z}); + mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMin().z}); + mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z}); + mapBBoxes.splitFace(); mapBBoxes.splitFace(); + + // ceil + mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z}); + mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMax().z}); + mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMax().z}); + mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMax().z}); + mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z}); + mapBBoxes.splitFace(); mapBBoxes.splitFace(); + + // up + mapBBoxes.addSegment({bb.getMin().x, bb.getMin().y, bb.getMin().z}, {bb.getMin().x, bb.getMin().y, bb.getMax().z}); + mapBBoxes.addSegment({bb.getMax().x, bb.getMin().y, bb.getMin().z}, {bb.getMax().x, bb.getMin().y, bb.getMax().z}); + mapBBoxes.addSegment({bb.getMin().x, bb.getMax().y, bb.getMin().z}, {bb.getMin().x, bb.getMax().y, bb.getMax().z}); + mapBBoxes.addSegment({bb.getMax().x, bb.getMax().y, bb.getMin().z}, {bb.getMax().x, bb.getMax().y, bb.getMax().z}); + + } + + void addBBoxPoly(const BBox3& bb, K::GnuplotColor c) { + + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->close(); + poly->setStroke(K::GnuplotStroke::NONE()); + poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, c)); + splot.getObjects().add(poly); + + } + void setGroundTruth(const Point3 pos_m) { gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n"; } @@ -202,7 +253,25 @@ public: addPolygon(points, c.toHEX(), front, fill); } - void addPolygon(const std::vector& points, const std::string& color, bool front = false, bool fill = true) { + void addRectangleW(const Point3 p1, const Point3 p2, const K::GnuplotColor c, const float w, bool front = false) { + std::vector points = { + Point3(p1.x, p1.y, p1.z), + Point3(p2.x, p1.y, p1.z), + Point3(p2.x, p2.y, p1.z), + Point3(p1.x, p2.y, p1.z), + Point3(p1.x, p1.y, p1.z), + }; + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->getStroke().setWidth(w); + poly->getStroke().setColor(c); + poly->setFront(front); + for (const Point3 p : points) { + poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST)); + } + splot.getObjects().add(poly); + } + + void addPolygon(const std::vector& points, const std::string& color, bool front = false, bool fill = true, const float alpha = 1) { for (const Point3 p : points) { if (p.z < settings.minZ) {return;} @@ -216,6 +285,7 @@ public: for (const Point3 p : points) { poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST)); poly->setZIndex(p.z); // manual depth ordering + poly->getFill().setAlpha(alpha); } poly->setFront(front); @@ -240,16 +310,16 @@ public: } - void addFloorRect(const Point3 pos_m, const float size, Color c) { + void addFloorRect(const Point3 pos_m, const float size, Color c, float ratio = 1.0) { - const Point3 p1 = pos_m + Point3(-size, -size, 0); - const Point3 p2 = pos_m + Point3(+size, -size, 0); - const Point3 p3 = pos_m + Point3(+size, +size, 0); - const Point3 p4 = pos_m + Point3(-size, +size, 0); + const Point3 p1 = pos_m + Point3(-size, -size/ratio, 0); + const Point3 p2 = pos_m + Point3(+size, -size/ratio, 0); + const Point3 p3 = pos_m + Point3(+size, +size/ratio, 0); + const Point3 p4 = pos_m + Point3(-size, +size/ratio, 0); std::vector points = {p1,p2,p3,p4,p1}; - addPolygon(points, c.toHEX()); + addPolygon(points, c.toHEX(), false, true); // gp << "set object polygon from "; // for (size_t i = 0; i < points.size(); ++i) { @@ -279,11 +349,12 @@ public: } void setView(const float degX, const float degY) { - gp << "set view " << degX << "," << degY << "\n"; + //gp << "set view " << degX << "," << degY << "\n"; + splot.getView().setCamera(degX, degY); } - void setScale(const float x, const float y) { - gp << "set multiplot layout 1,1 scale " << x << "," << y << "\n"; + void setScale(const float x, const float y, const float ox = 0, const float oy = 0) { + gp << "set multiplot layout 1,1 scale " << x << "," << y << " offset " << ox << "," << oy << "\n"; } void writeCodeTo(const std::string& file) { @@ -292,9 +363,12 @@ public: void noFrame() { gp << "unset border\n"; - gp << "unset xtics\n"; - gp << "unset ytics\n"; - gp << "unset ztics\n"; +// gp << "unset xtics\n"; +// gp << "unset ytics\n"; +// gp << "unset ztics\n"; + splot.getAxisX().setTicsVisible(false); + splot.getAxisY().setTicsVisible(false); + splot.getAxisZ().setTicsVisible(false); } void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) { @@ -357,7 +431,9 @@ public: // plot the floor's outline if (settings.outline) { for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - K::GnuplotColor color = (poly->outdoor) ? (K::GnuplotColor::fromRGB(200, 240, 200)) : (K::GnuplotColor::fromRGB(210,210,210)); + K::GnuplotColor color = K::GnuplotColor::fromRGB(210,210,210); + if (poly->outdoor) {color = K::GnuplotColor::fromRGB(200, 240, 200);} + //if (poly->method == Floorplan::OutlineMethod::REMOVE) {color = K::GnuplotColor::fromRGB(245,245,245);} K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color); K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, K::GnuplotStroke::NONE()); for (Point2 pt : poly->poly.points) { diff --git a/tex/chapters/experiments.tex b/tex/chapters/experiments.tex index 6005421..e3460f3 100644 --- a/tex/chapters/experiments.tex +++ b/tex/chapters/experiments.tex @@ -1,5 +1,12 @@ experiments +\todo{obwohl das angepasste modell doch recht gut laeuft und der fehler recht klein wird, sind immernoch stellen dabei, +wo es einfach nicht gut passt, unguenstige mehrdeutigkeiten vorliegen, oder regionen einfach nicht passen wie sie sollten. +das liegt teils auch daran, dass die fingerprints drehend aufgenommen wurden und beim laufen nach hinten durch den +menschen abgeschottet wird. auch zeitlicher verzug kann ein problem darstellen.} + +\todo{GPS ist leider kaum eine hilfe. entweder kein empfang wegen ueberdachung oder abschattung, oder +zu kurz draußen um einen guten gps-fix zu bekommen.} 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 @@ -59,9 +66,65 @@ 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 +\todo{ + we analyzed various paths throughout the whole building +} -\input{gfx/compare-wifi-in-out.tex} -starker einfluss der glasscheiben.. 3 meter nach dem AP ist nur noch sehr wenig uebrig +\todo{ + mit grafik: exp-dist vergroesert teils den abstand zu anderen locations , der GT selbst wird also besser, + aber an anderen stellen geht dafür der fehler hoch und kann zu verlaufen führen (z.B. treppenhaus) +} + \begin{figure} + \input{gfx/wifiCompare_normalVsExp_cross.tex} + \input{gfx/wifiCompare_normalVsExp_meter.tex} + \label{normal vs exponential} + \end{figure} + +\todo{ + erkenntnisse: + + schlechte messwerte (niedrige RSSI) aus der messung ignorieren hilft nur sehr sehr bedingt.. eher im gegenteil. meist geht der fehler (stark) hoch + + schlechteste messung weglassen ist auch schlecht + + sigma je nach signalstärke anpassen bringt leider auch nichts. wenn man das aber macht, + dann: fuer grosse signalstaerken ein grosses sigma! andersrum gehts nach hinten los! + + veto funktioniert auch nicht immer. es gibt stellen da ist ein AP wegen abschattung in der realität nicht sichtbar + das smpartphone sieht ihn deshalb nicht, im model ist er aber fälschlicherweise da deshalb falsches veto + oder das smartphone sieht einen AP wegen kollisionen nicht oder weil er durch den rücken stark verdeckt wird + + es gibt einfach stellen an denen das wifi nicht eindeutig ist, die an anderen stellen quasi exakt genauso vorliegen + da laesst sich dann nicht viel machen + +} + +\todo{ + das bbox modell hat probleme an den uebergängen zwischen bboxes da dort teils starke spruenge sind + die nicht immer in der realität so auch vorliegen. z.B. z-wechsel machen teils probleme. + hier wäre ein kontinuierliches modell hilfreich bzw interpolation in randbereichen +} + +\todo{ + wenn ich beim fingerprinten einen AP an einer stelle NICHT gesehen habe, + ist das auch eine aussage für die model optimierung.. da kann dann sicher keine signatlstaerke > -90 an der stelle raus kommen +} + +\todo{ + wir wollen nicht, dass die position des ground-truths durch das wifi so wahrscheinlich wie möglich ist, + wir wollen dass die position des ground-truth einfach eine höhere wahrscheinlichkeit hat, als alle anderen punkte im gebäude + das pruefen wir ab +} + +\begin{figure} + \centering + \input{gfx/compare-wifi-in-out.tex} + \caption{ + Measurable signal strengths of a testing \docAPshort{} (black dot). + While the signal diminishes slowly along the corridor (upper rectangle) + the metallised windows (dashed outline) attenuate the signal by over \SI{30}{\decibel} (lower rectangle). + } +\end{figure} 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 diff --git a/tex/chapters/system.tex b/tex/chapters/system.tex index b4568c1..b726d0d 100644 --- a/tex/chapters/system.tex +++ b/tex/chapters/system.tex @@ -58,11 +58,15 @@ $p(\mStateVec_{t} \mid \mStateVec_{t-1}, \mObsVec_{t-1})$ are sampled based on those sensor values. - Thus, the overall system works as described in \cite{Ebner-16}. + As this work focuses on \docWIFI{} optimization, not all parts of + the localization system are discussed in detail. + For missing explanations please refer to \cite{Ebner-16}. + % Since then, absolute heading and GPS have been added as additional sensors - to further enhance the localization. + to further enhance the localization by comparing the sensor values + using some distribution. + \todo{neues resampling?} - \todo{ueberleitung} \todo{ @@ -70,7 +74,7 @@ dafür braucht man entweder viele fingerprints oder ein modell } As GPS will only work outdoors, e.g. when moving from one building into another, - the system's absolute position is solely provided by the \docWIFI{} component. + the system's absolute position indoors is solely provided by the \docWIFI{} component. Therefore its crucial for this component to provide location estimations that are as accurate as possible, while ensuring fast setup and maintenance times. diff --git a/tex/chapters/work.tex b/tex/chapters/work.tex index 909534c..69d44f8 100644 --- a/tex/chapters/work.tex +++ b/tex/chapters/work.tex @@ -99,6 +99,11 @@ algorithms like gradient descent \todo{cite} and (downhill) simpelx \todo{cite} are well suited. + \todo{formel fuer opt: was wird optimiert?} + \begin{equation} + argmin_{bla} blub() + \end{equation} + \begin{figure} \input{gfx/wifiop_show_optfunc_params} \label{fig:wifiOptFuncTXPEXP} @@ -141,6 +146,9 @@ \subsection{Modified Signal Strength Model} + \todo{nicht: during initial eval, sondern gleich sagen, dass die vermutung nahe liegt, dass das modell + nicht gut klappen wird, weil waende und unser metall-glas nicht beruecksichtigt werden. deshalb + versuchen wir ein anderes modell das immernoch live arbeiten kann} During the initial eval, some issues were discovered. While aforementioned optimization was able to reduce the error between reference measurements and model estimations to \SI{50}{\percent}, the position estimation \ref{eq:wifiProb} did not benefit from improved model parameters. @@ -164,6 +172,15 @@ function like average, median or maximum. +\todo{abs-head ist in der observation besser, weil es beim resampling mehr bringt und dafuer srogt, dass die richtigen geloescht werden!} + +\todo{anfaenglich falsches heading ist gift, wegen rel. heading, weil sich dann alles verlaeuft. fix: anfaenglich große heading variation erlauben} + +\todo{wifi-veto erklaeren. wir fragen die 4 laut model an jeder pos staerksten APs ab und vergleichen die mit dem scan. +weichen min 50 prozent um mehr als 20 dB ab, oder sind im scan nicht gesehen worden, wird fuer diese position ein veto eingelegt: +0.001 vs 0.999. das geht auch nur so, da ja an jeder position andere APs die staerksten waeren.. direkt mit deren wahrscheinlichkeiten +zu arbeiten wuerde also aepfel mit birnen vergleichen} + wie wird optimiert a) bekannte pos + empirische params b) bekannte pos + opt params (fur alle APs gleich) [simplex] diff --git a/wifi/EvalWiFiGround.h b/wifi/EvalWiFiGround.h new file mode 100644 index 0000000..d72e681 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include + +#include "../Structs.h" +#include "../plots/Plotty.h" +#include "../plots/PlotErrTime.h" +#include "../plots/PlotErrFunc.h" +#include "../plots/PlotWiFiGroundProb.h" +//#include "../CSV.h" + +#include +#include + +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 diff --git a/wifi/EvalWiFiPathMethods.h b/wifi/EvalWiFiPathMethods.h new file mode 100644 index 0000000..447de47 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "../Structs.h" +#include "../plots/Plotty.h" +#include "../plots/PlotErrTime.h" +#include "../plots/PlotErrFunc.h" +#include "../plots/PlotWiFiGroundProb.h" +//#include "CSV.h" + +#include +#include +#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* statsOrig_m; + K::Statistics* statsOther_m; + PlotErrFunc* pef_m; + + K::Statistics* statsOrig_p; + K::Statistics* statsOther_p; + PlotErrFunc* pef_p; + + K::Statistics* statsOrig_c; + K::Statistics* statsOther_c; + PlotErrFunc* pef_c; + + WiFiModel* wiModel = nullptr; + + std::vector randomLoc; + std::vector 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 distX(mapBBox.getMin().x, mapBBox.getMax().x); + std::uniform_real_distribution distY(mapBBox.getMin().y, mapBBox.getMax().y); + for (const Floorplan::Floor* f : map->floors) { + + std::vector 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 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); +// 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(); + statsOrig_p = new K::Statistics(); + statsOrig_c = new K::Statistics(); + + statsOther_m = new K::Statistics(); + statsOther_p = new K::Statistics(); + statsOther_c = new K::Statistics(); + + 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 files, const std::vector> 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 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; + 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(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 func, Point3 gt, K::Statistics* 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::getProbability(rssi_model, sigma, rssi_scan); + //const double p = Distribution::Region::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::getProbability(rssi_model, sigma, rssi_scan); + //const double p = Distribution::Region::getProbability(rssi_model, sigma, rssi_scan); + //const double p = Distribution::Triangle::getProbability(rssi_model, sigma*2, rssi_scan); + //double p = Distribution::Exponential::getProbability(1, std::abs(rssi_model-rssi_scan)); + + double p = Distribution::Exponential::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 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 diff --git a/wifi/EvalWiFiPaths.h b/wifi/EvalWiFiPaths.h new file mode 100644 index 0000000..a3ae92d --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "../Structs.h" +#include "../plots/Plotty.h" +#include "../plots/PlotErrTime.h" +#include "../plots/PlotErrFunc.h" +#include "../plots/PlotWiFiGroundProb.h" +//#include "CSV.h" + +#include +#include +#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* stats_m; + PlotErrFunc* pef_m; + + K::Statistics* 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(); + stats_p = new K::Statistics(); + + pef_m->add(name, stats_m); + pef_p->add(name, stats_p); + + } + + void walks (const std::vector files, const std::vector> gtIndicies) { + for (size_t i = 0; i < files.size(); ++i) { + walk(files[i], gtIndicies[i]); + } + } + + void walk(const std::string& fPath, const std::vector 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::getProbability(rssi_model, sigma, rssi_scan); + //const double p = Distribution::Region::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; + 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.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