diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..bddc432 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,85 @@ +# Usage: +# Create build folder, like RC-build next to RobotControl and WifiScan folder +# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' +# make + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) + +# select build type +SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) + +PROJECT(Other2017) + +IF(NOT CMAKE_BUILD_TYPE) + MESSAGE(STATUS "No build type selected. Default to Debug") + SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../ +) + + +FILE(GLOB HEADERS + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h + ./tests/data/* + ./tests/data/*/* + ./tests/data/*/*/* +) + + +FILE(GLOB SOURCES + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ../KLib/inc/tinyxml/tinyxml2.cpp +) + + + +# system specific compiler flags +ADD_DEFINITIONS( + + -std=gnu++11 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + + -g3 + -O0 + -march=native + + -DWITH_TESTS + -DWITH_ASSERTIONS + -DWITH_DEBUG_LOG + + +) + + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} + gtest + pthread +) + diff --git a/CSV.h b/CSV.h new file mode 100644 index 0000000..3b268d8 --- /dev/null +++ b/CSV.h @@ -0,0 +1,64 @@ +#ifndef CSV_H +#define CSV_H + + +class CSV { + + int row = 0; + const char sep = ';'; + std::stringstream ss; + +public: + + void add(const OptStats& stats, const Floorplan::Ceilings& ceilings) { + + ss << ++row << sep; + ss << stats.mac.asString() << sep; + ss << (stats.knownToMap() ? 1 : 0) << sep; + + ss << stats.getEstErrorAvg() << sep; + ss << stats.getEstErrorMaxNeg().getError_db() << sep; + ss << stats.getEstErrorMaxPos().getError_db() << sep; + + ss << stats.getEstPos().x << sep; + ss << stats.getEstPos().y << sep; + ss << stats.getEstPos().z << sep; + + if (stats.knownToMap()) { + ss << stats.realPos.x << sep; + ss << stats.realPos.y << sep; + ss << stats.realPos.z << sep; + ss << stats.getRealEstDiff().length() << sep; + ss << ceilings.numCeilingsBetween(stats.realPos, stats.getEstPos()) << sep; + ss << stats.name << sep; + } + + ss << "\n"; + + } + + /** an AP that is known to the map, but unseen during fingerprinting [should not happen] */ + void addUnseen(const MACAddress& mac) { + ss << ++row << sep; + ss << mac.asString() << sep; + ss << "AP_IN_MAP_BUT_UNSEEN_DURING_FINGERPRINTING" << sep; + ss << "\n"; + } + + /** ap is seen, but there are not enough fingerprints [should not happen] */ + void addNotEnoughFingerprints(const OptStats& stats) { + ss << ++row << sep; + ss << stats.mac.asString() << sep; + ss << "NOT_ENOUGH_FINGERPRINTS" << sep; + } + + void dump() { + std::cout << ss.str() << std::endl; + } + + + +}; + + +#endif // CSV_H diff --git a/EvalApOpt.h b/EvalApOpt.h new file mode 100644 index 0000000..c4abf7e --- /dev/null +++ b/EvalApOpt.h @@ -0,0 +1,247 @@ +#ifndef EVALAPOPT_H +#define EVALAPOPT_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/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 + +void plot(const K::SimpleHistogram& h) { + + K::Gnuplot* gp = new K::Gnuplot(); + + K::GnuplotPlot plot; + K::GnuplotPlotElementHistogram hist; plot.add(&hist); + + hist.add(h); + + gp->draw(plot); + gp->flush(); + +} + + +class EvalApOpt { + + Floorplan::IndoorMap* map; + WiFiFingerprints* calib; + VAPGrouper* vap; + WiFiOptimizer::LogDistCeiling* opt; + Floorplan::Ceilings ceilings; + CSV csv; + +public: + + /** ctor with map and fingerprints */ + EvalApOpt(const std::string& mapFile, const std::string& fpFile) { + + // load floorplan + map = Floorplan::Reader::readFromFile(mapFile); + + // load fingerprints + calib = new WiFiFingerprints(fpFile); + + // 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); + + // some ceiling calculations + ceilings = Floorplan::Ceilings(map); + + // get average error + //opt->optimizeAll(opt->MIN_5_FPS); + return; + + } + + /** optimize all access-points given by the scanned fingerprints */ + std::vector optAll() { + + std::vector results; + CSV csv; + K::Statistics statsAbs; + K::Statistics statsSigned; + + // all APs known to the map + std::unordered_set mapAPs; + for (const Floorplan::Floor* f : map->floors) { + for (const Floorplan::AccessPoint* ap : f->accesspoints) { + mapAPs.insert(ap->mac); + } + } + + // process each AP seen during fingerprinting + for (const MACAddress& mac : opt->getAllMACs()) { + + // but: skip non-FHWS accesspoints [smartphones, portable stuff, ..] + if (mac.asString().substr(0,5) != "D8:84") { + std::cerr << "skipping non FHWS AP: " << mac.asString() << std::endl << std::endl; + continue; + } + + // optimize + const OptStats result = optOne(mac); + + // optimization will only work out, if there are enough fingerprints + if (result.opt.usedFingerprins < 10) { + csv.addNotEnoughFingerprints(result); + std::cerr << "skipping AP due to not enough fingerprints: " << mac.asString() << std::endl << std::endl; + continue; + } + + + results.push_back(result); + dumpStats(result); + csv.add(result, ceilings); + //if (mac == MACAddress("D8:84:66:4A:22:B0")) {plot(result);} // extrema + + K::SimpleHistogram hist(3.0); + for (const auto err : result.opt.errors) { + //std::cout << err.getError_db() << std::endl; + hist.add(err.getError_db(), 1.0); + statsAbs.add( std::abs(err.getError_db()) ); + statsSigned.add( err.getError_db() ); + } + //::plot(hist); + + int i = 0; (void) i; + + } + + // average error + std::cout << "error signed: " << statsSigned.asString() << std::endl; + std::cout << "error unsigned: " << statsAbs.asString() << std::endl; + + // all APs known to the map but invisible during fingerprinting [should not happen] + std::unordered_set mapUnseen = mapAPs; + for (const MACAddress& mac : opt->getAllMACs()) {mapUnseen.erase(mac);} + for (const MACAddress& mac : mapUnseen) {csv.addUnseen(mac);} + + csv.dump(); + + return results; + + } + + /** analyze the given optimization result obtained by optOne(mac) */ + void dumpStats(const OptStats& stats) { + + std::cout << "stats " << stats.mac.asString() << std::endl; + + if (!stats.knownToMap()) { + std::cerr << "\t" << "AP NOT KNOWN TO MAP!" << std::endl << std::endl; + return; + } + + std::cout << "\t" << "error: " << stats.getEstErrorAvg() << " dB" << std::endl; + std::cout << "\t" << "max neg: " << stats.getEstErrorMaxNeg().getError_db() << " dB" << std::endl; + std::cout << "\t" << "max pos: " << stats.getEstErrorMaxPos().getError_db() << " dB" << std::endl; + + std::cout << "\t" << "real position: " << stats.realPos.asString() << " m" << std::endl; + std::cout << "\t" << "est. position: " << stats.getEstPos().asString() << " m" << std::endl; + std::cout << "\t" << "real/est difference: " << stats.getRealEstDiff().length() << " m" << std::endl; + std::cout << "\t" << "ceiling delta: " << ceilings.numCeilingsBetween(stats.realPos, stats.getEstPos()) << std::endl; + + std::cout << std::endl; + + } + + + + /** plot the given optimization result obtained by optOne(mac) */ + void plot(const OptStats& stats) { + + // plot + Plotty plot(map); + + if (stats.name != "") { + plot.setTitle(stats.name); + plot.addLabel("x realPos", stats.realPos); + } else { + plot.setTitle(stats.mac.asString() + " ??????"); + } + + plot.addLabel("x estPos", Point3(stats.params.x, stats.params.y, stats.params.z)); + + for (const WiFiOptimizer::ErrorAtPosition& err : stats.opt.errors) { + + const float delta = err.model_rssi - err.scan_rssi; + const Point3 pos = err.pos_m; + + plot.cpoints.add(K::GnuplotPoint3(pos.x, pos.y, pos.z), delta); + + } + + plot.setPaletteRedBlue(); + + // for (const WiFiOptimizer::APParamsMAC& ap : list.get()) { + // const K::GnuplotPoint3 p3(ap.params.x, ap.params.y, ap.params.z); + // points.add(p3); + // const Floorplan::AccessPoint* fap = FloorplanHelper::getAP(map, ap.mac); + // std::string lbl = (fap) ? (fap->name) : (ap.mac.asString()); + // if (!fap) { + // gp << "set label '" << lbl << "' at " << ap.params.x+1 << "," << ap.params.y+1 << "," << ap.params.z+0.3 << "\n"; + + // std::cout << "AP missing in Map: " << ap.mac.asString() << " @ " << ap.params.x << "," << ap.params.y << "," << ap.params.z << std::endl; + // } + // } + + plot.plot(); + return; + + } + + + OptStats optOne(const MACAddress& mac) { + + OptStats optStats; + optStats.mac = mac; + + const WiFiOptimizer::LogDistCeiling::APParams params = opt->optimize(mac, optStats.opt); + const std::vector fps = calib->getFingerprintsFor(vap->getBaseMAC(mac)); + + optStats.params = params; + + // fetch AP from MAP + std::pair ap = FloorplanHelper::getAP(map, mac); + + // ap known to the map? + if (ap.first) { + optStats.name = ap.first->name; + optStats.realPos = ap.first->getPos(ap.second); + } + + return optStats; + + } + + +}; + +#endif // EVALAPOPT_H diff --git a/EvalCompareOpt.h b/EvalCompareOpt.h new file mode 100644 index 0000000..952dc7b --- /dev/null +++ b/EvalCompareOpt.h @@ -0,0 +1,342 @@ +#ifndef EVALCOMPAREOPT_H +#define EVALCOMPAREOPT_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" + +using APAtFloor = std::pair; + +/** + * compare different optimzation levels + * fixed ap pos / fixed params + * fixed ap pos / optimized params + * optimized ap pos / optimized params [+/- WAF] + */ +class EvalCompareOpt { + +protected: + + Floorplan::IndoorMap* map; + WiFiFingerprints* calib; + VAPGrouper* vap; + Floorplan::Ceilings ceilings; + std::vector mapAPs; + WiFiOptimizer::Base* base; + + + /** ctor with map and fingerprints */ + EvalCompareOpt(const std::string& mapFile, const std::string& fpFile) { + + // load floorplan + map = Floorplan::Reader::readFromFile(mapFile); + + // load fingerprints + calib = new WiFiFingerprints(fpFile); + + // some ceiling calculations + ceilings = Floorplan::Ceilings(map); + + // all APs within the map + mapAPs = FloorplanHelper::getAPs(map); + + // how to group VAPs + vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + + // used to aggreagate fingerprints + base = new WiFiOptimizer::Base(*vap); + base->addFingerprints(*calib); + + } + + /** get the error for the given AP at the provided location */ + K::Statistics analyzeErrorForAPs(const std::vector& aps) { + + K::Statistics statsAbs; + + // process each AP + for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : aps) { + analyzeErrorForAP(ap, statsAbs); + } + + // done + //std::cout << "overall error: " << std::endl << statsAbs.asString() << std::endl; + return statsAbs; + + } + + /** get the error for the given AP at the provided location */ + void analyzeErrorForAP(const MACAddress& mac, const Point3 pos, const float txp, const float exp, const float waf, K::Statistics& dstAbs) { + + WiFiOptimizer::LogDistCeiling::APParams params; + params.exp = exp; + params.txp = txp; + params.waf = waf; + params.x = pos.x; + params.y = pos.y; + params.z = pos.z; + + const WiFiOptimizer::LogDistCeiling::APParamsMAC ap(mac, params); + + analyzeErrorForAP(ap, dstAbs); + + } + + /** get the error for the given AP at the provided location */ + void analyzeErrorForAP(const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap, K::Statistics& dstAbs) { + + // + const WiFiOptimizer::LogDistCeiling::APParams& params = ap.params; + const MACAddress& mac = ap.mac; + + // always using the same model + WiFiModelLogDistCeiling model(map); + model.addAP(mac, WiFiModelLogDistCeiling::APEntry(params.getPos(), params.txp, params.exp, params.waf), false); + + // get all fingerprints for the given AP + const std::vector entries = base->getFingerprintsFor(mac); + + // stats + K::Statistics stats; + + // process each fingerprint for this ap + for (const WiFiOptimizer::RSSIatPosition& reading : entries) { + + // get the model-estimation for the fingerprint's position + const float rssiModel = model.getRSSI(mac, reading.pos_m); + + // difference between estimation and measurement + const float diff = std::abs(rssiModel - reading.rssi); + + // adjust + stats.add(std::abs(diff)); + dstAbs.add(std::abs(diff)); + + } + + // show + //std::cout << " --- " << mac.asString() << ": " << stats.asString() << std::endl; + + } + + + +}; + +/** fixed ap pos, fixed ap params */ +class EvalCompareOptAllFixed : public EvalCompareOpt { + +private: + + // looks good + float txp = -40; + float exp = 2.65; + float waf = -6.5; + +public: + + EvalCompareOptAllFixed(const std::string& mapFile, const std::string& fpFile) : EvalCompareOpt(mapFile, fpFile) { + + } + + + + /** get the error when using the given 3 params for ALL aps */ + K::Statistics getStatsAll(const float txp, const float exp, const float waf) { + + // all access points with params + std::vector aps; + + // construct vector containing each AP within the map + add fixed parameters + for (const APAtFloor& mapAP : mapAPs) { + + WiFiOptimizer::LogDistCeiling::APParams params; + params.exp = exp; + params.txp = txp; + params.waf = waf; + params.x = mapAP.first->getPos(mapAP.second).x; + params.y = mapAP.first->getPos(mapAP.second).y; + params.z = mapAP.first->getPos(mapAP.second).z; + + const MACAddress mac = MACAddress(mapAP.first->mac); + + WiFiOptimizer::LogDistCeiling::APParamsMAC ap(mac, params); + aps.push_back(ap); + + } + + return analyzeErrorForAPs(aps); + + } + + /** calculate error for fixed positions and fixed constants */ + void fixedPosFixedParamsForAll() { + + // fire + std::cout << "----------------------------------------------------" << std::endl; + std::cout << "AP POS FROM MAP, FIXED TXP/EXP/WAF FOR ALL APS" << std::endl; + std::cout << getStatsAll(txp, exp, waf).asString() << std::endl; + std::cout << std::endl; + + } + + /** calculate error for fixed positions and optimized constants, but the same 3 for all APs */ + void fixedPosOptParamsForAll() { + + auto func = [&] (const float* params) { + return getStatsAll(params[0], params[1], params[2]).getAvg(); + }; + + // use simplex + float params[3] = {-40, 2, -8}; + K::NumOptAlgoDownhillSimplex opt(3); + opt.setMaxIterations(50); + opt.setNumRestarts(10); + opt.calculateOptimum(func, params); + + // use genetic +// K::NumOptAlgoGenetic opt(3); +// opt.setPopulationSize(100); +// opt.setMaxIterations(50); +// opt.setValRange({1, 0.1, 0.2}); +// opt.setElitism(0.05f); +// opt.setMutation(0.25); +// opt.calculateOptimum(func, params); + + std::cout << "----------------------------------------------------" << std::endl; + std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF: THE SAME FOR ALL APS" << std::endl; + std::cout << "params: " << params[0] << "," << params[1] << "," << params[2] << std::endl; + std::cout << getStatsAll(params[0], params[1], params[2]).asString() << std::endl; + std::cout << std::endl; + + } + + /** calculate error for fixed positions and optimized constants, each AP on its own */ + void fixedPosOptParamsForEach() { + + K::Statistics _dstAbs; + + // construct vector containing each AP within the map + add fixed parameters + for (const APAtFloor& mapAP : mapAPs) { + + // fixed + const MACAddress mac(mapAP.first->mac); + const Point3 pos = mapAP.first->getPos(mapAP.second); + + // opt-func for one AP + auto func = [&] (const float* params) { + K::Statistics dstAbs; + analyzeErrorForAP(mac, pos, params[0], params[1], params[2], dstAbs); + return dstAbs.getAvg(); + }; + + // use simplex + float params[3] = {-40, 2, -8}; + K::NumOptAlgoDownhillSimplex opt(3); + opt.setMaxIterations(50); + opt.setNumRestarts(10); + opt.calculateOptimum(func, params); + + // use genetic [usually not better!] +// K::NumOptAlgoGenetic opt(3); +// opt.setPopulationSize(100); +// opt.setMaxIterations(50); +// opt.setValRange({1, 0.1, 0.2}); +// opt.setElitism(0.05f); +// opt.setMutation(0.25); +// opt.calculateOptimum(func, params); + + // local stats + K::Statistics tmp; + analyzeErrorForAP(mac, pos, params[0], params[1], params[2], tmp); + + // adjust global error with the resulting params + std::cout << "--" << mac.asString() << " params: " << params[0] << "," << params[1] << "," << params[2] << " err: " << tmp.getAvg() << std::endl; + analyzeErrorForAP(mac, pos, params[0], params[1], params[2], _dstAbs); + + } + + std::cout << "----------------------------------------------------" << std::endl; + std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl; + std::cout << _dstAbs.asString() << std::endl; + std::cout << std::endl; + + } + + /** calculate error for fixed positions and optimized constants, each AP on its own */ + void optPosOptParamsForEach() { + + K::Statistics _dstAbs; + + // construct vector containing each AP within the map + add fixed parameters + for (const APAtFloor& mapAP : mapAPs) { + + // fixed + const MACAddress mac(mapAP.first->mac); + const Point3 pos = mapAP.first->getPos(mapAP.second); + + // opt-func for one AP + auto func = [&] (const float* params) { + K::Statistics dstAbs; + analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], dstAbs); + return dstAbs.getAvg(); + }; + + // use simplex + float params[6] = {40, 40, 5, -40, 2, -8}; +// K::NumOptAlgoDownhillSimplex opt(6); +// opt.setMaxIterations(50); +// opt.setNumRestarts(10); +// opt.calculateOptimum(func, params); + + using LeOpt = K::NumOptAlgoRangeRandom; + const std::vector valRegion = { + LeOpt::MinMax(-20, 120), // x + LeOpt::MinMax(-20, 120), // y + LeOpt::MinMax( -5, 17), // z + LeOpt::MinMax(-50, -30), // txp + LeOpt::MinMax( 1, 4), // exp + LeOpt::MinMax(-15, -0), // waf + }; + + + K::NumOptAlgoRangeRandom opt(valRegion); + opt.setPopulationSize(500); + opt.setNumIerations(150); + opt.calculateOptimum(func, params); + + // local stats + K::Statistics tmp; + analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], tmp); + + // adjust global error with the resulting params + std::cout << "--" << mac.asString() << " params: " << params[0] << "," << params[1] << "," << params[2] << "," << params[3] << "," << params[4] << "," << params[5] << " err: " << tmp.getAvg() << std::endl; + analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], _dstAbs); + + } + + std::cout << "----------------------------------------------------" << std::endl; + std::cout << "OPTIMIZING POS/TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl; + std::cout << _dstAbs.asString() << std::endl; + std::cout << std::endl; + + } + +}; + + + + +#endif // EVALCOMPAREOPT_H diff --git a/EvalWalk.h b/EvalWalk.h new file mode 100644 index 0000000..bf269ae --- /dev/null +++ b/EvalWalk.h @@ -0,0 +1,4 @@ +#ifndef EVALWALK_H +#define EVALWALK_H + +#endif // EVALWALK_H diff --git a/Plotty.h b/Plotty.h new file mode 100644 index 0000000..3b599ef --- /dev/null +++ b/Plotty.h @@ -0,0 +1,103 @@ +#ifndef PLOTTY_H +#define PLOTTY_H + +class Plotty { + +public: + + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementPoints points; + K::GnuplotSplotElementColorPoints cpoints; + K::GnuplotSplotElementLines lines; + +public: + + Plotty(Floorplan::IndoorMap* map) { + + //gp << "set view equal xy\n"; + + gp << "set palette model RGB\n"; + //gp << "set palette defined (0 '#0000ff', 1 '#ff0000')\n"; + gp << "r(x) = (x < 0) ? 0 : (x/2)\n"; + gp << "g(x) = 0\n"; + gp << "b(x) = (x > 0) ? 0 : (-x/2)\n"; + gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; + + // draw floorplan + splot.add(&lines); + 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); + } + } + 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); + } + } + } + + splot.add(&points); + points.setPointType(7); + points.setPointSize(0.5); + + splot.add(&cpoints); + cpoints.setPointSize(2); + cpoints.setPointType(7); + + } + + + void setPaletteRedBlue() { + + float max = -9999; + float min = +9999; + for (const auto& e : cpoints.get()) { + if (e.color > max) {max = e.color;} + if (e.color < min) {min = e.color;} + } + setPaletteRedBlue(min, max); + + } + + void setPaletteRedBlue(const float blueVal, const float redVal) { + + // we need to map the range from [blueVal:redVal] to [0:1] + const float min = blueVal; + const float max = redVal; + const float range = (max - min); + const float center01 = (0-min)/range; + + // values above 0 dB = red + // values below 0 dB = blue + gp << "set palette model RGB\n"; + gp << "cen01 = " << center01 << "\n"; + gp << "r(x) = (x < cen01) ? 0 : ((x-cen01) / (1-cen01))\n"; + gp << "g(x) = 0\n"; + gp << "b(x) = (x > cen01) ? 0 : (1 - (x/cen01))\n"; + gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; + } + + void addLabel(const std::string& txt, const Point3 pos) { + gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n"; + } + + void setTitle(const std::string& title) { + gp << "set title '" << title << "'\n"; + } + + void plot() { + gp.draw(splot); + gp.flush(); + } + +}; + +#endif // PLOTTY_H diff --git a/Structs.h b/Structs.h new file mode 100644 index 0000000..5e11cef --- /dev/null +++ b/Structs.h @@ -0,0 +1,47 @@ +#ifndef STRUCTS_H +#define STRUCTS_H + +#include +#include + +struct OptStats { + + const Point3 unknown = Point3(-1, -1, -1); + + MACAddress mac; + std::string name; // AP's name from map (if known) + Point3 realPos = unknown; // real position from map (if known) + WiFiOptimizer::LogDistCeiling::Stats opt; + WiFiOptimizer::LogDistCeiling::APParams params; + + const Point3 getRealEstDiff() const { + if (realPos == unknown) {throw Exception("error!");} + return realPos - getEstPos(); + } + + bool knownToMap() const { + return name != ""; + } + + float getEstErrorAvg() const { + return opt.error_db; + } + + + /** get the AP's estimation position */ + Point3 getEstPos() const { + return Point3(params.x, params.y, params.z); + } + + WiFiOptimizer::ErrorAtPosition getEstErrorMaxNeg() const { + return opt.getEstErrorMaxNeg(); + } + + WiFiOptimizer::ErrorAtPosition getEstErrorMaxPos() const { + return opt.getEstErrorMaxPos(); + } + + +}; + +#endif // STRUCTS_H diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..35c1b4b --- /dev/null +++ b/main.cpp @@ -0,0 +1,97 @@ +#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/floorplan/v2/Floorplan.h" +#include "Indoor/floorplan/v2/FloorplanReader.h" +#include "Indoor/floorplan/v2/FloorplanHelper.h" + +#include +#include +#include +#include + +#include "EvalCompareOpt.h" + +#include "EvalApOpt.h" + +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"; + +// EvalApOpt eval(fMap, fCalib); +// eval.optAll(); + + { + EvalCompareOptAllFixed allFixed(fMap, fCalib); + //allFixed.fixedPosFixedParamsForAll(); + //allFixed.fixedPosOptParamsForAll(); + //allFixed.fixedPosOptParamsForEach(); + allFixed.optPosOptParamsForEach(); + } + + + +// Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/android/workspace/OTHER2017/data/SHL33a.xml"); + + +// WiFiFingerprints calib("/apps/android/workspace/OTHER2017/data/wifi_fp.dat"); + +// VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + +// WiFiOptimizer opt(map, vap); +// opt.addFingerprints(calib); +// WiFiOptimizer::APParamsList list = opt.optimizeAll(); + +// for (Floorplan::Floor* floor : map->floors) { +// for (Floorplan::AccessPoint* ap : floor->accesspoints) { +// const WiFiOptimizer::APParamsMAC* params = list.get(ap->mac); +// if (params) { +// const float delta = ap->getPos(floor).getDistance(Point3(params->params.x, params->params.y, params->params.z)); +// std::cout << ap->mac << ": " << delta << "m" << std::endl; +// } +// } +// } + + + +// static K::Gnuplot gp; +// gp << "set view equal xy\n"; + +// K::GnuplotSplot splot; +// K::GnuplotSplotElementPoints points; splot.add(&points); points.setPointType(7); points.setPointSize(0.5); +// K::GnuplotSplotElementLines lines; splot.add(&lines); + +// 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); +// } +// } +// } + +// for (const WiFiOptimizer::APParamsMAC& ap : list.get()) { +// const K::GnuplotPoint3 p3(ap.params.x, ap.params.y, ap.params.z); +// points.add(p3); +// const Floorplan::AccessPoint* fap = FloorplanHelper::getAP(map, ap.mac); +// std::string lbl = (fap) ? (fap->name) : (ap.mac.asString()); +// if (!fap) { +// gp << "set label '" << lbl << "' at " << ap.params.x+1 << "," << ap.params.y+1 << "," << ap.params.z+0.3 << "\n"; + +// std::cout << "AP missing in Map: " << ap.mac.asString() << " @ " << ap.params.x << "," << ap.params.y << "," << ap.params.z << std::endl; +// } +// } + +// gp.draw(splot); +// gp.flush(); + +// int i = 0; (void) i; + +}