556 lines
16 KiB
C++
556 lines
16 KiB
C++
#ifndef EVALWIFIPATHMETHODS_H
|
|
#define EVALWIFIPATHMETHODS_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 by using the same model but various probability functions */
|
|
class EvalWiFiPathMethods {
|
|
|
|
private:
|
|
|
|
Floorplan::IndoorMap* map;
|
|
BBox3 mapBBox;
|
|
|
|
VAPGrouper* vap = nullptr;
|
|
|
|
K::Statistics<float>* statsOrig_m;
|
|
K::Statistics<float>* statsOther_m;
|
|
PlotErrFunc* pef_m;
|
|
|
|
K::Statistics<float>* statsOrig_p;
|
|
K::Statistics<float>* statsOther_p;
|
|
PlotErrFunc* pef_p;
|
|
|
|
K::Statistics<float>* statsOrig_c;
|
|
K::Statistics<float>* statsOther_c;
|
|
PlotErrFunc* pef_c;
|
|
|
|
WiFiModel* wiModel = nullptr;
|
|
|
|
std::vector<Point3> randomLoc;
|
|
std::vector<AccessPoint> allAPs;
|
|
|
|
int idx = -2;
|
|
|
|
public:
|
|
|
|
/** ctor with map and fingerprints */
|
|
EvalWiFiPathMethods(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 (meter)", "measurements (%)");
|
|
pef_m->showMarkers(false);
|
|
|
|
pef_p = new PlotErrFunc("-log(p(..))", "measurements (%)");
|
|
pef_p->showMarkers(false);
|
|
pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
|
|
|
|
|
|
pef_c = new PlotErrFunc("cross error", "measurements (%)");
|
|
pef_c->showMarkers(false);
|
|
pef_c->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
|
|
|
|
|
|
|
|
|
|
// generate some random locations within the walkable area
|
|
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);
|
|
for (const Floorplan::Floor* f : map->floors) {
|
|
|
|
std::vector<Point3> floorLoc;
|
|
|
|
while (floorLoc.size() < 100) {
|
|
|
|
const Point3 pt(distX(gen), distY(gen), f->atHeight + 1.3); // human above ground
|
|
|
|
|
|
for (const Floorplan::FloorOutlinePolygon* poly : f->outline) {
|
|
|
|
// ensure the random samples are nicely placed along the floor
|
|
for (const Point3& p : floorLoc) {
|
|
if (p.getDistance(pt) < 4) {continue;}
|
|
}
|
|
|
|
HelperPoly hp(*poly);
|
|
if (hp.contains(pt.xy()*100)) {
|
|
floorLoc.push_back(pt);
|
|
}
|
|
}
|
|
}
|
|
|
|
for (const Point3 p : floorLoc) {randomLoc.push_back(p);}
|
|
|
|
}
|
|
|
|
// Plotty p(map);
|
|
// p.buildFloorplan();
|
|
// for (const Point3 pt :randomLoc) {
|
|
// p.points.add({pt.x, pt.y, pt.z});
|
|
// }
|
|
// p.plot();
|
|
// sleep(1000);
|
|
|
|
|
|
// // generate some random locations within the building
|
|
// 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);
|
|
// for (int i = 0; i < 250; ++i) {
|
|
// const Point3 pt(distX(gen), distY(gen), distZ(gen));
|
|
// for (const Point3& p : randomLoc) {
|
|
// if (p.getDistance(pt) < 3) {continue;} // ensure the random samples are nicely placed
|
|
// }
|
|
// randomLoc.push_back(pt);
|
|
// }
|
|
|
|
}
|
|
|
|
|
|
|
|
void loadModel(const std::string& xmlFile, const std::string& modelName, const std::string& modeNameA, const std::string& modeNameB) {
|
|
|
|
idx += 2;
|
|
|
|
WiFiModelFactory fac(map);
|
|
|
|
// setup the model
|
|
this->wiModel = fac.loadXML(xmlFile);
|
|
|
|
statsOrig_m = new K::Statistics<float>();
|
|
statsOrig_p = new K::Statistics<float>();
|
|
statsOrig_c = new K::Statistics<float>();
|
|
|
|
statsOther_m = new K::Statistics<float>();
|
|
statsOther_p = new K::Statistics<float>();
|
|
statsOther_c = new K::Statistics<float>();
|
|
|
|
pef_m->add(modeNameA, statsOrig_m);
|
|
pef_m->add(modeNameB, statsOther_m);
|
|
pef_m->getPlot().getAxisX().setTicsLabelFormat("%h m");
|
|
|
|
pef_p->add(modeNameA, statsOrig_p);
|
|
pef_p->add(modeNameB, statsOther_p);
|
|
|
|
pef_c->add(modeNameA, statsOrig_c);
|
|
pef_c->add(modeNameB, statsOther_c);
|
|
pef_c->getPlot().getAxisX().setTicsLabelFormat("%h %%");
|
|
|
|
|
|
this->allAPs = wiModel->getAllAPs();
|
|
for (const AccessPoint& ap : allAPs) {
|
|
std::cout << ap.getMAC().asString() << std::endl;
|
|
}
|
|
int i = 0; (void) i;
|
|
|
|
}
|
|
|
|
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 writeGP(const std::string& path, const std::string& name) {
|
|
writeGP(*pef_m, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_meter");
|
|
writeGP(*pef_c, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_cross");
|
|
writeGP(*pef_p, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + 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(-4);
|
|
pef.getPlot().getAxisY().setLabelOffset(2.5, 0);
|
|
pef.getPlot().getAxisY().setTicsStep(0, 25, 95);
|
|
pef.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0,95));
|
|
|
|
pef.getPlot().getAxisX().setLabel("");
|
|
pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
|
|
pef.getGP() << "set lmargin 4.5\n";
|
|
pef.getGP() << "set tmargin 0.1\n";
|
|
pef.getGP() << "set rmargin 0.4\n";
|
|
pef.getGP() << "set bmargin 2.0\n";
|
|
pef.writeCodeTo(file+".gp");
|
|
pef.plot();
|
|
pef.writeCodeTo("");
|
|
}
|
|
|
|
void walk(const std::string& fPath, const std::vector<int> gtIndices) {
|
|
|
|
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 funcOrig = [&] (const float* params) -> double { return errFuncOrig(params, mes); };
|
|
auto funcOther = [&] (const float* params) -> double { return errFuncOther(params, mes); };
|
|
|
|
// parameters (x,y,z);
|
|
float paramsOrig[3] = {0,0,0};
|
|
float paramsOther[3] = {0,0,0};
|
|
|
|
// 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(400);
|
|
opt.setNumIerations(30);
|
|
|
|
opt.calculateOptimum(funcOrig, paramsOrig);
|
|
opt.calculateOptimum(funcOther, paramsOther);
|
|
|
|
|
|
// estimation
|
|
//std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
|
|
const Point3 curEstOrig(paramsOrig[0], paramsOrig[1], paramsOrig[2]);
|
|
const Point3 curEstOther(paramsOther[0], paramsOther[1], paramsOther[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 errOrig_m = gt.getDistance(curEstOrig); // 3D
|
|
const float errOther_m = gt.getDistance(curEstOther); // 3D
|
|
|
|
statsOrig_m->add(errOrig_m);
|
|
//pet_m.addErr(ts, errOrig_m, idx+0);
|
|
statsOther_m->add(errOther_m);
|
|
//pet_m.addErr(ts, errOther_m, idx+1);
|
|
|
|
// // error in -log(p)
|
|
float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3}; // human above ground
|
|
|
|
{
|
|
const double probOnGT_a = -errFuncOrig(gtFloat, mes);
|
|
const double logProbOnGT_a = -std::log10(probOnGT_a);
|
|
const double logProbOnGTNorm_a = (logProbOnGT_a/mes.entries.size());
|
|
statsOrig_p->add(logProbOnGTNorm_a);
|
|
}
|
|
|
|
{
|
|
const double probOnGT_b = -errFuncOther(gtFloat, mes);
|
|
const double logProbOnGT_b = -std::log10(probOnGT_b);
|
|
const double logProbOnGTNorm_b = (logProbOnGT_b/mes.entries.size());
|
|
|
|
// break on huge errors
|
|
if (logProbOnGTNorm_b > 2) {
|
|
// std::cout << "-------------------------------"<< std::endl;
|
|
// std::cout << gt.asString() << std::endl;
|
|
// getVeto(gt, mes);
|
|
// throw "123";
|
|
}
|
|
statsOther_p->add(logProbOnGTNorm_b);
|
|
}
|
|
|
|
checkCross(funcOrig, gt, statsOrig_c);
|
|
checkCross(funcOther, gt, statsOther_c);
|
|
|
|
//pet_p.addErr(ts, logProbOnGTNorm, idx);
|
|
|
|
static int xx = 0; ++xx;
|
|
|
|
if ( (xx % 20) == 0) {
|
|
pef_p->plot();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
pef_m->plot();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
pef_c->plot();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
|
|
|
|
void checkCross(std::function<double(const float* params)> func, Point3 gt, K::Statistics<float>* stats) {
|
|
|
|
const float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3}; // above ground
|
|
const double orig_p = -func(gtFloat);
|
|
float bigger = 0;
|
|
int cnt = 0;
|
|
|
|
float errSum = 0;
|
|
int errCnt = 0;
|
|
|
|
for (const Point3 pt : randomLoc) {
|
|
|
|
if (pt.getDistance(gt) > 20) {continue;}
|
|
float params[3] = {pt.x, pt.y, pt.z};
|
|
const double other_p = -func(params);
|
|
|
|
if (pt.getDistance(gt) >= 3) { // at least 3 meter away
|
|
if (other_p > orig_p*0.75) {++bigger;}
|
|
}
|
|
|
|
if (pt.getDistance(gt) >=5) { // at least 5 meter away
|
|
if (other_p > orig_p) {errSum += pt.getDistance(gt); ++errCnt;}
|
|
}
|
|
++cnt;
|
|
}
|
|
|
|
|
|
// for (float rad = 0; rad < 2*M_PI; rad += 0.1) {
|
|
// Point3 p = gt + Point3(std::cos(rad), std::sin(rad), 0) * 5; // 5 meters around gt
|
|
// float params[3] = {p.x, p.y, p.z};
|
|
// const double other_p = func(params);
|
|
// if (other_p >orig_p) {++bigger;}
|
|
// ++cnt;
|
|
// }
|
|
|
|
const float factor = bigger / (float)cnt;
|
|
//const float factor = errSum / (errCnt+0.00001f);
|
|
|
|
// break on huge errors
|
|
if (factor > 0.12) {
|
|
// std::cout << "-------------------------------"<< std::endl;
|
|
// std::cout << gt.asString() << std::endl;
|
|
// func(gtFloat);
|
|
// const float gtFloat2[3] = {gt.x, gt.y, gt.z-1};
|
|
// func(gtFloat2);
|
|
// throw "123";
|
|
}
|
|
|
|
stats->add(factor * 100); // scale to "%"
|
|
|
|
}
|
|
|
|
double errFuncOrig(const float* params, const WiFiMeasurements& mes) {
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
double errFuncOther(const float* params, const WiFiMeasurements& mes) {
|
|
|
|
// 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;
|
|
double error = 0;
|
|
int cnt = 0;
|
|
|
|
//const auto comp = [] (const WiFiMeasurement& m1, const WiFiMeasurement& m2) {return m1.getRSSI() < m2.getRSSI();};
|
|
//const auto& min = std::min_element(mes.entries.begin(), mes.entries.end(), comp);
|
|
|
|
// 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;}
|
|
|
|
// TESTING
|
|
//if (m.getAP().getMAC() == min->getAP().getMAC()) {continue;}
|
|
//if (m.getRSSI() < -90) {continue;}
|
|
//if (m.getRSSI() > -55) {continue;}
|
|
|
|
const float rssi = m.getRSSI();
|
|
// const volatile float min = -100;
|
|
// const volatile float max = -40;
|
|
// const volatile float val = (m.getRSSI() - min) / (max-min);
|
|
|
|
// 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
|
|
//double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
|
|
//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);
|
|
//const double p = Distribution::Triangle<double>::getProbability(rssi_model, sigma*2, rssi_scan);
|
|
//double p = Distribution::Exponential<double>::getProbability(1, std::abs(rssi_model-rssi_scan));
|
|
|
|
double p = Distribution::Exponential<double>::getProbability(1.0, std::abs(rssi_model-rssi_scan));
|
|
|
|
//p = 0.85 * p + 0.15 * (1.0-p);
|
|
//p = 0.95 * p + 0.05;
|
|
|
|
// const double diff = std::abs(rssi_model - rssi_scan);
|
|
//// error += diff;
|
|
// ++cnt;
|
|
// if (diff < 3) {error += 0.001;}
|
|
// else if (diff < 5) {error += 0.001;}
|
|
// else if (diff < 8) {error += 0.001;}
|
|
// else if (diff < 10) {error += 3.0;}
|
|
// else if (diff < 15) {error += 5.0;}
|
|
// else {error += 15.0;}
|
|
// error += std::pow((diff / 10.0f), 5) / 10.0f;
|
|
|
|
// adjust
|
|
prob *= p;
|
|
|
|
}
|
|
|
|
const double err = -prob;
|
|
return err;
|
|
|
|
}
|
|
|
|
double getVeto(const Point3& pos_m, const WiFiMeasurements& obs) const {
|
|
|
|
struct APR {
|
|
AccessPoint ap;
|
|
float rssi;
|
|
APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;}
|
|
};
|
|
|
|
std::vector<APR> all;
|
|
for (const AccessPoint& ap : allAPs) {
|
|
const float rssi = wiModel->getRSSI(ap.getMAC(), pos_m);
|
|
if (rssi != rssi) {throw Exception("should not happen?!");} // unknown to the model
|
|
all.push_back(APR(ap, rssi));
|
|
}
|
|
|
|
// stort by RSSI
|
|
auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;};
|
|
std::sort(all.begin(), all.end(), comp);
|
|
|
|
int numVetos = 0;
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
const APR& apr = all[i];
|
|
if (apr.rssi < -80) {continue;}
|
|
const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
|
|
if (!mes) {
|
|
++numVetos;
|
|
continue;
|
|
//std::cout << apr.ap.getMAC().asString() << ":" << apr.rssi << std::endl;
|
|
}
|
|
const float rssiScan = mes->getRSSI();
|
|
const float rssiModel = apr.rssi;
|
|
const float diff = std::abs(rssiScan - rssiModel);
|
|
if (diff > 20) {++numVetos;}
|
|
}
|
|
|
|
return (numVetos < 1) ? (0.999) : (0.001);
|
|
|
|
// if (numVetos == 0) {return 0.70;}
|
|
// if (numVetos == 1) {return 0.70;}
|
|
// else {return 0.01;}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
#endif // EVALWIFIPATHMETHODS_H
|