started with eval
This commit is contained in:
266
EvalWiFi.h
Normal file
266
EvalWiFi.h
Normal file
@@ -0,0 +1,266 @@
|
||||
#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 "Plotty.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;
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
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:
|
||||
|
||||
/** is the given mac one of a FHWS ap? */
|
||||
bool isFHWS_AP(const MACAddress& mac) {
|
||||
return mac.asString().substr(0,5) == "D8:84";
|
||||
}
|
||||
|
||||
void run() {
|
||||
|
||||
Plotty* plot = new Plotty(map);
|
||||
Line<Point3> path;
|
||||
plot->setGroundTruth(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;
|
||||
|
||||
// debug output
|
||||
std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl;
|
||||
|
||||
// perform vap grouping
|
||||
const WiFiMeasurements mes = vap->group(_mes);
|
||||
|
||||
|
||||
// error calculation
|
||||
auto func = [&] (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 = 6.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 (!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);
|
||||
|
||||
// adjust
|
||||
prob *= p;
|
||||
|
||||
}
|
||||
|
||||
const double err = -prob;
|
||||
return err;
|
||||
|
||||
};
|
||||
|
||||
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) {
|
||||
params[0] = distX(gen);
|
||||
params[1] = distY(gen);
|
||||
params[2] = distZ(gen);
|
||||
};
|
||||
|
||||
|
||||
float params[3];
|
||||
K::NumOptAlgoGenetic<float> opt(3);
|
||||
opt.setPopulationSize(200);
|
||||
opt.setMaxIterations(20);
|
||||
opt.calculateOptimum(func, params, init);
|
||||
|
||||
std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
|
||||
path.add(Point3(params[0], params[1], params[2]));
|
||||
|
||||
// draw a smoothed version of th epath
|
||||
plot->pathEst.clear();
|
||||
for (const Point3 p : path.getAverage(1)) {
|
||||
const K::GnuplotPoint3 gp3(p.x, p.y, p.z);
|
||||
plot->pathEst.add(gp3);
|
||||
}
|
||||
plot->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
|
||||
Reference in New Issue
Block a user