263 lines
7.5 KiB
C++
Executable File
263 lines
7.5 KiB
C++
Executable File
#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 <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"
|
|
|
|
/** evaluate just the wifi error for several given paths */
|
|
class EvalWiFiPaths {
|
|
|
|
private:
|
|
|
|
Floorplan::IndoorMap* map;
|
|
BBox3 mapBBox;
|
|
|
|
VAPGrouper* vap = nullptr;
|
|
|
|
K::Statistics<float>* stats_m;
|
|
PlotErrFunc* pef_m;
|
|
|
|
K::Statistics<float>* 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("error (m)", "measurements (%)");
|
|
pef_m->showMarkers(false, false);
|
|
|
|
pef_p = new PlotErrFunc("-log(p(..))", "measurements (%)");
|
|
pef_p->showMarkers(false, false);
|
|
pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
|
|
|
|
}
|
|
|
|
void dumpStats() {
|
|
for (const auto& entry : pef_m->getEntries()) {
|
|
const K::Statistics<float>* stats = entry.stats;
|
|
std::cout << stats->getQuantile(0.25) << " " << stats->getMedian() << " " << stats->getQuantile(0.75) << " " << stats->getAvg() << std::endl;
|
|
}
|
|
}
|
|
|
|
void writeGP(const std::string& path, const std::string& name) {
|
|
writeGP(*pef_m, K::GnuplotSize(8.6, 3.3), path + "/" + name + "_meter");
|
|
writeGP(*pef_p, K::GnuplotSize(8.6, 3.3), path + "/" + 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(7);
|
|
pef.getPlot().getAxisY().setLabelOffset(2.5, 0);
|
|
//pef.getPlot().getAxisY().setTicsStep(0, 25, 95);
|
|
pef.getPlot().getAxisY().setTicsStep({0, 25, 50, 75, 95});
|
|
pef.getPlot().getAxisX().setTicsLabelFormat("%h m"); // CHECK!
|
|
|
|
// MANUAL AXIS RANGE SETTINGS
|
|
pef.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(0,25));
|
|
pef.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0,95));
|
|
|
|
pef.getPlot().getAxisX().setLabel("");
|
|
pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
|
|
pef.getPlot().getMargin().set(4.5, 0.4, 0.1, 2.0);
|
|
pef.writePlotToFile(file+".gp");
|
|
pef.plot();
|
|
pef.writePlotToFile("");
|
|
}
|
|
|
|
|
|
|
|
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<float>();
|
|
stats_p = new K::Statistics<float>();
|
|
|
|
pef_m->add(name, stats_m);
|
|
pef_p->add(name, stats_p);
|
|
|
|
}
|
|
|
|
void walks (const std::vector<std::string> files, const std::vector<std::vector<int>> gtIndicies) {
|
|
for (size_t i = 0; i < files.size(); ++i) {
|
|
walk(files[i], gtIndicies[i]);
|
|
}
|
|
}
|
|
|
|
void walk(const std::string& fPath, const std::vector<int> 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<double>::getProbability(rssi_model, sigma, rssi_scan);
|
|
//const double p = Distribution::Region<double>::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<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);
|
|
|
|
// 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
|