From 25e9b823ebac36ac123a38b8f90a54da39d902cf Mon Sep 17 00:00:00 2001 From: kazu Date: Fri, 24 Mar 2017 13:31:31 +0100 Subject: [PATCH] started with eval --- EvalData.h | 52 +++++++++ EvalWalk.h | 32 ++++++ EvalWiFi.h | 266 ++++++++++++++++++++++++++++++++++++++++++++++ Helper.h | 29 +++++ Plotty.h | 46 +++++++- Settings.h | 37 +++++++ TestMapEarthReg.h | 73 +++++++++++++ main.cpp | 45 ++++++-- 8 files changed, 567 insertions(+), 13 deletions(-) create mode 100644 EvalData.h create mode 100644 EvalWiFi.h create mode 100644 Helper.h create mode 100644 Settings.h create mode 100644 TestMapEarthReg.h diff --git a/EvalData.h b/EvalData.h new file mode 100644 index 0000000..15ef0c8 --- /dev/null +++ b/EvalData.h @@ -0,0 +1,52 @@ +#ifndef EVALDATA_H +#define EVALDATA_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/setup/WiFiOptimizerLogDistCeiling.h" + +#include "Indoor/sensors/radio/VAPGrouper.h" + +#include "Indoor/floorplan/v2/Floorplan.h" +#include "Indoor/floorplan/v2/FloorplanReader.h" +#include "Indoor/floorplan/v2/FloorplanHelper.h" +#include "Indoor/floorplan/v2/FloorplanCeilings.h" + +#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h" + +#include "Indoor/sensors/offline/FileReader.h" +#include "Helper.h" + + +class EvalData { + +public: + + /** + * read path + * dump gps measurements to cout + * dump gps measurements to googleLines for googleMaps vis + */ + static void dumpGPSforPath(const std::string& fPath) { + + std::cout << "dumping GPS for " << fPath << std::endl; + + Offline::FileReader fr(fPath); + + std::vector poss; + for (const auto gps : fr.gps) { + std::cout << gps.ts << ":" << gps.data.asString() << std::endl; + EarthPos pos(gps.data.lat, gps.data.lon, gps.data.alt); + poss.push_back(pos); + } + LeHelper::writeGoogleLine(poss); + + } + + +}; + +#endif // EVALDATA_H diff --git a/EvalWalk.h b/EvalWalk.h index bf269ae..5fc5054 100644 --- a/EvalWalk.h +++ b/EvalWalk.h @@ -1,4 +1,36 @@ #ifndef EVALWALK_H #define EVALWALK_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/setup/WiFiOptimizerLogDistCeiling.h" + +#include "Indoor/sensors/radio/VAPGrouper.h" + +#include "Indoor/floorplan/v2/Floorplan.h" +#include "Indoor/floorplan/v2/FloorplanReader.h" +#include "Indoor/floorplan/v2/FloorplanHelper.h" +#include "Indoor/floorplan/v2/FloorplanCeilings.h" + +#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h" + +#include "Indoor/sensors/offline/FileReader.h" +#include "Helper.h" + +class EvalWalk { + +public: + + EvalWalk() { + + + + + } + +}; + #endif // EVALWALK_H diff --git a/EvalWiFi.h b/EvalWiFi.h new file mode 100644 index 0000000..ff1f82b --- /dev/null +++ b/EvalWiFi.h @@ -0,0 +1,266 @@ +#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/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 "Plotty.h" +#include "CSV.h" + +#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; + +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); + + } + + void fixedParams(const float txp, const float exp, const float waf) { + + // how to handle VAPs + vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + + // setup the model + WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map); + wiModel->loadAPs(map, *vap, txp, exp, waf, false); + this->wiModel = wiModel; + + // fire + run(); + + } + +private: + + /** is the given mac one of a FHWS ap? */ + bool isFHWS_AP(const MACAddress& mac) { + return mac.asString().substr(0,5) == "D8:84"; + } + + void run() { + + Plotty* plot = new Plotty(map); + Line path; + plot->setGroundTruth(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; + + // debug output + std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl; + + // perform vap grouping + const WiFiMeasurements mes = vap->group(_mes); + + + // error calculation + auto func = [&] (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 = 6.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 (!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); + + // adjust + prob *= p; + + } + + const double err = -prob; + return err; + + }; + + std::minstd_rand gen; + std::uniform_real_distribution distX(mapBBox.getMin().x, mapBBox.getMax().x); + std::uniform_real_distribution distY(mapBBox.getMin().y, mapBBox.getMax().y); + std::uniform_real_distribution distZ(mapBBox.getMin().z, mapBBox.getMax().z); + + // initializer for the optimizer: random position within the map's bbox + auto init = [&] (const int childIdx, float* params) { + params[0] = distX(gen); + params[1] = distY(gen); + params[2] = distZ(gen); + }; + + + float params[3]; + K::NumOptAlgoGenetic opt(3); + opt.setPopulationSize(200); + opt.setMaxIterations(20); + opt.calculateOptimum(func, params, init); + + std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl; + path.add(Point3(params[0], params[1], params[2])); + + // draw a smoothed version of th epath + plot->pathEst.clear(); + for (const Point3 p : path.getAverage(1)) { + const K::GnuplotPoint3 gp3(p.x, p.y, p.z); + plot->pathEst.add(gp3); + } + plot->plot(); + + + } + + } + +// // 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/Helper.h b/Helper.h new file mode 100644 index 0000000..4a3396d --- /dev/null +++ b/Helper.h @@ -0,0 +1,29 @@ +#ifndef LEHELPER_H +#define LEHELPER_H + +#include +#include + +class LeHelper { + +public: + + static void writeGoogleLine(std::vector& poss) { + const std::string file = "/apps/android/workspace/OTHER2017/osm/googleLine.js"; + std::ofstream out(file); + out << "function getLines() {return [\n"; + out.precision(15); + + for (const EarthPos& pos : poss) { + out << "\t{" << "lat:" << pos.lat << ", lng:" << pos.lon << "},\n"; + } + + out << "];}"; + out.flush(); + out.close(); + + } + +}; + +#endif // HELPER_H diff --git a/Plotty.h b/Plotty.h index 3b599ef..a7b15e2 100644 --- a/Plotty.h +++ b/Plotty.h @@ -1,19 +1,33 @@ #ifndef PLOTTY_H #define PLOTTY_H +#include +#include + +#include +#include +#include +#include +#include +#include +#include + class Plotty { public: + Floorplan::IndoorMap* map; K::Gnuplot gp; K::GnuplotSplot splot; K::GnuplotSplotElementPoints points; K::GnuplotSplotElementColorPoints cpoints; - K::GnuplotSplotElementLines lines; + K::GnuplotSplotElementLines pathReal; + K::GnuplotSplotElementLines pathEst; + K::GnuplotSplotElementLines mapOutline; public: - Plotty(Floorplan::IndoorMap* map) { + Plotty(Floorplan::IndoorMap* map) : map(map) { //gp << "set view equal xy\n"; @@ -25,25 +39,39 @@ public: gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; // draw floorplan - splot.add(&lines); + mapOutline.setColorHex("#888888"); + splot.add(&mapOutline); for (Floorplan::Floor* floor : map->floors) { for (Floorplan::FloorObstacle* obs : floor->obstacles) { Floorplan::FloorObstacleLine* line = dynamic_cast(obs); if (line) { const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight); const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight); - lines.addSegment(p1, p2); + mapOutline.addSegment(p1, p2); } } for (Floorplan::Stair* s : floor->stairs) { for (const Floorplan::StairPart& sp : s->getParts()) { const K::GnuplotPoint3 p1(sp.start.x, sp.start.y, sp.start.z + floor->atHeight); const K::GnuplotPoint3 p2(sp.end.x, sp.end.y, sp.end.z + floor->atHeight); - lines.addSegment(p1, p2); + mapOutline.addSegment(p1, p2); } } + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + gp << "set object polygon from "; + for (size_t i = 0; i < poly->poly.points.size(); ++i) { + if (i > 0) {gp << " to ";} + const Point2 pt = poly->poly.points[i]; + gp << pt.x << "," << pt.y << "," << floor->atHeight << " "; + } + gp << " fs solid fc rgb " << ( poly->outdoor ? "'#ddffdd'" : "'#dddddd'"); + gp << "\n"; + } } + splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#0000ff"); + splot.add(&pathEst); + splot.add(&points); points.setPointType(7); points.setPointSize(0.5); @@ -93,6 +121,14 @@ public: gp << "set title '" << title << "'\n"; } + void setGroundTruth(const std::vector indices) { + const std::vector path = FloorplanHelper::getGroundTruth(map, indices); + pathReal.clear(); + for (const Point3& p : path) { + pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z)); + } + } + void plot() { gp.draw(splot); gp.flush(); diff --git a/Settings.h b/Settings.h new file mode 100644 index 0000000..c0b1a31 --- /dev/null +++ b/Settings.h @@ -0,0 +1,37 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include + +namespace Settings { + + const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/"; + + 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"; + + + const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL37.xml"; + const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat"; + + namespace GroundTruth { + + // IPIN2017 [toni] + //const std::vector path1 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; + //const std::vector path2 = {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 path3 = {31, 32, 33, 34, 35, 36, 37, 38}; + + // OTHER2017 [frank] + const std::vector path1 = {40, 41, 42, 43, 44, 45, 1, 0, 46, 47, 48, 49, 50, 51, 52}; + 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}; + + } + +} + +#endif // SETTINGS_H diff --git a/TestMapEarthReg.h b/TestMapEarthReg.h new file mode 100644 index 0000000..3089081 --- /dev/null +++ b/TestMapEarthReg.h @@ -0,0 +1,73 @@ +#ifndef TESTMAPEARTHREG_H +#define TESTMAPEARTHREG_H + +/** + * test map<-> earth registration + * by converting the map to lon/lat and exporting to google-maps + */ +void testMapEarthReg(const std::string& fMap) { + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(fMap); + + std::ofstream outOSM("/apps/android/workspace/OTHER2017/osm/osm.txt"); + outOSM << "lat lon title icon iconSize iconOffset\n"; + outOSM.precision(20); + + // our markers + std::ofstream outGooglePOI("/apps/android/workspace/OTHER2017/osm/googlePOI.js"); + outGooglePOI << "function getPOI() {return [\n"; + outGooglePOI.precision(20); + + int cnt = 0; + for (Floorplan::EarthPosMapPos* em : map->earthReg.correspondences) { + //mapCenter += em->posOnMap_m.xy(); + //earthCenter.lat += em->posOnEarth.lat; + //earthCenter.lon += em->posOnEarth.lon; + outOSM << em->posOnEarth.lat << "\t" << em->posOnEarth.lon << "\t" << "title" << "\t" << "osm.png" << "\t" << "16,16" << "\t" << "-8,-8" << "\n"; + outGooglePOI << "\t{lat:" << em->posOnEarth.lat << ", lng:" << em->posOnEarth.lon << "},\n"; + ++cnt; + } + + outGooglePOI << "];}"; + outGooglePOI.flush(); + + outOSM.flush(); + + //const EarthMapping em1(Point3(mapCenter.x, mapCenter.y, 0), earthCenter, 0); + + // construct mapper + const EarthMapping em(map); + + // export building's first floor + std::ofstream outGooglePoly("/apps/android/workspace/OTHER2017/osm/googlePoly.js"); + outGooglePoly << "function getPoly() {return [\n"; + outGooglePoly.precision(20); + + // export to google maps lon/lat + Floorplan::Floor* f1 = map->floors[1]; + for (Floorplan::FloorOutlinePolygon* poly : f1->outline) { + + if (poly->method != Floorplan::OutlineMethod::ADD) {continue;} + if (poly->outdoor) {continue;} + + outGooglePoly << "[\n"; + for (int i = 0; i <= poly->poly.points.size(); ++i) { + const Point2 p = poly->poly.points[i % poly->poly.points.size()]; + const EarthPos ep = em.mapToWorld(Point3(p.x, p.y, 0)); + outGooglePoly << "\t{lat:" << ep.lat << ", lng:" << ep.lon << "},\n"; + } + outGooglePoly << "],"; + + + } + + + outGooglePoly << "];}"; + outGooglePoly.flush(); + + + int i = 0; (void) i; + +} + +#endif // TESTMAPEARTHREG_H diff --git a/main.cpp b/main.cpp index 35c1b4b..24ff843 100644 --- a/main.cpp +++ b/main.cpp @@ -13,24 +13,53 @@ #include #include #include +#include + +#include "Settings.h" #include "EvalCompareOpt.h" - #include "EvalApOpt.h" +#include "EvalWalk.h" +#include "EvalData.h" +#include "EvalWiFi.h" + +#define BREAK raise(SIGTRAP); int main(void) { - const std::string fMap = "/apps/android/workspace/OTHER2017/data/SHL35.xml"; - const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat"; + // test gps within data files + if (1 == 0) { + EvalData::dumpGPSforPath(Settings::path1a); BREAK; + EvalData::dumpGPSforPath(Settings::path1b); BREAK; + EvalData::dumpGPSforPath(Settings::path2a); BREAK; + EvalData::dumpGPSforPath(Settings::path2b); BREAK; + } + + // test wifi within data files + if (1 == 1) { + //EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2); + EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path1); + //ew1.fixedParams(-40, 2.5, -8); BREAK; + ew1.fixedParams(-64.5905, 1.25988, -2.47863); BREAK; + } + + + // run walking + + + // run earthmapping + //testMapEarthReg(fMap); // EvalApOpt eval(fMap, fCalib); // eval.optAll(); - { - EvalCompareOptAllFixed allFixed(fMap, fCalib); - //allFixed.fixedPosFixedParamsForAll(); - //allFixed.fixedPosOptParamsForAll(); - //allFixed.fixedPosOptParamsForEach(); + + // run evaluation + if (1 == 0) { + EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib); + allFixed.fixedPosFixedParamsForAll(); + allFixed.fixedPosOptParamsForAll(); + allFixed.fixedPosOptParamsForEach(); allFixed.optPosOptParamsForEach(); }