FHWS/OTHER2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

current state

Browse Source
This commit is contained in:
FrankE
2017-04-18 18:03:31 +02:00
parent 3e244118cc
commit 11cb939758
18 changed files with 1809 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -58,7 +58,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
-O0
-O2
-march=native
-DWITH_TESTS

391
EvalWiFi.h
View File

@@ -1,391 +0,0 @@
#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/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 "Structs.h"
#include "plots/Plotty.h"
#include "plots/PlotErrTime.h"
#include "plots/PlotErrFunc.h"
#include "CSV.h"
#include <unordered_set>
#include <thread>
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::AVERAGE);
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("walktime (seconds)", "\\small{error (m)}", "");
pet->getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 40));
pet2 = new PlotErrTime("walktime (seconds)", "\\small{-log(p(groundTruth | measurements))", "");
plot = new Plotty(map);
plot->buildFloorplan();
plot->setGroundTruth(gtIndices);
}
void load(const std::string& xmlFile, const std::string& name) {
WiFiModelFactory fac(map);
// setup the model
this->wiModel = fac.loadXML(xmlFile);
// fire
build(name);
}
private:
void build(const std::string& name) {
K::GnuplotStroke stroke(
K::GnuplotDashtype::SOLID, 1.5,
K::GnuplotColor::AUTO()
);
static int idx = -1; ++idx;
const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);
Line<Point3> path;
K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines();
gpPath->setStroke(stroke);
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];
// USE GENETIC
// 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);
// 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);
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()); // 2D
//const float err_m = gt.getDistance(curEst); // 3D
stats->add(err_m);
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());
statsProbOnGT->add(logProbOnGTNorm);
static int xxx = 0; ++xxx;
// plot err
pet->addErr(ts, err_m, idx);
pet2->addErr(ts, logProbOnGTNorm, idx);
if (xxx%2 == 0) {
plot->plot();
pet->plot();
pef->plot();
pet2->plot();
pef2->plot();
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
}
}
// // 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

108
EvalWiFiSigStrength.h
View File

@@ -103,18 +103,118 @@ public:
std::vector<WiFiMeasurement> mes = fp.getAllForMAC(mac);
if (!mes.empty()) {
const float rssi = mes.front().getRSSI();
const float s = scale(rssi, -100, -40);
const Color c = Color::fromHSV(s*100, 255, 255);
// fingerprint onto the floor
p->addFloorRect(fp.pos_m - Point3(0,0,1.3), 1, c);
p->addFloorRect(fp.pos_m, 1, c);
}
}
}
/** SPECIAL PAPER_PLOT OUTPUT */
void forPaperNN(Plotty* p, const MACAddress& mac) {
VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// get AP/floor
std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> _ap = FloorplanHelper::getAP(map, mac);
const Floorplan::Floor* floor = _ap.second;
const Floorplan::AccessPoint* ap = _ap.first;
// copy
WiFiFingerprints calib = this->calib;
for (WiFiFingerprint& fp : calib.getFingerprints()) {
fp.measurements = vap.group(fp.measurements);
}
const Point3 apPos3 = ap->getPos(floor);
K::GnuplotPoint3 apPos(apPos3.x, apPos3.y, apPos3.z);
p->points.add(apPos);
p->points.setPointSize(1.5); // The AP
// get all positions
struct Entry {
Point3 pos;
float rssi;
Entry(Point3 pos, float rssi) : pos(pos), rssi(rssi) {;}
};
std::vector<Entry> entries;
// process every fingerprint location
for (const WiFiFingerprint& fp : calib.getFingerprints()) {
// either 0 entries [ap not seen here] or 1 entry due to vap grouping [ap seen here]
std::vector<WiFiMeasurement> mes = fp.getAllForMAC(mac);
if (!mes.empty()) {
const float rssi = mes.front().getRSSI();
Entry e(fp.pos_m - Point3(0,0,1.3), rssi);
// only floor 0!!!!!
if (e.pos.z != 0) {continue;}
entries.push_back(e);
}
}
const float ss = 1.0;
for (const Floorplan::Floor* f : map->floors) {
// only floor 0!!!!!
if (f->atHeight != 0) {continue;}
BBox3 bb = FloorplanHelper::getBBox(f);
for (float y = bb.getMin().y; y < bb.getMax().y; y += ss) {
for (float x = bb.getMin().x; x < bb.getMax().x; x += ss) {
const Point3 pos(x,y,f->atHeight);
// float rssi = 0; float distSum = 0.01;
// for (const Entry& e : entries) {
// const float dist = e.pos.getDistance(pos);
// const float imp = 1.0 / std::pow((dist), 4);
// rssi += e.rssi * imp;
// distSum += imp;
// if (dist < 5) {rssi = e.rssi; distSum = 1;}
// }
// rssi /= distSum;
auto comp = [&] (const Entry& e1, const Entry& e2) {return e1.pos.getDistance(pos) < e2.pos.getDistance(pos);};
const auto& it = std::min_element(entries.begin(), entries.end(), comp);
if (it->pos.getDistance(pos) > 2.99) {continue;}
// const float rssi = it->rssi;
// const float s = scale(rssi, -100, -40);
// const Color c = Color::fromHSV(s*100, 255, 255);
// p->addFloorRect(pos, ss/2, c);
}
}
}
for (Entry e : entries) {
// FIX ONE FP NEAR THE AP
if (e.pos.xy().getDistance(Point2(67.2,32.1)) < 0.1) {
e.pos.y += 0.75;
}
p->gp << "set label '\\scriptsize{" << std::round(e.rssi) << "}' front at " << (e.pos.x-2) << "," << e.pos.y << "\n";
const float rssi = e.rssi;
const float s = scale(rssi, -100, -40);
const Color c = Color::fromHSV(s*100, 255, 255);
p->addFloorRect(e.pos, 2.6, c, 1.4);
}
}
void perAP_avg() {

38
Settings.h
View File

@@ -8,14 +8,34 @@ namespace Settings {
const std::string pathData = "/apps/android/workspace/OTHER2017/data/";
const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/";
const std::string pathWalksToni = "/apps/android/workspace/OTHER2017/data/toni/";
const std::string pathWiFi = "/apps/android/workspace/OTHER2017/data/wifi/";
// GPS walks
const std::string path1a = pathWalks + "path1/1490208103510.csv";
const std::string path1b = pathWalks + "path1/1490208519267.csv";
const std::string path2a = pathWalks + "path2/1490208940015.csv";
const std::string path2b = pathWalks + "path2/1490209320343.csv";
// nonGPS walks
const std::string path_toni_all_1a = pathWalksToni + "all/path1/1490031549543.csv";
const std::string path_toni_all_1b = pathWalksToni + "all/path1/1490031883742.csv";
const std::string path_toni_all_2a = pathWalksToni + "all/path2/1490032575999.csv";
const std::string path_toni_all_2b = pathWalksToni + "all/path2/1490032861864.csv";
const std::string path_toni_inst_1a = pathWalksToni + "inst/path1/1489769326868.csv";
const std::string path_toni_inst_1b = pathWalksToni + "inst/path1/1489769510080.csv";
const std::string path_toni_inst_2a = pathWalksToni + "inst/path2/1489774173022.csv";
const std::string path_toni_inst_2b = pathWalksToni + "inst/path2/1489774361865.csv";
const std::string path_toni_inst_3a = pathWalksToni + "inst/path3/1489776812891.csv";
const std::string path_toni_inst_3b = pathWalksToni + "inst/path3/1489776979143.csv";
const std::string wifiAllFixed = pathWiFi + "allFixed.xml";
const std::string wifiAllOptPar = pathWiFi + "allOptPar.xml";
const std::string wifiEachOptPar = pathWiFi + "eachOptPar.xml";
@@ -28,22 +48,23 @@ namespace Settings {
const std::string wifiEachOptParPos_multimodel = pathWiFi + "eachOptParPos_multimodel.xml";
const std::string wifiEachOptParPos_perBBox = pathWiFi + "eachOptParPos_perBBox.xml";
const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38.xml";
const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38_no_elev.xml";
const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat";
const std::string fPathGFX = "/apps/android/workspace/OTHER2017/tex/gfx/";
int numParticles = 5000;
int numParticles = 2500;
float smartphoneAboveGround = 1.3;
namespace IMU {
const float turnSigma = 1.25;//1.5;
const float stepLength = 0.70;
const float stepSigma = 0.45;
const float absHeadSigma = 80;
const float stepLength = 0.70+0.3;
const float stepSigma = 0.10;
const float absHeadSigma = 20;
}
namespace Grid {
@@ -92,6 +113,13 @@ namespace Settings {
const std::vector<int> path2 = {53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66};
const std::vector<int> path3 = {52, 51, 67, 68, 50, 49, 69, 70, 71, 72, 73, 74, 75, 76, 77, 73, 78, 79, 80, 81, 61, 82, 83, 84, 42, 41, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95};
const std::vector<int> path_toni_all_1 = {40,41,42,43,44,45, 1, 0, 46,47,48,49,50,51,52};
const std::vector<int> path_toni_all_2 = {53,54,55,56,57,58,59,60,61,62,63,64,65,66};
const std::vector<int> path_toni_inst_1 = {0,1,2,3,4,5,6,7,8,9,10};
const std::vector<int> path_toni_inst_2 = {11, 12, 3, 2, 1, 0, 13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30, 11};
const std::vector<int> path_toni_inst_3 = {31,32,33,34,35,36,37,38};
}
}

44
bboxes.h Normal file
View File

@@ -0,0 +1,44 @@
// dummy
const BBox3 dummy(Point3( 0, 0, -200), Point3( 1, 1, -199));
// floor 0
const BBox3 floor0Ha(Point3( 0.0, 0.0, 0.0), Point3( 18.6, 31.3, 3.99));
const BBox3 floor0Hb(Point3( 0.0, 31.3, 0.0), Point3( 77.5, 50.7, 3.99));
const BBox3 floor0Hc(Point3(77.5, 41.0, 0.0), Point3(111.6, 50.7, 3.99));
BBoxes3 bboxes0H({floor0Ha, floor0Hb, floor0Hc});
const BBox3 floor0Oa(Point3(18.6, 6.5, 0.0), Point3( 91.0, 31.3, 3.99));
const BBox3 floor0Ob(Point3(77.5, 31.3, 0.0), Point3( 91.0, 41.0, 3.99));
BBoxes3 bboxes0O({floor0Oa, floor0Ob});
const BBox3 floor0Ia(Point3(43.7, -8.5, 0.0), Point3( 91.0, 6.5, 3.99));
const BBox3 floor0Ib(Point3(91.0, -8.5, 0.0), Point3(111.6, 41.0, 3.99));
BBoxes3 bboxes0I({floor0Ia, floor0Ib});
// floor 1
const BBox3 floor1Ha(Point3( 0.0, 0.0, 4.0), Point3( 18.6, 31.3, 7.39));
const BBox3 floor1Hb(Point3( 0.0, 31.3, 4.0), Point3( 77.5, 50.7, 7.39));
BBoxes3 bboxes1H({floor1Ha, floor1Hb});
const BBox3 floor1Oa(Point3(77.5, 41.0, 4.0), Point3(111.6, 50.7, 7.39));
const BBox3 floor1Ob(Point3(61.2, 50.7, 4.0), Point3(111.6, 58.0, 7.39));
//BBoxes3 bboxes1O({floor1Oa, floor1Ob});
const BBox3 floor1Ia(Point3(43.7, -8.5, 4.0), Point3( 91.0, 6.5, 7.39));
const BBox3 floor1Ib(Point3(91.0, -8.5, 4.0), Point3(111.6, 41.0, 7.39));
//BBoxes3 bboxes1I({floor1Ia, floor1Ib});
// or
BBoxes3 bboxes1O({dummy});
BBoxes3 bboxes1I({floor1Ia, floor1Ib, floor1Oa, floor1Ob});
// floor 2
const BBox3 floor2Ha(Point3( 0.0, 0.0, 7.4), Point3(111.6, 50.7, 10.79));
BBoxes3 bboxes2H({floor2Ha});
// floor 3
const BBox3 floor3Ha(Point3( 0.0, 0.0, 10.8), Point3(111.6, 50.7, 14.2));
BBoxes3 bboxes3H({floor3Ha});

296
main.cpp
View File

@@ -26,11 +26,15 @@
#include "EvalApOpt.h"
#include "EvalData.h"
#include "EvalWiFi.h"
#include "EvalWiFiSigStrength.h"
#include "pf/EvalWalk.h"
#include "EvalWifiOptResult.h"
#include "wifi/EvalWiFiConvex.h"
#include "wifi/EvalWiFiGround.h"
#include "wifi/EvalWiFi.h"
#include "wifi/EvalWiFiPaths.h"
#include "wifi/EvalWiFiPathMethods.h"
#include "plots/PlotErrFunc.h"
@@ -42,7 +46,7 @@ void paperOutputs() {
// show optimization behaviour
if (1 == 1) {
if (1 == 0) {
EvalWiFiConvex eval(map, Settings::fCalib);
eval.showParams();
@@ -52,29 +56,72 @@ void paperOutputs() {
}
// show fingerprints as plot
if (1==0){
if (1 == 0){
EvalWiFiSigStrength sig(Settings::fMap, Settings::fCalib);
Plotty* p = new Plotty(map);
p->writeCodeTo(Settings::fPathGFX + "compare-wifi-in-out.gp");
p->writeEpsTex(Settings::fPathGFX + "compare-wifi-in-out.tex");
p->writeEpsTex(Settings::fPathGFX + "compare-wifi-in-out.tex", K::GnuplotSize(8.6, 4.7));
p->settings.floors = {0};
p->settings.maxZ = 1;
p->settings.outlineColor = K::GnuplotColor::fromRGB(170,170,170);
p->buildFloorplan();
sig.forAP_avg(p, MACAddress("d8:84:66:4a:23:d0"));
sig.forPaperNN(p, MACAddress("d8:84:66:4a:23:d0"));
p->equalXY();
p->setView(0,0);
p->setScale(3.1, 3.1);
p->addRectangle(Point3(62, 24, 0), Point3(72, 34, 0), Color::fromRGB(0,0,0), false, false);
p->setScale(3.45, 3.45, -0.005, -0.04);
p->addRectangleW(Point3(62.5, 24, 0), Point3(72, 35, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true); // small
p->addRectangleW(Point3(8.0, 39.75, 0), Point3(72, 43.75, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true); // big
p->noFrame();
p->plot();
delete p;
}
// perform varios AP-param optimizations
// generate error plot showing the performance of each
// save the resulting wifi-models to XML for later re-use during the walk-eval <<<<<< !!!!
if (1 == 1) {
if (1 == 0) {
#include "bboxes.h"
Plotty pt(map);
pt.settings.outline = false;
pt.buildFloorplan();
// coloring
K::GnuplotColor cH = K::GnuplotColor::fromRGB(220,220,220);
K::GnuplotColor cO = K::GnuplotColor::fromRGB(0,128,0);
K::GnuplotColor cI = K::GnuplotColor::fromRGB(190,190,190);
// floor 0
pt.addBBoxes(bboxes0H, cH);
pt.addBBoxes(bboxes0O, cO);
pt.addBBoxes(bboxes0I, cI);
// floor 1
pt.addBBoxes(bboxes1H, cH);
pt.addBBoxes(bboxes1O, cO);
pt.addBBoxes(bboxes1I, cI);
// floor 2
pt.addBBoxes(bboxes2H, cH);
// floor 3
pt.addBBoxes(bboxes3H, cH);
pt.plot();
int i = 0; (void) i;
}
// perform varios AP-param optimizations
// generate error plot showing the performance of each
// save the resulting wifi-models to XML for later re-use during the walk-eval <<<<<< !!!!
if (1 == 0) {
// // use walks?
// std::vector<std::pair<std::string, std::vector<int>>> calibWalks = {
@@ -85,36 +132,42 @@ void paperOutputs() {
// };
// EvalCompareOpt2 opt1(Settings::fMap, calibWalks);
auto removeNone = [] (const WiFiFingerprint& fp) -> bool {
return false;
};
// all combined
auto removeNone = [] (const WiFiFingerprint& fp) -> bool {return false;};
auto only0th = [] (const WiFiFingerprint& fp) -> bool {
return std::abs(fp.pos_m.z - (1.3)) > 0.1;
};
// per floor
auto only0th = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (1.3)) > 0.1;};
auto only1st = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+1.3)) > 0.1;};
auto only2nd = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+3.4+1.3)) > 0.1;};
auto only3rd = [] (const WiFiFingerprint& fp) -> bool {return std::abs(fp.pos_m.z - (4+3.4+3.4+1.3)) > 0.1;};
auto only1st = [] (const WiFiFingerprint& fp) -> bool {
return std::abs(fp.pos_m.z - (4+1.3)) > 0.1;
};
// per bbox
#include "bboxes.h"
auto only0H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0H.contains(fp.pos_m);};
auto only0O = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0O.contains(fp.pos_m);};
auto only0I = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes0I.contains(fp.pos_m);};
auto only2nd = [] (const WiFiFingerprint& fp) -> bool {
return std::abs(fp.pos_m.z - (4+3.4+1.3)) > 0.1;
};
auto only1H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1H.contains(fp.pos_m);};
auto only1O = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1O.contains(fp.pos_m);};
auto only1I = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes1I.contains(fp.pos_m);};
auto only2H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes2H.contains(fp.pos_m);};
auto only3H = [&] (const WiFiFingerprint& fp) -> bool {return !bboxes3H.contains(fp.pos_m);};
auto only3rd = [] (const WiFiFingerprint& fp) -> bool {
return std::abs(fp.pos_m.z - (4+3.4+3.4+1.3)) > 0.1;
};
// use fingerprints?
EvalCompareOpt2 opt1(Settings::fMap, Settings::fCalib, removeNone);
// optimize using all floors
EvalCompareOpt2::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK;
EvalCompareOpt2::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK;
EvalCompareOpt2::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK;
EvalCompareOpt2::Result s4 = opt1.optPosOptParamsForEach(); //BREAK;
// optimize only for the 0th floor
// optimize per floor
EvalCompareOpt2 opt_f0(Settings::fMap, Settings::fCalib, only0th);
EvalCompareOpt2::Result sf0 = opt_f0.optPosOptParamsForEach();
EvalCompareOpt2 opt_f1(Settings::fMap, Settings::fCalib, only1st);
@@ -124,6 +177,30 @@ void paperOutputs() {
EvalCompareOpt2 opt_f3(Settings::fMap, Settings::fCalib, only3rd);
EvalCompareOpt2::Result sf3 = opt_f3.optPosOptParamsForEach();
// optimize per bbox
EvalCompareOpt2 opt_0H(Settings::fMap, Settings::fCalib, only0H);
EvalCompareOpt2::Result sf0H = opt_0H.optPosOptParamsForEach();
EvalCompareOpt2 opt_0O(Settings::fMap, Settings::fCalib, only0O);
EvalCompareOpt2::Result sf0O = opt_0O.optPosOptParamsForEach();
EvalCompareOpt2 opt_0I(Settings::fMap, Settings::fCalib, only0I);
EvalCompareOpt2::Result sf0I = opt_0I.optPosOptParamsForEach();
EvalCompareOpt2 opt_1H(Settings::fMap, Settings::fCalib, only1H);
EvalCompareOpt2::Result sf1H = opt_1H.optPosOptParamsForEach();
EvalCompareOpt2 opt_1O(Settings::fMap, Settings::fCalib, only1O);
EvalCompareOpt2::Result sf1O = opt_1O.optPosOptParamsForEach();
EvalCompareOpt2 opt_1I(Settings::fMap, Settings::fCalib, only1I);
EvalCompareOpt2::Result sf1I = opt_1I.optPosOptParamsForEach();
EvalCompareOpt2 opt_2H(Settings::fMap, Settings::fCalib, only2H);
EvalCompareOpt2::Result sf2H = opt_2H.optPosOptParamsForEach();
EvalCompareOpt2 opt_3H(Settings::fMap, Settings::fCalib, only3H);
EvalCompareOpt2::Result sf3H = opt_3H.optPosOptParamsForEach();
// save models to file
s1.model.saveXML(Settings::wifiAllFixed);
s2.model.saveXML(Settings::wifiAllOptPar);
@@ -142,6 +219,18 @@ void paperOutputs() {
wmpf.add(&sf3.model, map->floors[3]);
wmpf.saveXML(Settings::wifiEachOptParPos_multimodel);
// ultra fancy combined model
WiFiModelPerBBox wmbb(map);
wmbb.add(&sf0H.model, bboxes0H);
wmbb.add(&sf0O.model, bboxes0O);
wmbb.add(&sf0I.model, bboxes0I);
wmbb.add(&sf1H.model, bboxes1H);
wmbb.add(&sf1O.model, bboxes1O);
wmbb.add(&sf1I.model, bboxes1I);
wmbb.add(&sf2H.model, bboxes2H);
wmbb.add(&sf3H.model, bboxes3H);
wmbb.saveXML(Settings::wifiEachOptParPos_perBBox);
PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}");
pef.add("\\small{empiric}", &s1.errSingle);
pef.add("\\small{real pos, opt params}", &s2.errSingle);
@@ -160,12 +249,10 @@ void paperOutputs() {
//return;
//sleep(1000);
}
/** plot wifi eval results */
if (1 == 1) {
if (1 == 0) {
WiFiFingerprints fps;
fps.load(Settings::fCalib);
@@ -176,7 +263,7 @@ void paperOutputs() {
EvalWiFiOptResult eval2(Settings::fMap);
eval2.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiAllFixed, fps);
// advanced model [1 model per floor]
EvalWiFiOptResult evalfloor(Settings::fMap);
evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only0th, fps);
evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only1st, fps);
@@ -184,6 +271,10 @@ void paperOutputs() {
evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only3rd, fps);
evalfloor.showErrorPerFingerprint<WiFiModelPerFloor>(Settings::wifiEachOptParPos_multimodel, fps);
// more advanved model [1 model per bbox-region]
EvalWiFiOptResult evalBBox(Settings::fMap);
evalBBox.showErrorPerFingerprint<WiFiModelPerBBox>(Settings::wifiEachOptParPos_perBBox, fps);
int i = 0; (void) i;
}
@@ -279,12 +370,95 @@ void testWAF() {
}
void showFingerprintsFor(const std::string& mapFile, const std::string& fpFile, const std::string& smac) {
WiFiFingerprints calib(fpFile);
VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
for (WiFiFingerprint& fp : calib.getFingerprints()) {
fp.measurements = vap.group(fp.measurements);
}
const MACAddress mac(smac);
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
Plotty p(map);
p.buildFloorplan();
std::vector<WiFiFingerprint> fps = calib.getFingerprintsFor(mac);
for (const WiFiFingerprint& fp : fps) {
if (fp.measurements.entries.size() != 1) {throw "123";}
const float rssi = fp.measurements.entries[0].getRSSI();
const float s = (rssi-100) / (-40 - -100);
const Color c = Color::fromHSV(s*100, 255, 255);
p.addFloorRect(fp.pos_m, 3, c);
}
p.plot();
sleep(1);
}
void showModelFor(const std::string& mapFile, const std::string& modelXml, const std::string& smac) {
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
WiFiModelFactory fac(map);
WiFiModel* model = fac.loadXML(modelXml);
const MACAddress mac(smac);
Plotty p(map);
p.buildFloorplan();
const float ss = 2.0;
for (Floorplan::Floor* floor : map->floors) {
for (float y = -20; y < 70; y+=ss) {
for (float x = -10; x < 130; x+=ss) {
const Point3 pos(x,y,floor->atHeight+1.3);
// limit estimations to the floorplan's outline
bool contained = false;
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
HelperPoly hp(*poly);
if (hp.contains(pos.xy()*100)) {
if (poly->method == Floorplan::OutlineMethod::ADD) {
contained = true;
}
}
}
if (!contained) {continue;}
const float rssi = model->getRSSI(mac, pos);
const float s = (rssi-100) / (-40 - -100);
const Color c = Color::fromHSV(s*100, 255, 255);
p.addFloorRect(pos, 3, c);
}
}
}
p.plot();
sleep(1);
}
int main(void) {
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);
//testWAF();
paperOutputs(); // return 0;
//paperOutputs(); return 0;
//showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:D0");
//showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:D0");
//showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:E0");
//showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:E0");
// calib error in/out
if (1 == 0) {
@@ -313,12 +487,21 @@ int main(void) {
EvalCompareOpt::Result s2 = opt2.optPosOptParamsForEach();
std::cout << s2.errAbs.asString() << std::endl;
}
// prob on ground
if (1 == 0) {
EvalWiFiGround eval(map, Settings::wifiEachOptParPos_multimodel);
//eval.show(Settings::path1a);
eval.show(Settings::path2a);
int i = 0; (void) i;
}
// walks
if (1 == 1) {
if (1 == 0) {
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);;
EvalWalk walk(map);
walk.walk1();
@@ -343,24 +526,69 @@ int main(void) {
// test wifi within data files
if (1 == 1) {
if (1 == 0) {
//EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1);
EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1);
//EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2);
//ew1.fixedParams(-40, 2.5, -8); BREAK;
//ew1.fixedParams(-64.5905, 1.25988, -2.47863); BREAK;
//ew1.fixedParams(-59.4903,1.52411,-3.25077); BREAK;
//ew1.load(Settings::wifiEachOptParPos);
//ew1.load<WiFiModelLogDistCeiling>(Settings::wifiAllFixed, "empirc");
ew1.load(Settings::wifiAllFixed, "empirc");
//ew1.load(Settings::wifiAllOptPar, "opt params all APs");
//ew1.load(Settings::wifiEachOptPar, "opt params each AP");
ew1.load(Settings::wifiEachOptParPos, "everything opt");
ew1.load(Settings::wifiEachOptParPos, "normal model");
ew1.load(Settings::wifiEachOptParPos_multimodel, "model per floor");
ew1.load(Settings::wifiEachOptParPos_perBBox, "model per region");
//ew1.load<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only1st, "everything opt");
//ew1.writeTeX("path1");
sleep(1000);
}
if (1 == 1) {
std::vector<std::string> files = {
Settings::path1a, //Settings::path1b,
//Settings::path2a, Settings::path2b,
//Settings::path_toni_all_1a, Settings::path_toni_all_1b,
//Settings::path_toni_all_2a, Settings::path_toni_all_2b,
//Settings::path_toni_inst_1a, Settings::path_toni_inst_1b,
//Settings::path_toni_inst_2a, Settings::path_toni_inst_2b,
//Settings::path_toni_inst_3a, Settings::path_toni_inst_3b,
};
std::vector<std::vector<int>> gtIndices = {
Settings::GroundTruth::path1, //Settings::GroundTruth::path1,
//Settings::GroundTruth::path2, Settings::GroundTruth::path2,
//Settings::GroundTruth::path_toni_all_1, Settings::GroundTruth::path_toni_all_1,
//Settings::GroundTruth::path_toni_all_2, Settings::GroundTruth::path_toni_all_2,
//Settings::GroundTruth::path_toni_inst_1, Settings::GroundTruth::path_toni_inst_1,
//Settings::GroundTruth::path_toni_inst_2, Settings::GroundTruth::path_toni_inst_2,
//Settings::GroundTruth::path_toni_inst_3, Settings::GroundTruth::path_toni_inst_3,
};
// EvalWiFiPaths ewp(Settings::fMap);
// ewp.loadModel(Settings::wifiAllFixed, "empirc");
// ewp.walks(files, gtIndices);
// ewp.loadModel(Settings::wifiEachOptParPos, "normal model");
// ewp.walks(files, gtIndices);
// ewp.loadModel(Settings::wifiEachOptParPos_multimodel, "model per floor");
// ewp.walks(files, gtIndices);
// ewp.loadModel(Settings::wifiEachOptParPos_perBBox, "model per region");
// ewp.walks(files, gtIndices);
EvalWiFiPathMethods ewpm(Settings::fMap);
ewpm.loadModel(Settings::wifiEachOptParPos_perBBox, "model per region", "original", "alternative");
ewpm.walks(files, gtIndices);
ewpm.writeGP(Settings::fPathGFX, "normalVsExp");
sleep(10000);
}

54
pf/EvalWalk.h
View File

@@ -108,7 +108,7 @@ public:
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
//pf->setNEffThreshold(0.35);
//pf->setNEffThreshold(0.15);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setNEffThreshold(0.75);
@@ -117,22 +117,26 @@ public:
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05);
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
res->setDrawCallback([&] (K::Particle<MyState>& p) {
static std::minstd_rand gen;
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(-0.3, +0.3);
for (int j = 0; j < 2; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
p.state.heading.direction += distTurn(gen);
});
// move during resampling. NOT ALLOWED!
// res->setDrawCallback([&] (K::Particle<MyState>& p) {
// static std::minstd_rand gen;
// static int cnt = 0; ++cnt;
// bool init = cnt < pf->getParticles().size() * 50;
// const MyGridNode* n = grid->getNodePtrFor(p.state.position);
// std::normal_distribution<float> distTurn(0, (init) ? (0.5) : (0.05));
// for (int j = 0; j < 2; ++j) {
// std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
// const int idx = distIdx(gen);
// n = &(grid->getNeighbor(*n, idx));
// }
// p.state.position = *n;
// p.state.heading.direction += distTurn(gen);
// });
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -144,15 +148,20 @@ public:
void walk1() {
// path1
absHead = M_PI/2;
const std::string path = Settings::path1b;
const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// absHead = M_PI/2;
// const std::string path = Settings::path1b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// path2
// absHead = 0;
// const std::string path = Settings::path2a;
// const std::string path = Settings::path2b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
// path_toni_inst_2
absHead = M_PI;
const std::string path = Settings::path_toni_inst_2b;
const std::vector<int> pathPoints = Settings::GroundTruth::path_toni_inst_2;
runName = "";
// get ground-truth
@@ -162,8 +171,12 @@ public:
//WiFiModelLogDistCeiling wifiModel(map);
//wifiModel.loadXML(Settings::wifiAllFixed);
//wifiModel.loadXML(Settings::wifiEachOptParPos);
WiFiModelPerFloor wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
//WiFiModelPerFloor wifiModel(map);
//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
WiFiModelPerBBox wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
@@ -226,6 +239,7 @@ public:
const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
curCtrl.compassAzimuth_rad += diff * 0.01;
curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad;
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
}

148
pf/PF.h
View File

@@ -12,7 +12,8 @@
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>
@@ -92,6 +93,9 @@ struct MyObservation {
/** gps measurements */
GPSData gps;
/** absolute heading in radians */
float compassAzimuth_rad = 0;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
@@ -181,9 +185,8 @@ public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
//walker.addModule(&modAbsHead);
//walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
@@ -197,40 +200,77 @@ public:
}
int numTrans = 0;
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
++numTrans;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// std::normal_distribution<float> turnNoise (0, 0.2);
// std::uniform_real_distribution<float> initTurnNoise(-0.3, +0.3);
// std::uniform_real_distribution<float> initDistNoise(0.0, 0.5);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
//#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.5);
// first transitions: more variation as the state is unknown
if (numTrans < 50 || numTrans % 10 == 0) {
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(0, 0.4);
for (int j = 0; j < 5; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
if (numTrans < 50) {
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
}
}
//p.state.heading.direction += turnNoise(gen);
// "blur" the density by adjusting the weights
//p.weight = std::pow(p.weight, 0.4);
//p.weight = 1;
double prob;
double prob = 1.0;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.7); // add grid-walk-probability
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
//throw Exception("weight = 0.0");
}
}
@@ -253,7 +293,6 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
@@ -264,88 +303,109 @@ public:
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
//wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
const float kappa = 0.75;
double getAbsHead(const MyState& s, const MyObservation& obs) {
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
// compare the heading against the state's heading - the last error
const Heading stateHead = s.heading.direction;
const Heading obsHead(obs.compassAzimuth_rad);
case ActivityButterPressure::Activity::STAY:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
// get the difference
const float angularDiff = obsHead.getDiffHalfRAD(stateHead);
case ActivityButterPressure::Activity::UP:
case ActivityButterPressure::Activity::DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
double res = 0;
if (angularDiff > Angle::degToRad(90)) {res = 0.05;}
else if (angularDiff > Angle::degToRad(45)) {res = 0.25;}
else {res = 0.70;}
return res;
}
WiFiQualityAnalyzer wqa;
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// vap-grouping
const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
wqa.add(wifiObs);
const float quality = wqa.getQuality();
// GPS
const GPSProbability gpsProb(em);
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// use (0.9 * p) + (0.1 * (1-p)) for error cases
wiFiProbability.setUseError(false);
//#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
const MyGridNode& node = grid->getNodeFor(p.state.position);
// WiFi free
const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
if (quality < 0.2) {
pWiFi = 1;
pWiFiVeto = 1;
std::cout << "disabling WiFi" << std::endl;
}
// WiFi grid
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
//const double pStair = 1;//getStairProb(p, observation.activity);
const double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
const double prob = pWiFi * pGPS;
const double pAbsHead = getAbsHead(p.state, observation);
const double prob = pWiFi * pWiFiVeto;// * pAbsHead;
//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?
if (pGPS != 1) {
int i = 0; (void) i;
}
// TESTING
//p.weight = std::pow(p.weight, 0.5);
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
//p.weight = prob; // does NOT keep the weights returned by the transition step
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
// throw Exception("weight = 0.0");
}
}
return sum;
return 0;
}

4
plots/PlotErrFunc.h
View File

@@ -71,6 +71,10 @@ public:
return gp;
}
K::GnuplotPlot& getPlot() {
return gplot;
}
void writeCodeTo(const std::string& file) {
this->codeFile = file;
}

25
plots/PlotErrTime.h
View File

@@ -19,6 +19,7 @@ private:
K::GnuplotPlotElementLines lineErr[8];
K::GnuplotPlotElementLines lineB;
K::GnuplotPlotElementLines lineC;
std::string codeFile;
public:
@@ -40,6 +41,25 @@ public:
}
void clear() {
for (int i = 0; i < 8; ++i) {lineErr[i].clear();}
lineB.clear();
lineC.clear();
}
void writeCodeTo(const std::string& file) {
this->codeFile = file;
}
void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
gp.setTerminal("epslatex", size);
gp.setOutput(file);
gp << "set key\n";
gp << "set rmargin 0.2\n";
gp << "set tmargin 0.2\n";
gp << "set bmargin 1.2\n";
}
K::GnuplotPlot& getPlot() {
return gpplot;
}
@@ -63,6 +83,11 @@ public:
void plot() {
gp.draw(gpplot);
if (codeFile != "") {
std::ofstream out(codeFile);
out << gp.getBuffer();
out.close();
}
gp.flush();
}

163
plots/PlotWiFiGroundProb.h Normal file
View File

@@ -0,0 +1,163 @@
#ifndef EVALWIFIGROUNDPROB_H
#define EVALWIFIGROUNDPROB_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 "Indoor/sensors/radio/WiFiProbabilityFree.h"
#include "../Helper.h"
#include <vector>
#include "../plots/Plotty.h"
class PlotWiFiGroundProb {
private:
const Floorplan::IndoorMap* map;
const float stepSize_m = 3.0;
//std::vector<Point3> pos;
struct Entry {
Point3 pos;
K::GnuplotObjectPolygon* poly;
float max = 0;
float sum = 0;
int cnt = 0;
};
std::vector<Entry> pos;
Plotty plot;
public:
/** ctor */
PlotWiFiGroundProb(const Floorplan::IndoorMap* map) : map(map), plot(map) {
buildEvalPoints();
plot.settings.outline = false;
plot.buildFloorplan();
}
void show(const WiFiObserverFree& prob, const WiFiMeasurements& _mes) {
VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
const WiFiMeasurements mes = vap.group(_mes);
double min = +99999;
double max = -99999;
for (Entry& e : pos) {
const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes);
if (p < min) {min = p;}
if (p > max) {max = p;}
}
double diff = max-min;
for (Entry& e : pos) {
const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes);
const double f = (p-min) / diff * 255;
e.sum += p;
e.cnt++;
//if (f > e.max) {
const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255);
e.poly->getFill().setColor(c);
e.max = f;
//}
//plot.cpoints.add(K::GnuplotPoint3(pt.x, pt.y, pt.z), p);
}
}
void update() {
double min = +99999;
double max = -99999;
for (Entry& e : pos) {
const double p = e.sum / e.cnt;
if (p < min) {min = p;}
if (p > max) {max = p;}
}
double diff = max-min;
for (Entry& e : pos) {
const double p = e.sum / e.cnt;
const double f = (p-min) / diff * 255;
const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255);
e.poly->getFill().setColor(c);
}
}
void plotMe() {
plot.plot();
}
private:
/** get a list of all points among the outline */
void buildEvalPoints() {
BBox3 bbox = FloorplanHelper::getBBox(map);
for (Floorplan::Floor* f : map->floors) {
for (float y = bbox.getMin().y; y < bbox.getMax().y; y += stepSize_m) {
for (float x = bbox.getMin().x; x < bbox.getMax().x; x += stepSize_m) {
GridFactory<int>::PartOfOutline part = GridFactory<int>::isPartOfFloorOutline(x*100, y*100, f->outline);
if (part != GridFactory<int>::PartOfOutline::NO) {
const float s = stepSize_m/2.0f;
Entry e;
e.poly = new K::GnuplotObjectPolygon();
e.poly->add(K::GnuplotCoordinate3(x-s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
e.poly->add(K::GnuplotCoordinate3(x+s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
e.poly->add(K::GnuplotCoordinate3(x+s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
e.poly->add(K::GnuplotCoordinate3(x-s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
e.poly->close();
e.poly->getFill().setStyle(K::GnuplotFillStyle::SOLID);
e.poly->setStroke(K::GnuplotStroke::NONE());
e.pos = Point3(x, y, f->atHeight);
pos.push_back(e);
plot.splot.getObjects().add(e.poly);
}
}
}
}
}
};
#endif // EVALWIFIGROUNDPROB_H

104
plots/Plotty.h
View File

@@ -3,6 +3,7 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/geo/BBoxes3.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
@@ -11,6 +12,7 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjects.h>
struct Color {
@@ -99,6 +101,7 @@ public:
K::GnuplotSplotElementLines mapOutlineGlass;
K::GnuplotSplotElementLines mapOutlineDrywall;
K::GnuplotSplotElementLines mapOutlineConcrete;
K::GnuplotSplotElementLines mapBBoxes;
std::string codeFile;
@@ -130,9 +133,11 @@ public:
mapOutlineConcrete.getStroke().getColor().setHexStr("#888888"); mapOutlineConcrete.getStroke().setWidth(2);
mapOutlineDrywall.getStroke().getColor().setHexStr("#888888");
mapOutlineGlass.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().setType(K::GnuplotDashtype::DASHED);
mapBBoxes.getStroke().setWidth(2);
splot.add(&mapOutlineConcrete);
splot.add(&mapOutlineDrywall);
splot.add(&mapOutlineGlass);
splot.add(&mapBBoxes);
splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777");
@@ -149,6 +154,52 @@ public:
}
void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {
for (const BBox3& bb : boxes.get()) {
addBBoxPoly(bb, c);
}
}
void addBBox(const BBox3& bb) {
// floor
mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMin().z});
mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMin().z});
mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMin().z});
mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
mapBBoxes.splitFace(); mapBBoxes.splitFace();
// ceil
mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMax().z});
mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMax().z});
mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMax().z});
mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
mapBBoxes.splitFace(); mapBBoxes.splitFace();
// up
mapBBoxes.addSegment({bb.getMin().x, bb.getMin().y, bb.getMin().z}, {bb.getMin().x, bb.getMin().y, bb.getMax().z});
mapBBoxes.addSegment({bb.getMax().x, bb.getMin().y, bb.getMin().z}, {bb.getMax().x, bb.getMin().y, bb.getMax().z});
mapBBoxes.addSegment({bb.getMin().x, bb.getMax().y, bb.getMin().z}, {bb.getMin().x, bb.getMax().y, bb.getMax().z});
mapBBoxes.addSegment({bb.getMax().x, bb.getMax().y, bb.getMin().z}, {bb.getMax().x, bb.getMax().y, bb.getMax().z});
}
void addBBoxPoly(const BBox3& bb, K::GnuplotColor c) {
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->close();
poly->setStroke(K::GnuplotStroke::NONE());
poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, c));
splot.getObjects().add(poly);
}
void setGroundTruth(const Point3 pos_m) {
gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
}
@@ -202,7 +253,25 @@ public:
addPolygon(points, c.toHEX(), front, fill);
}
void addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true) {
void addRectangleW(const Point3 p1, const Point3 p2, const K::GnuplotColor c, const float w, bool front = false) {
std::vector<Point3> points = {
Point3(p1.x, p1.y, p1.z),
Point3(p2.x, p1.y, p1.z),
Point3(p2.x, p2.y, p1.z),
Point3(p1.x, p2.y, p1.z),
Point3(p1.x, p1.y, p1.z),
};
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
poly->getStroke().setWidth(w);
poly->getStroke().setColor(c);
poly->setFront(front);
for (const Point3 p : points) {
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
}
splot.getObjects().add(poly);
}
void addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true, const float alpha = 1) {
for (const Point3 p : points) {
if (p.z < settings.minZ) {return;}
@@ -216,6 +285,7 @@ public:
for (const Point3 p : points) {
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
poly->setZIndex(p.z); // manual depth ordering
poly->getFill().setAlpha(alpha);
}
poly->setFront(front);
@@ -240,16 +310,16 @@ public:
}
void addFloorRect(const Point3 pos_m, const float size, Color c) {
void addFloorRect(const Point3 pos_m, const float size, Color c, float ratio = 1.0) {
const Point3 p1 = pos_m + Point3(-size, -size, 0);
const Point3 p2 = pos_m + Point3(+size, -size, 0);
const Point3 p3 = pos_m + Point3(+size, +size, 0);
const Point3 p4 = pos_m + Point3(-size, +size, 0);
const Point3 p1 = pos_m + Point3(-size, -size/ratio, 0);
const Point3 p2 = pos_m + Point3(+size, -size/ratio, 0);
const Point3 p3 = pos_m + Point3(+size, +size/ratio, 0);
const Point3 p4 = pos_m + Point3(-size, +size/ratio, 0);
std::vector<Point3> points = {p1,p2,p3,p4,p1};
addPolygon(points, c.toHEX());
addPolygon(points, c.toHEX(), false, true);
// gp << "set object polygon from ";
// for (size_t i = 0; i < points.size(); ++i) {
@@ -279,11 +349,12 @@ public:
}
void setView(const float degX, const float degY) {
gp << "set view " << degX << "," << degY << "\n";
//gp << "set view " << degX << "," << degY << "\n";
splot.getView().setCamera(degX, degY);
}
void setScale(const float x, const float y) {
gp << "set multiplot layout 1,1 scale " << x << "," << y << "\n";
void setScale(const float x, const float y, const float ox = 0, const float oy = 0) {
gp << "set multiplot layout 1,1 scale " << x << "," << y << " offset " << ox << "," << oy << "\n";
}
void writeCodeTo(const std::string& file) {
@@ -292,9 +363,12 @@ public:
void noFrame() {
gp << "unset border\n";
gp << "unset xtics\n";
gp << "unset ytics\n";
gp << "unset ztics\n";
// gp << "unset xtics\n";
// gp << "unset ytics\n";
// gp << "unset ztics\n";
splot.getAxisX().setTicsVisible(false);
splot.getAxisY().setTicsVisible(false);
splot.getAxisZ().setTicsVisible(false);
}
void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
@@ -357,7 +431,9 @@ public:
// plot the floor's outline
if (settings.outline) {
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
K::GnuplotColor color = (poly->outdoor) ? (K::GnuplotColor::fromRGB(200, 240, 200)) : (K::GnuplotColor::fromRGB(210,210,210));
K::GnuplotColor color = K::GnuplotColor::fromRGB(210,210,210);
if (poly->outdoor) {color = K::GnuplotColor::fromRGB(200, 240, 200);}
//if (poly->method == Floorplan::OutlineMethod::REMOVE) {color = K::GnuplotColor::fromRGB(245,245,245);}
K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color);
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, K::GnuplotStroke::NONE());
for (Point2 pt : poly->poly.points) {

67
tex/chapters/experiments.tex
View File

@@ -1,5 +1,12 @@
experiments
\todo{obwohl das angepasste modell doch recht gut laeuft und der fehler recht klein wird, sind immernoch stellen dabei,
wo es einfach nicht gut passt, unguenstige mehrdeutigkeiten vorliegen, oder regionen einfach nicht passen wie sie sollten.
das liegt teils auch daran, dass die fingerprints drehend aufgenommen wurden und beim laufen nach hinten durch den
menschen abgeschottet wird. auch zeitlicher verzug kann ein problem darstellen.}
\todo{GPS ist leider kaum eine hilfe. entweder kein empfang wegen ueberdachung oder abschattung, oder
zu kurz draußen um einen guten gps-fix zu bekommen.}
wir betrachten nur die fest-installierten APs die man meist anhand einer bestimmten mac-range ausmachen kann
portable geraete von studenten, beamer, aehnliches werden ignoriert
@@ -59,9 +66,65 @@ reines wifi eval mittels num-opt springt stark durch die gegend
d.h. das bewegungsmodell rettet uns
kann man auch testen wenn man beim particle-filter das resampling ganz aus macht
\todo{
we analyzed various paths throughout the whole building
}
\input{gfx/compare-wifi-in-out.tex}
starker einfluss der glasscheiben.. 3 meter nach dem AP ist nur noch sehr wenig uebrig
\todo{
mit grafik: exp-dist vergroesert teils den abstand zu anderen locations , der GT selbst wird also besser,
aber an anderen stellen geht dafür der fehler hoch und kann zu verlaufen führen (z.B. treppenhaus)
}
\begin{figure}
\input{gfx/wifiCompare_normalVsExp_cross.tex}
\input{gfx/wifiCompare_normalVsExp_meter.tex}
\label{normal vs exponential}
\end{figure}
\todo{
erkenntnisse:
schlechte messwerte (niedrige RSSI) aus der messung ignorieren hilft nur sehr sehr bedingt.. eher im gegenteil. meist geht der fehler (stark) hoch
schlechteste messung weglassen ist auch schlecht
sigma je nach signalstärke anpassen bringt leider auch nichts. wenn man das aber macht,
dann: fuer grosse signalstaerken ein grosses sigma! andersrum gehts nach hinten los!
veto funktioniert auch nicht immer. es gibt stellen da ist ein AP wegen abschattung in der realität nicht sichtbar
das smpartphone sieht ihn deshalb nicht, im model ist er aber fälschlicherweise da deshalb falsches veto
oder das smartphone sieht einen AP wegen kollisionen nicht oder weil er durch den rücken stark verdeckt wird
es gibt einfach stellen an denen das wifi nicht eindeutig ist, die an anderen stellen quasi exakt genauso vorliegen
da laesst sich dann nicht viel machen
}
\todo{
das bbox modell hat probleme an den uebergängen zwischen bboxes da dort teils starke spruenge sind
die nicht immer in der realität so auch vorliegen. z.B. z-wechsel machen teils probleme.
hier wäre ein kontinuierliches modell hilfreich bzw interpolation in randbereichen
}
\todo{
wenn ich beim fingerprinten einen AP an einer stelle NICHT gesehen habe,
ist das auch eine aussage für die model optimierung.. da kann dann sicher keine signatlstaerke > -90 an der stelle raus kommen
}
\todo{
wir wollen nicht, dass die position des ground-truths durch das wifi so wahrscheinlich wie möglich ist,
wir wollen dass die position des ground-truth einfach eine höhere wahrscheinlichkeit hat, als alle anderen punkte im gebäude
das pruefen wir ab
}
\begin{figure}
\centering
\input{gfx/compare-wifi-in-out.tex}
\caption{
Measurable signal strengths of a testing \docAPshort{} (black dot).
While the signal diminishes slowly along the corridor (upper rectangle)
the metallised windows (dashed outline) attenuate the signal by over \SI{30}{\decibel} (lower rectangle).
}
\end{figure}
ware das grid-model nicht da, wuerde der outdoor teil richtig schlecht laufen,
weil das wlan hier absolut ungenau ist.. da die partikel aber aufgrund des vorherigen

12
tex/chapters/system.tex
View File

@@ -58,11 +58,15 @@
$p(\mStateVec_{t} \mid \mStateVec_{t-1}, \mObsVec_{t-1})$ are sampled
based on those sensor values.
Thus, the overall system works as described in \cite{Ebner-16}.
As this work focuses on \docWIFI{} optimization, not all parts of
the localization system are discussed in detail.
For missing explanations please refer to \cite{Ebner-16}.
%
Since then, absolute heading and GPS have been added as additional sensors
to further enhance the localization.
\todo{neues resampling?}
to further enhance the localization by comparing the sensor values
using some distribution.
\todo{neues resampling?}
\todo{ueberleitung}
\todo{
@@ -70,7 +74,7 @@
dafür braucht man entweder viele fingerprints oder ein modell
}
As GPS will only work outdoors, e.g. when moving from one building into another,
the system's absolute position is solely provided by the \docWIFI{} component.
the system's absolute position indoors is solely provided by the \docWIFI{} component.
Therefore its crucial for this component to provide location estimations
that are as accurate as possible, while ensuring fast setup and
maintenance times.

17
tex/chapters/work.tex
View File

@@ -99,6 +99,11 @@
algorithms like gradient descent \todo{cite} and (downhill) simpelx \todo{cite}
are well suited.
\todo{formel fuer opt: was wird optimiert?}
\begin{equation}
argmin_{bla} blub()
\end{equation}
\begin{figure}
\input{gfx/wifiop_show_optfunc_params}
\label{fig:wifiOptFuncTXPEXP}
@@ -141,6 +146,9 @@
\subsection{Modified Signal Strength Model}
\todo{nicht: during initial eval, sondern gleich sagen, dass die vermutung nahe liegt, dass das modell
nicht gut klappen wird, weil waende und unser metall-glas nicht beruecksichtigt werden. deshalb
versuchen wir ein anderes modell das immernoch live arbeiten kann}
During the initial eval, some issues were discovered. While aforementioned optimization was able to
reduce the error between reference measurements and model estimations to \SI{50}{\percent},
the position estimation \ref{eq:wifiProb} did not benefit from improved model parameters.
@@ -164,6 +172,15 @@
function like average, median or maximum.
\todo{abs-head ist in der observation besser, weil es beim resampling mehr bringt und dafuer srogt, dass die richtigen geloescht werden!}
\todo{anfaenglich falsches heading ist gift, wegen rel. heading, weil sich dann alles verlaeuft. fix: anfaenglich große heading variation erlauben}
\todo{wifi-veto erklaeren. wir fragen die 4 laut model an jeder pos staerksten APs ab und vergleichen die mit dem scan.
weichen min 50 prozent um mehr als 20 dB ab, oder sind im scan nicht gesehen worden, wird fuer diese position ein veto eingelegt:
0.001 vs 0.999. das geht auch nur so, da ja an jeder position andere APs die staerksten waeren.. direkt mit deren wahrscheinlichkeiten
zu arbeiten wuerde also aepfel mit birnen vergleichen}
wie wird optimiert
a) bekannte pos + empirische params
b) bekannte pos + opt params (fur alle APs gleich) [simplex]

77
wifi/EvalWiFiGround.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef EVALWIFIGROUND_H
#define EVALWIFIGROUND_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 "../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>
class EvalWiFiGround {
private:
WiFiModel* mdl;
WiFiObserverFree* obs;
PlotWiFiGroundProb* groundProb;
public:
EvalWiFiGround(Floorplan::IndoorMap* map, const std::string calibModel) {
mdl = WiFiModelFactory(map).loadXML(calibModel);
obs = new WiFiObserverFree(8.0, *mdl);
groundProb = new PlotWiFiGroundProb(map);
}
void show(const std::string walkFile) {
Offline::FileReader reader(walkFile);
int cnt = 0;
for (const auto& entry : reader.getWiFiGroupedByTime()) {
const WiFiMeasurements& mes = entry.data;
groundProb->show(*obs, mes);
groundProb->plotMe();
//if (++cnt > 70) {break;}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
//groundProb->update();
groundProb->plotMe();
int i = 0; (void) i;
}
};
#endif // EVALWIFIGROUND_H

555
wifi/EvalWiFiPathMethods.h Normal file
View File

@@ -0,0 +1,555 @@
#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

226
wifi/EvalWiFiPaths.h Normal file
View File

@@ -0,0 +1,226 @@
#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("\\small{error (m)}", "\\small{measurements (\\%)}");
pef_m->showMarkers(false);
pef_p = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}");
pef_p->showMarkers(false);
pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));
}
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