388 lines
9.6 KiB
C++
388 lines
9.6 KiB
C++
#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 <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 "Structs.h"
|
|
#include "plots/Plotty.h"
|
|
#include "plots/PlotErrTime.h"
|
|
#include "plots/PlotErrFunc.h"
|
|
|
|
#include "CSV.h"
|
|
|
|
#include <unordered_set>
|
|
|
|
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;
|
|
|
|
// error in probability
|
|
PlotErrFunc* pef2;
|
|
PlotErrTime* pet2;
|
|
|
|
Plotty* plot;
|
|
|
|
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::MEDIAN);
|
|
|
|
pef = new PlotErrFunc("\\small{error (m)}", "\\small{measurements (\\%)}");
|
|
pef->showMarkers(false);
|
|
|
|
pef2 = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}");
|
|
pef2->showMarkers(false);
|
|
|
|
pet = new PlotErrTime("2", "3", "");
|
|
pet->getPlot().setRangeY(K::GnuplotAxisRange(0, 40));
|
|
|
|
pet2 = new PlotErrTime("2", "3", "");
|
|
|
|
plot = new Plotty(map);
|
|
plot->buildFloorplan();
|
|
plot->setGroundTruth(gtIndices);
|
|
|
|
}
|
|
|
|
void load(const std::string& xmlFile, const std::string& name) {
|
|
|
|
// setup the model
|
|
WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map);
|
|
wiModel->loadXML(xmlFile);
|
|
this->wiModel = wiModel;
|
|
|
|
// fire
|
|
build(name);
|
|
|
|
}
|
|
|
|
// 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:
|
|
|
|
void build(const std::string& name) {
|
|
|
|
static int idx = -1; ++idx;
|
|
|
|
|
|
const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);
|
|
|
|
Line<Point3> path;
|
|
K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines();
|
|
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];
|
|
|
|
// 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);
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
// 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
|
|
//const float err_m = gt.xy().getDistance(curEst.xy());
|
|
const float err_m = gt.getDistance(curEst);
|
|
stats->add(err_m);
|
|
|
|
float gtFloat[3] = {gt.x, gt.y, gt.z};
|
|
const double probOnGT = -func(gtFloat);
|
|
const double logProbOnGT = -log(probOnGT);
|
|
statsProbOnGT->add(logProbOnGT);
|
|
|
|
// plot err
|
|
pet->addErr(ts, err_m, idx);
|
|
pet2->addErr(ts, logProbOnGT/mes.entries.size(), idx);
|
|
|
|
// fire
|
|
plot->plot();
|
|
|
|
// pet->plot();
|
|
// pef->plot();
|
|
|
|
pet2->plot();
|
|
pef2->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
|