409 lines
10 KiB
C++
Executable File
409 lines
10 KiB
C++
Executable File
#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 <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
|
|
|
|
#include <KLib/math/statistics/Statistics.h>
|
|
#include <Indoor/math/MovingAVG.h>
|
|
#include <Indoor/math/MovingMedian.h>
|
|
|
|
#include "../Structs.h"
|
|
#include "../plots/Plotty.h"
|
|
#include "../plots/PlotErrTime.h"
|
|
#include "../plots/PlotErrFunc.h"
|
|
#include "../plots/PlotWiFiGroundProb.h"
|
|
//#include "CSV.h"
|
|
|
|
#include <unordered_set>
|
|
#include <thread>
|
|
#include "../Settings.h"
|
|
|
|
template <typename T> class Line {
|
|
|
|
private:
|
|
|
|
std::vector<T> elements;
|
|
|
|
public:
|
|
|
|
void add(const T& elem) {
|
|
elements.push_back(elem);
|
|
}
|
|
|
|
std::vector<T> getAverage(const int size) {
|
|
std::vector<T> 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<int> gtIndices;
|
|
|
|
// error in meter
|
|
PlotErrFunc* pef;
|
|
PlotErrTime* pet;
|
|
MovingAVG<float> mavgMeter = MovingAVG<float>(10);
|
|
|
|
// error in probability
|
|
PlotErrFunc* pef2;
|
|
PlotErrTime* pet2;
|
|
|
|
Plotty* plot;
|
|
|
|
//PlotWiFiGroundProb* groundProb;
|
|
|
|
public:
|
|
|
|
/** ctor with map and fingerprints */
|
|
EvalWiFi(const std::string& mapFile, const std::string& fPath, const std::vector<int> 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, false);
|
|
|
|
pef2 = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}");
|
|
pef2->showMarkers(false, false);
|
|
|
|
pet = new PlotErrTime("walktime (seconds)", "\\small{error (m)}", "");
|
|
pet->getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 25));
|
|
pet->getPlot().getKey().setVisible(true);
|
|
|
|
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);
|
|
|
|
|
|
}
|
|
|
|
void writeTeX(const std::string& name) {
|
|
|
|
pet->writeEpsTex(Settings::fPathGFX + "/wifi_eval_" + name + ".tex", K::GnuplotSize(8.6, 3.8));
|
|
pet->writeCodeTo(Settings::fPathGFX + "/wifi_eval_" + name + ".gp");
|
|
pet->plot();
|
|
|
|
}
|
|
|
|
|
|
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<Point3> path;
|
|
K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines();
|
|
gpPath->setStroke(stroke);
|
|
plot->splot.add(gpPath);
|
|
|
|
K::Statistics<float>* stats = new K::Statistics<float>();
|
|
K::Statistics<float>* statsProbOnGT = new K::Statistics<float>();
|
|
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<double>::getProbability(rssi_model, sigma, rssi_scan);
|
|
//const double p = Distribution::Region<double>::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<double>::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<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
|
|
// std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
|
|
// std::uniform_real_distribution<float> 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<float> opt(3);
|
|
// opt.setPopulationSize(400);
|
|
// opt.setMaxIterations(20);
|
|
// opt.calculateOptimum(func, params, init);
|
|
|
|
|
|
// USE RANGE RANDOM WITH COOLING
|
|
using LeOpt = K::NumOptAlgoRangeRandom<float>;
|
|
const std::vector<LeOpt::MinMax> 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<float> 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);
|
|
}
|
|
|
|
static int xxx = 0; ++xxx;
|
|
|
|
// 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 in meter
|
|
const float err_m = gt.xy().getDistance(curEst.xy()); // 2D
|
|
//const float err_m = gt.getDistance(curEst); // 3D
|
|
stats->add(err_m);
|
|
mavgMeter.add(err_m);
|
|
if (xxx % 3 == 0) {
|
|
pet->addErr(ts, mavgMeter.get(), 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());
|
|
statsProbOnGT->add(logProbOnGTNorm);
|
|
pet2->addErr(ts, logProbOnGTNorm, idx);
|
|
|
|
if (xxx%4 == 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
|