506 lines
14 KiB
C++
506 lines
14 KiB
C++
#ifndef EVALCOMPAREOPT2_H
|
|
#define EVALCOMPAREOPT2_H
|
|
|
|
#include <KLib/math/statistics/Statistics.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/setup/WiFiOptimizerLogDistCeiling.h"
|
|
|
|
#include "Indoor/sensors/radio/VAPGrouper.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 "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
|
|
#include "Indoor/sensors/offline/FileReader.h"
|
|
|
|
#include "Helper.h"
|
|
|
|
#include <vector>
|
|
|
|
// FOR TESTING
|
|
#define FAST 1
|
|
#define QUALITY 2
|
|
#define MODE QUALITY
|
|
|
|
//#define DO_SHOW_FINGERPRINTS
|
|
|
|
class EvalCompareOpt2 {
|
|
|
|
int power = 1;
|
|
Floorplan::IndoorMap* map;
|
|
WiFiFingerprints calib;
|
|
VAPGrouper* vap;
|
|
Floorplan::Ceilings ceilings;
|
|
std::vector<APAtFloor> mapAPs;
|
|
|
|
public:
|
|
|
|
/** ctor with map and fingerprints */
|
|
EvalCompareOpt2(const std::string& mapFile, const std::string& fpFile, std::function<bool(const WiFiFingerprint& fp)> remove, bool removeStaircases = false) {
|
|
|
|
setup(mapFile);
|
|
|
|
// load fingerprints
|
|
calib = WiFiFingerprints(fpFile);
|
|
// if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);}
|
|
if (removeStaircases) {calib = LeHelper::removeStaircases(calib);}
|
|
// if (ignoreIndoor) {calib = LeHelper::removeIndoor(calib);}
|
|
calib = LeHelper::removeIf(calib, remove);
|
|
|
|
cleanup();
|
|
|
|
}
|
|
|
|
/** ctor with map and live-walk [ground-truth] */
|
|
EvalCompareOpt2(const std::string& mapFile, std::vector<std::pair<std::string, std::vector<int>>> walks) {
|
|
|
|
setup(mapFile);
|
|
|
|
|
|
// ensure each AP is present at least once
|
|
WiFiMeasurements mes;
|
|
for (const APAtFloor& apf : mapAPs) {
|
|
const Floorplan::AccessPoint* ap = apf.first;
|
|
WiFiMeasurement m(AccessPoint(MACAddress(ap->mac)), -100);
|
|
mes.entries.push_back(m);
|
|
}
|
|
WiFiFingerprint fpDummy(Point3(-50, -50, -50), mes);
|
|
calib.add(fpDummy);
|
|
|
|
for (const auto& it : walks) {
|
|
|
|
const std::string walkFile = it.first;
|
|
const std::vector<int> gtPathIndices = it.second;
|
|
|
|
// load "fingerprints" by matching ground-truth against live walk
|
|
Offline::FileReader reader(walkFile);
|
|
Offline::FileReader::GroundTruth gt = reader.getGroundTruth(map, gtPathIndices);
|
|
|
|
for (const auto& it : reader.getWiFiGroupedByTime()) {
|
|
const Timestamp ts = Timestamp::fromMS(it.ts); // time of the measurement
|
|
const WiFiMeasurements& mes = it.data; // wifi measurement
|
|
const Point3 gtPos_m = gt.get(ts); // location on the ground-truth during time of measuring
|
|
const WiFiFingerprint fp(gtPos_m, mes); // location + measurement = fingerprint
|
|
calib.add(fp);
|
|
}
|
|
|
|
}
|
|
|
|
cleanup();
|
|
|
|
}
|
|
|
|
|
|
private:
|
|
|
|
void setup(const std::string& mapFile) {
|
|
|
|
// load floorplan
|
|
map = Floorplan::Reader::readFromFile(mapFile);
|
|
|
|
// how to group VAPs
|
|
vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN);
|
|
|
|
// some ceiling calculations
|
|
ceilings = Floorplan::Ceilings(map);
|
|
|
|
// all APs within the map
|
|
mapAPs = FloorplanHelper::getAPs(map);
|
|
|
|
|
|
}
|
|
|
|
void cleanup() {
|
|
|
|
LeHelper::removeNonFHWS(calib);
|
|
|
|
// VAP-group fingerprints
|
|
for (WiFiFingerprint& fp : calib.getFingerprints()) {
|
|
fp.measurements = vap->group(fp.measurements);
|
|
}
|
|
|
|
// plot
|
|
#ifdef DO_SHOW_FINGERPRINTS
|
|
LeHelper::plot(map, calib);
|
|
#endif
|
|
|
|
}
|
|
|
|
public:
|
|
|
|
struct Result {
|
|
|
|
// the configured model
|
|
WiFiModelLogDistCeiling model;
|
|
|
|
// the resulting error
|
|
K::Statistics<float> errAvg;
|
|
K::Statistics<float> errSingle;
|
|
|
|
Result(Floorplan::IndoorMap* map) : model(map) {;}
|
|
|
|
};
|
|
|
|
|
|
Result fixedPosFixedParamsForAll() {
|
|
|
|
const float txp = -40;
|
|
const float exp = 2.65;
|
|
const float waf = -6.5;
|
|
|
|
Result res(map);
|
|
|
|
// process each AP
|
|
for (const auto _ap : mapAPs) {
|
|
const Floorplan::AccessPoint* ap = _ap.first;
|
|
const Floorplan::Floor* floor = _ap.second;
|
|
const MACAddress mac(ap->mac);
|
|
const Point3 pos_m = ap->getPos(floor);
|
|
res.model.addAP(mac, pos_m, txp, exp, waf, true);
|
|
K::Statistics<float> err = getError(mac, res.model);
|
|
res.errAvg.add(err.getAvg());
|
|
res.errSingle.add(err);
|
|
}
|
|
|
|
dump(res);
|
|
return res;
|
|
|
|
}
|
|
|
|
Result fixedPosOptParamsForAll() {
|
|
|
|
|
|
// get fingerprints for each AP [once]
|
|
std::unordered_map<MACAddress, std::vector<WiFiFingerprint>> fps;
|
|
for (const auto _ap : mapAPs) {
|
|
const Floorplan::AccessPoint* ap = _ap.first;
|
|
const MACAddress mac(ap->mac);
|
|
fps[mac] = calib.getFingerprintsFor(mac);
|
|
}
|
|
|
|
// resulting error
|
|
auto errFunc = [&] (const float txp, const float exp, const float waf) -> Result {
|
|
|
|
Result res(map);
|
|
|
|
// process each AP
|
|
for (const auto _ap : mapAPs) {
|
|
const Floorplan::AccessPoint* ap = _ap.first;
|
|
const Floorplan::Floor* floor = _ap.second;
|
|
const MACAddress mac(ap->mac);
|
|
const Point3 pos_m = ap->getPos(floor);
|
|
res.model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params
|
|
K::Statistics<float> err = getError(mac, res.model, fps[mac]);
|
|
res.errAvg.add(err.getAvg());
|
|
res.errSingle.add(err);
|
|
}
|
|
|
|
return res;
|
|
|
|
};
|
|
|
|
// to-be-optimized function
|
|
auto optFunc = [&] (const float* params) {
|
|
return whatToOpt(errFunc(params[0], params[1], params[2]).errSingle);
|
|
};
|
|
|
|
// use simplex
|
|
float params[3] = {-40, 2, -8};
|
|
K::NumOptAlgoDownhillSimplex<float> opt(3);
|
|
#if MODE == FAST
|
|
opt.setMaxIterations(50);
|
|
opt.setNumRestarts(10);
|
|
#elif MODE == QUALITY
|
|
opt.setMaxIterations(200);
|
|
opt.setNumRestarts(25);
|
|
#endif
|
|
opt.calculateOptimum(optFunc, params);
|
|
|
|
// // use genetic
|
|
// K::NumOptAlgoGenetic<float> opt(3);
|
|
// opt.setPopulationSize(100);
|
|
// opt.setMaxIterations(100);
|
|
// opt.setValRange({1, 0.1, 0.2});
|
|
// opt.setElitism(0.05f);
|
|
// opt.setMutation(0.25);
|
|
// opt.calculateOptimum(optFunc, params);
|
|
|
|
// get the error result for all APs
|
|
Result res = errFunc(params[0], params[1], params[2]);
|
|
|
|
// done
|
|
dump(res);
|
|
return res;
|
|
|
|
}
|
|
|
|
Result fixedPosOptParamsForEach() {
|
|
|
|
Result res(map);
|
|
|
|
// process each AP
|
|
for (const auto _ap : mapAPs) {
|
|
|
|
// fixed AP params
|
|
const Floorplan::AccessPoint* ap = _ap.first;
|
|
const Floorplan::Floor* floor = _ap.second;
|
|
const MACAddress mac(ap->mac);
|
|
const Point3 pos_m = ap->getPos(floor);
|
|
const std::vector<WiFiFingerprint> fps = getFingerprints(mac);
|
|
|
|
// resulting error
|
|
auto errFunc = [&] (const float txp, const float exp, const float waf) -> K::Statistics<float> {
|
|
WiFiModelLogDistCeiling model(map); // new, empty model for this AP
|
|
model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params
|
|
K::Statistics<float> err = getError(mac, model, fps); // calculate error among all fingerprints
|
|
return err;
|
|
};
|
|
|
|
// to-be-optimized function
|
|
auto optFunc = [&] (const float* params) {
|
|
return whatToOpt(errFunc(params[0], params[1], params[2]));
|
|
};
|
|
|
|
// use simplex
|
|
float params[3] = {-40, 2, -8};
|
|
K::NumOptAlgoDownhillSimplex<float> opt(3);
|
|
#if MODE == FAST
|
|
opt.setMaxIterations(50);
|
|
opt.setNumRestarts(10);
|
|
#elif MODE == QUALITY
|
|
opt.setMaxIterations(200);
|
|
opt.setNumRestarts(25);
|
|
#endif
|
|
opt.calculateOptimum(optFunc, params);
|
|
|
|
// // use genetic
|
|
// K::NumOptAlgoGenetic<float> opt(3);
|
|
// opt.setPopulationSize(100);
|
|
// opt.setMaxIterations(100);
|
|
// opt.setValRange({1, 0.1, 0.2});
|
|
// opt.setElitism(0.05f);
|
|
// opt.setMutation(0.25);
|
|
// opt.calculateOptimum(optFunc, params);
|
|
|
|
// get the error result for the current AP
|
|
K::Statistics<float> err = errFunc(params[0], params[1], params[2]);
|
|
res.errAvg.add(err.getAvg());
|
|
res.errSingle.add(err);
|
|
|
|
// add finalized params to the model
|
|
res.model.addAP(mac, pos_m, params[0], params[1], params[2], false);
|
|
|
|
}
|
|
|
|
// done
|
|
dump(res);
|
|
return res;
|
|
|
|
}
|
|
|
|
Result optPosOptParamsForEach() {
|
|
|
|
Result res(map);
|
|
|
|
// process each AP
|
|
for (const auto _ap : mapAPs) {
|
|
|
|
// fixed AP params
|
|
const Floorplan::AccessPoint* ap = _ap.first;
|
|
const MACAddress mac(ap->mac);
|
|
const std::vector<WiFiFingerprint> fps = getFingerprints(mac);
|
|
|
|
// resulting error
|
|
auto errFunc = [&] (const Point3 pos_m, const float txp, const float exp, const float waf) -> K::Statistics<float> {
|
|
WiFiModelLogDistCeiling model(map); // new, empty model for this AP
|
|
model.addAP(mac, pos_m, txp, exp, waf, false); // allow unsafe txp,exp,waf params
|
|
K::Statistics<float> err = getError(mac, model, fps); // calculate error among all fingerprints
|
|
return err;
|
|
};
|
|
|
|
// to-be-optimized function
|
|
auto optFunc = [&] (const float* params) {
|
|
const Point3 pos_m(params[0], params[1], params[2]);
|
|
return whatToOpt(errFunc(pos_m, params[3], params[4], params[5]));
|
|
};
|
|
|
|
// params
|
|
float params[6] = {0};
|
|
|
|
std::minstd_rand gen;
|
|
BBox3 mapBBox = FloorplanHelper::getBBox(map);
|
|
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);
|
|
params[3] = -40;
|
|
params[4] = 2;
|
|
params[5] = -8;
|
|
};
|
|
|
|
// // use genetic
|
|
// K::NumOptAlgoGenetic<float> opt(6);
|
|
// opt.setPopulationSize(300);
|
|
// opt.setMaxIterations(50);
|
|
// opt.setValRange({1, 1, 1, 1, 0.1, 0.2});
|
|
// opt.setElitism(0.05f);
|
|
// opt.setMutation(0.25);
|
|
// opt.calculateOptimum(optFunc, 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
|
|
LeOpt::MinMax(-50, -30), // txp
|
|
LeOpt::MinMax( 1, 4), // exp
|
|
LeOpt::MinMax(-15, -0), // waf
|
|
};
|
|
|
|
K::NumOptAlgoRangeRandom<float> opt(valRegion);
|
|
|
|
#if MODE == FAST
|
|
opt.setPopulationSize(100);
|
|
opt.setNumIerations(50);
|
|
#elif MODE == QUALITY
|
|
opt.setPopulationSize(500);
|
|
opt.setNumIerations(250);
|
|
#endif
|
|
|
|
opt.calculateOptimum(optFunc, params);
|
|
|
|
|
|
|
|
// K::NumOptAlgoDownhillSimplex<float> opt(6);
|
|
// opt.setMaxIterations(40);
|
|
// opt.setNumRestarts(20);
|
|
// opt.calculateOptimum(optFunc, params);
|
|
|
|
// get the error result for the current AP
|
|
const Point3 pos_m(params[0], params[1], params[2]);
|
|
K::Statistics<float> err = errFunc(pos_m, params[3], params[4], params[5]);
|
|
res.errAvg.add(err.getAvg());
|
|
res.errSingle.add(err);
|
|
|
|
// add finalized params to the model
|
|
res.model.addAP(mac, pos_m, params[3], params[4], params[5], false);
|
|
|
|
}
|
|
|
|
// done
|
|
dump(res);
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private:
|
|
|
|
// the stats to optimize
|
|
static inline float whatToOpt(const K::Statistics<float>& s) {
|
|
//return s.getAvg() + s.getStdDev() / 2 + s.getMax() / 4; // for dBm
|
|
//return s.getAvg() + s.getStdDev() * 2 + s.getMax() / 2; // for probability
|
|
//return s.getQuantile(0.96);
|
|
return s.getAvg();
|
|
}
|
|
|
|
std::vector<WiFiFingerprint> getFingerprints(const MACAddress& mac) {
|
|
|
|
// get all fingerprints where the given mac was seen
|
|
const std::vector<WiFiFingerprint> fps = calib.getFingerprintsFor(mac);
|
|
|
|
// sanity check
|
|
// if (fps.size() < 5) {throw Exception("not enought fingerprints for AP " + mac.asString());}
|
|
|
|
return fps;
|
|
|
|
}
|
|
|
|
|
|
|
|
void dump(const Result& res) {
|
|
std::cout << res.errSingle.asString() << std::endl;
|
|
std::cout << res.errAvg.asString() << std::endl;
|
|
std::cout << "------------------------------------------------------" << std::endl;
|
|
}
|
|
|
|
/** calculate the error for the given AP [mac + configured model] for all fingerprints for this mac */
|
|
K::Statistics<float> getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model) {
|
|
|
|
// get all fingerprints where the given mac was seen
|
|
const std::vector<WiFiFingerprint> fps = getFingerprints(mac);
|
|
|
|
// fire
|
|
return getError(mac, model, fps);
|
|
|
|
}
|
|
|
|
/** calculate the error for the given AP [mac + configured model] for all of the given FPS */
|
|
K::Statistics<float> getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model, const std::vector<WiFiFingerprint>& fps) {
|
|
|
|
K::Statistics<float> res;
|
|
|
|
// calculate the model error for each fingerprint
|
|
for (const WiFiFingerprint& fp : fps) {
|
|
|
|
// sanity checks
|
|
if (fp.measurements.entries.size() != 1) {throw Exception("invalid fingerprint");}
|
|
if (fp.measurements.entries.front().getAP().getMAC() != mac) {throw Exception("invalid fingerprint");}
|
|
|
|
// rssi prediction from the configured model
|
|
const float model_rssi = model.getRSSI(mac, fp.pos_m);
|
|
|
|
// sanity check
|
|
if (model_rssi != model_rssi) {throw Exception("model rssi not available for " + mac.asString());}
|
|
|
|
// rssi measured during scan at this location
|
|
const float scan_rssi = fp.measurements.entries.front().getRSSI();
|
|
|
|
// sanity check
|
|
if (scan_rssi < -100) {throw Exception("scan rssi out of range for " + mac.asString());}
|
|
if (scan_rssi > -35) {throw Exception("scan rssi out of range for " + mac.asString());}
|
|
|
|
// dB error
|
|
const float err = model_rssi - scan_rssi;
|
|
|
|
// probability matching error
|
|
//const float err = -std::log(Distribution::Normal<float>::getProbability(model_rssi, 8, scan_rssi));
|
|
|
|
// quadratic
|
|
float aErr = std::pow(std::abs(err), 2);
|
|
|
|
// penalty
|
|
// if (model.getAP(mac).waf > -2) {aErr += 9999;}
|
|
// if (model.getAP(mac).txp > -30) {aErr += 9999;}
|
|
// if (model.getAP(mac).txp < -50) {aErr += 9999;}
|
|
|
|
|
|
res.add(aErr);
|
|
|
|
}
|
|
|
|
// done
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // EVALCOMPAREOPT2_H
|