From a8123d532d00098a6abfdff562c6682d694017a0 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 16 May 2018 13:02:06 +0200 Subject: [PATCH] added wifi per-floor optimization added plot to wifi-quality-analyzer changes to per-floor wifi models minor fixes --- geo/BBox3.h | 13 +- navMesh/walk/NavMeshWalkKLD.h | 2 +- sensors/radio/WiFiProbabilityFree.h | 24 ++- sensors/radio/WiFiQualityAnalyzer.h | 33 ++++ sensors/radio/model/WiFiModelPerFloor.h | 75 +++++++++- sensors/radio/model/WiFiModels.h | 1 + .../radio/setup/WiFiOptimizerLogDistCeiling.h | 37 +++-- sensors/radio/setup/WiFiOptimizerPerFloor.h | 141 ++++++++++++++++++ wifi/estimate/ray3/FloorplanMesh.h | 2 +- 9 files changed, 304 insertions(+), 24 deletions(-) create mode 100644 sensors/radio/setup/WiFiOptimizerPerFloor.h diff --git a/geo/BBox3.h b/geo/BBox3.h index 2d1d863..b0fbd1a 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -89,11 +89,14 @@ public: p2 += p; // increase maximum } - /** set both, min/max z to the same value */ - void setZ(const float z) { - p1.z = z; - p2.z = z; - } + /** set both, min/max z to the same value */ + void setZ(const float z) { + p1.z = z; + p2.z = z; + } + + void setMinZ(const float z) {this->p1.z = z;} + void setMaxZ(const float z) {this->p2.z = z;} /** does the bbox contain the given point? */ bool contains(const Point3& p) const { diff --git a/navMesh/walk/NavMeshWalkKLD.h b/navMesh/walk/NavMeshWalkKLD.h index 2490e9b..ca7d31e 100644 --- a/navMesh/walk/NavMeshWalkKLD.h +++ b/navMesh/walk/NavMeshWalkKLD.h @@ -81,7 +81,7 @@ namespace NM { // to-be-walked distance; const float toBeWalkedDist = params.getToBeWalkedDistance(); const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; - const float toBeWalkedDistKld = (kld * qualityWifi * 0.5); + const float toBeWalkedDistKld = (kld * qualityWifi); // construct reachable region NavMeshSub reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD... diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 8f69abd..7f4a02a 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -15,6 +15,14 @@ */ class WiFiObserverFree : public WiFiProbability { +public: + + enum class EvalDist { + NORMAL_DISTIRBUTION, + CAPPED_NORMAL_DISTRIBUTION, + EXPONENTIAL, + }; + private: const float sigma; @@ -31,9 +39,11 @@ private: bool useError = false; + EvalDist dist; + public: - WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) { + WiFiObserverFree(const float sigma, WiFiModel& model, EvalDist dist = EvalDist::NORMAL_DISTIRBUTION) : sigma(sigma), model(model), dist(dist) { allAPs = model.getAllAPs(); } @@ -76,8 +86,16 @@ public: const float sigma = this->sigma + this->sigmaPerSecond * age.sec(); // probability for this AP - double local = Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); - //double local = Distribution::Exponential::getProbability(0.1, std::abs(modelRSSI-scanRSSI)); + double local = NAN; + switch (dist) { + case EvalDist::NORMAL_DISTIRBUTION: + local = Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); break; + case EvalDist::CAPPED_NORMAL_DISTRIBUTION: + local = Distribution::Region::getProbability(modelRSSI, sigma, scanRSSI); break; + case EvalDist::EXPONENTIAL: + local = Distribution::Exponential::getProbability(0.05, std::abs(modelRSSI-scanRSSI)); break; + default: throw Exception("unsupported distribution"); + } // also add the error value? [location is OK but model is wrong] if (useError) { diff --git a/sensors/radio/WiFiQualityAnalyzer.h b/sensors/radio/WiFiQualityAnalyzer.h index 522b8cf..8fe4b54 100644 --- a/sensors/radio/WiFiQualityAnalyzer.h +++ b/sensors/radio/WiFiQualityAnalyzer.h @@ -4,6 +4,12 @@ #include "WiFiMeasurements.h" #include +#ifdef WITH_DEBUG_PLOT + #include + #include + #include +#endif + class WiFiQualityAnalyzer { private: @@ -12,8 +18,25 @@ private: std::vector history; float quality = 0; + #ifdef WITH_DEBUG_PLOT + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines line1; + K::GnuplotPlotElementLines line2; + int gpX = 0; + #endif + + public: + WiFiQualityAnalyzer() { + #ifdef WITH_DEBUG_PLOT + plot.add(&line1); + plot.add(&line2); + plot.setTitle("WiFi Quality"); + #endif + } + /** attach the current measurement and infer the quality */ void add(const WiFiMeasurements& mes) { @@ -43,6 +66,16 @@ private: quality = qAvgdB; + #ifdef WITH_DEBUG_PLOT + line1.add(K::GnuplotPoint2(gpX,qAvgdB)); line1.setTitle("dB"); line1.getStroke().setWidth(2); + line2.add(K::GnuplotPoint2(gpX,qCnt)); line2.setTitle("visible"); + while(line1.size() > 50) {line1.remove(0);} + while(line2.size() > 50) {line2.remove(0);} + ++gpX; + gp.draw(plot); + gp.flush(); + #endif + } /** score [0:1] based on the average sig-strength. the higher the better */ diff --git a/sensors/radio/model/WiFiModelPerFloor.h b/sensors/radio/model/WiFiModelPerFloor.h index f14dd7e..8d84b5f 100644 --- a/sensors/radio/model/WiFiModelPerFloor.h +++ b/sensors/radio/model/WiFiModelPerFloor.h @@ -14,6 +14,8 @@ */ class WiFiModelPerFloor : public WiFiModel { +public: + struct ModelForFloor { float fromZ; @@ -30,6 +32,8 @@ class WiFiModelPerFloor : public WiFiModel { }; +private: + Floorplan::IndoorMap* map; /** all contained models [one per floor] */ @@ -46,6 +50,10 @@ public: } + /** get a list of all models for the distinct floors */ + std::vector& getFloorModels() { + return models; + } /** get a list of all APs known to the model */ std::vector getAllAPs() const override { @@ -70,11 +78,70 @@ public: float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override { - for (const ModelForFloor& mff : models) { - if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);} - } + #if (1==0) - return -120; + float res = -120; + + // find the best matching one + for (const ModelForFloor& mff : models) { + if (mff.matches(position_m.z)) { + const float rssi = mff.mdl->getRSSI(accessPoint, position_m); + if (rssi > res) {res = rssi;} + } + } + + return res; + + + #elif (1==0) + + // nearest matching model + float nearest = 9999; + float res = -120; + + for (const ModelForFloor& mff : models) { + const float distToFloor = std::abs(position_m.z - mff.fromZ); + if (distToFloor < nearest) { + const float rssi = mff.mdl->getRSSI(accessPoint, position_m); + if (rssi == rssi) { + res = rssi; + nearest = distToFloor; + } + } + } + + Assert::isNotNaN(res, "detected NaN"); + return res; + + #elif (1==1) + + // average of all matching models + + float sum = 0; + int cnt = 0; + + // find the best matching one + for (const ModelForFloor& mff : models) { + if (mff.matches(position_m.z)) { + const float rssi = mff.mdl->getRSSI(accessPoint, position_m); + if (rssi == rssi) { + sum += rssi; ++cnt; + } + } + } + + Assert::isNotNaN(sum, "detected NaN"); + + return (cnt > 0) ? (sum/cnt) : (-120); + + #endif + + +// for (const ModelForFloor& mff : models) { +// if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);} +// } + +// return -120; } diff --git a/sensors/radio/model/WiFiModels.h b/sensors/radio/model/WiFiModels.h index 6dd00c9..0955adf 100644 --- a/sensors/radio/model/WiFiModels.h +++ b/sensors/radio/model/WiFiModels.h @@ -1,6 +1,7 @@ #ifndef WIFIMODELS_H #define WIFIMODELS_H +/** umbrella header for WiFiModel and factory */ #include "WiFiModel.h" #include "WiFiModelFactory.h" #include "WiFiModelFactoryImpl.h" diff --git a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h index d093b94..3ac1c88 100644 --- a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h +++ b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h @@ -99,7 +99,7 @@ namespace WiFiOptimizer { return (waf > 0) || (txp < -50) || (txp > -30) || - (exp > 4) || + (exp > 4) || (exp < 1); } @@ -138,18 +138,29 @@ namespace WiFiOptimizer { }; - using APFilter = std::function; + using APFilter = std::function; - const APFilter NONE = [] (const int numFingerprints, const MACAddress& mac) { - (void) numFingerprints; (void) mac; + const APFilter NONE = [] (const Stats& stats, const MACAddress& mac) { + (void) stats; (void) mac; return false; }; - const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) { + const APFilter MIN_2_FPS = [] (const Stats& stats, const MACAddress& mac) { (void) mac; - return numFingerprints < 5; + return stats.usedFingerprins < 2; }; + const APFilter MIN_5_FPS = [] (const Stats& stats, const MACAddress& mac) { + (void) mac; + return stats.usedFingerprins < 5; + }; + + const APFilter MIN_10_FPS = [] (const Stats& stats, const MACAddress& mac) { + (void) mac; + return stats.usedFingerprins < 10; + }; + + private: Floorplan::IndoorMap* map; @@ -179,15 +190,21 @@ namespace WiFiOptimizer { float errSum = 0; int errCnt = 0; std::vector res; for (const MACAddress& mac : getAllMACs()) { + + // perform optimization, get resulting parameters and optimization stats Stats stats; const APParams params = optimize(mac, stats); - if (!filter(stats.usedFingerprins, mac)) { + + // filter based on stats (option to ignore/filter some access-points) + if (!filter(stats, mac)) { res.push_back(APParamsMAC(mac, params)); errSum += stats.error_db; ++errCnt; } else { - std::cout << "ignored due to filter!" << std::endl; + Log::add(name, "ignoring opt-result for AP " + mac.asString() + " due to filter"); + //std::cout << "ignored due to filter!" << std::endl; } + } const float avgErr = errSum / errCnt; @@ -222,8 +239,8 @@ namespace WiFiOptimizer { LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z LeOpt::MinMax(-50, -30), // txp - LeOpt::MinMax(1, 4), // exp - LeOpt::MinMax(-15, -0), // waf + LeOpt::MinMax(1, 4), // exp + LeOpt::MinMax(-15, -0), // waf }; diff --git a/sensors/radio/setup/WiFiOptimizerPerFloor.h b/sensors/radio/setup/WiFiOptimizerPerFloor.h new file mode 100644 index 0000000..0969f56 --- /dev/null +++ b/sensors/radio/setup/WiFiOptimizerPerFloor.h @@ -0,0 +1,141 @@ +#ifndef WIFIOPTIMIZERPERFLOOR_H +#define WIFIOPTIMIZERPERFLOOR_H + +#include "WiFiOptimizerLogDistCeiling.h" +#include "../model/WiFiModelPerFloor.h" +#include "../../../floorplan/v2/FloorplanHelper.h" +#include + +#define WITH_DEBUG_PLOT +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include +#endif + +/** + * uses the log-distance model, but one per floor. + * the model is optimized using all fingerprints that belong to this floor + */ +class WiFiOptimizerPerFloor { + + WiFiModelPerFloor* mdl = nullptr; + + WiFiFingerprints fps; + + Floorplan::IndoorMap* map; + std::mutex mtx; + +#ifdef WITH_DEBUG_PLOT + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementColorPoints pts; + K::GnuplotSplotElementLines lines; +#endif + +public: + + WiFiOptimizerPerFloor(Floorplan::IndoorMap* map) : map(map) { + + // the overall model (contains one sub-model per floor) + mdl = new WiFiModelPerFloor(map); + + #ifdef WITH_DEBUG_PLOT + splot.add(&pts); pts.setPointSize(1); pts.setPointType(7); + splot.add(&lines); + BBox3 bb = FloorplanHelper::getBBox(map); + splot.getAxisX().setRange(bb.getMin().x, bb.getMax().x); + splot.getAxisY().setRange(bb.getMin().y, bb.getMax().y); + splot.getAxisZ().setRange(bb.getMin().z, bb.getMax().z); + #endif + + } + + + /** make the given fingerprint known to the optimizer */ + void addFingerprint(const WiFiFingerprint& fp) { + fps.add(fp); + } + + WiFiModelPerFloor* optimizeAll() { + + const VAPGrouper vg = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); + + // optimize each floor on its own + //for (WiFiModelPerFloor::ModelForFloor& mdlForFloor : mdl->getFloorModels()) { + for (size_t i = 0; i < map->floors.size(); ++i) { + + const Floorplan::Floor* floor = map->floors[i]; + + // 1) create a new optimizer for the current floor + WiFiOptimizer::LogDistCeiling opt(map, vg); + + // 2) create the model for this floor + mtx.lock(); + WiFiModelLogDistCeiling* mdlForFloor = new WiFiModelLogDistCeiling(map); + mdl->add(mdlForFloor, floor); + mtx.unlock(); + + // 3) get the floor's bbox and adjust the z-region (needed for museum in Rothenburg) + BBox3 bb = FloorplanHelper::getBBox(floor); + bb.setMinZ(floor->atHeight+0.25); + bb.setMaxZ(floor->atHeight+2); + + // 4) find all fingerprints that belong to the floor/model and add them to the optimizer + for (const WiFiFingerprint& fp : fps.getFingerprints()) { + //if (mdlForFloor.matches(fp.pos_m.z)) { + // std::cout << fp.pos_m.z << std::endl; + // opt.addFingerprint(fp); + //} + if (bb.contains(fp.pos_m)) { + //if (fp.pos_m.z >= floor->atHeight && fp.pos_m.z < floor->atHeight+floor->height) { + std::cout << fp.pos_m.z << std::endl; + opt.addFingerprint(fp); + + #ifdef WITH_DEBUG_PLOT + pts.add(K::GnuplotPoint3(fp.pos_m.x, fp.pos_m.y, fp.pos_m.z), i); + #endif + + } + } + + #ifdef WITH_DEBUG_PLOT + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + for (const Point2 pt : poly->poly.points) { + lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight)); + } + lines.splitFace(); lines.splitFace(); + for (const Point2 pt : poly->poly.points) { + lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight+floor->height)); + } + lines.splitFace(); lines.splitFace(); + } + gp.draw(splot); + gp.flush(); + pts.clear(); + lines.clear(); + sleep(1); + #endif + + // 5) run the optimizer + const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.MIN_2_FPS); + + // 6) add all optimized APs to the floor's model + for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { + // model is per-floor. so model cant optimize waf.. set to VERY HIGH manually + const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); + mdlForFloor->addAP(ap.mac, entry); + } + + } + + return mdl; + + } + +}; + + +#endif // WIFIOPTIMIZERPERFLOOR_H diff --git a/wifi/estimate/ray3/FloorplanMesh.h b/wifi/estimate/ray3/FloorplanMesh.h index a8de51c..bf159e2 100644 --- a/wifi/estimate/ray3/FloorplanMesh.h +++ b/wifi/estimate/ray3/FloorplanMesh.h @@ -78,7 +78,7 @@ namespace Ray3D { }; /** DEBUG: convert to .obj file code for exporting */ - OBJData toOBJ(const std::string& name) { + OBJData toOBJ(const std::string name) { bool swapYZ = true; int nVerts = 1;