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

started with eval

Browse Source
This commit is contained in:
FrankE
2017-03-24 13:31:31 +01:00
parent 94b7e59953
commit 25e9b823eb
8 changed files with 567 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
EvalData.h Normal file
View File

@@ -0,0 +1,52 @@
#ifndef EVALDATA_H
#define EVALDATA_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"
class EvalData {
public:
/**
* read path
* dump gps measurements to cout
* dump gps measurements to googleLines for googleMaps vis
*/
static void dumpGPSforPath(const std::string& fPath) {
std::cout << "dumping GPS for " << fPath << std::endl;
Offline::FileReader fr(fPath);
std::vector<EarthPos> poss;
for (const auto gps : fr.gps) {
std::cout << gps.ts << ":" << gps.data.asString() << std::endl;
EarthPos pos(gps.data.lat, gps.data.lon, gps.data.alt);
poss.push_back(pos);
}
LeHelper::writeGoogleLine(poss);
}
};
#endif // EVALDATA_H

32
EvalWalk.h
View File

@@ -1,4 +1,36 @@
#ifndef EVALWALK_H
#define EVALWALK_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"
class EvalWalk {
public:
EvalWalk() {
}
};
#endif // EVALWALK_H

266
EvalWiFi.h Normal file
View File

@@ -0,0 +1,266 @@
#ifndef EVALWIFI_H
#define EVALWIFI_H
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/VAPGrouper.h"
#include "Indoor/sensors/offline/FileReader.h"
#include "Indoor/floorplan/v2/Floorplan.h"
#include "Indoor/floorplan/v2/FloorplanReader.h"
#include "Indoor/floorplan/v2/FloorplanHelper.h"
#include "Indoor/floorplan/v2/FloorplanCeilings.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
#include <KLib/math/statistics/Statistics.h>
#include "Structs.h"
#include "Plotty.h"
#include "CSV.h"
#include <unordered_set>
template <typename T> class Line {
private:
std::vector<T> elements;
public:
void add(const T& elem) {
elements.push_back(elem);
}
std::vector<T> getAverage(const int size) {
std::vector<T> res;
for (int i = 0; i < (int)elements.size(); ++i) {
T sum;
int cnt = 0;
// calculate sume of all elements around i
for (int j = -size; j <= +size; ++j) {
int idx = i+j;
if (idx < 0) {continue;}
if (idx >= elements.size()) {continue;}
sum += elements[idx];
++cnt;
}
// calculate average
T avg = sum / cnt;
res.push_back(avg);
}
return res;
}
};
/**
* read path
* fetch wifi
* use given model to estimate the most likely location
* -> WIFI ONLY
*/
class EvalWiFi {
private:
Floorplan::IndoorMap* map;
BBox3 mapBBox;
//WiFiFingerprints* calib;
VAPGrouper* vap = nullptr;
//WiFiOptimizer::LogDistCeiling* opt;
Offline::FileReader reader;
WiFiModel* wiModel = nullptr;
std::vector<int> gtIndices;
public:
/** ctor with map and fingerprints */
EvalWiFi(const std::string& mapFile, const std::string& fPath, const std::vector<int> gtIndices) : reader(fPath), gtIndices(gtIndices) {
std::cout << "EvalWiFi for " << fPath << std::endl;
// load floorplan
map = Floorplan::Reader::readFromFile(mapFile);
// estimate bbox
mapBBox = FloorplanHelper::getBBox(map);
// // how to handle VAPs
// vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// the optimizer
// opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);
}
void fixedParams(const float txp, const float exp, const float waf) {
// how to handle VAPs
vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// setup the model
WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map);
wiModel->loadAPs(map, *vap, txp, exp, waf, false);
this->wiModel = wiModel;
// fire
run();
}
private:
/** is the given mac one of a FHWS ap? */
bool isFHWS_AP(const MACAddress& mac) {
return mac.asString().substr(0,5) == "D8:84";
}
void run() {
Plotty* plot = new Plotty(map);
Line<Point3> path;
plot->setGroundTruth(gtIndices);
// process each wifi entry within the offline file
for (const auto wifi : reader.wifi) {
// all seen APs at one timestamp
const WiFiMeasurements& _mes = wifi.data;
// debug output
std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl;
// perform vap grouping
const WiFiMeasurements mes = vap->group(_mes);
// error calculation
auto func = [&] (float* params) -> double {
// crop z to 1 meter
params[2] = std::round(params[2]);
// suggested position
const Point3 pos_m(params[0], params[1], params[2]);
const float sigma = 6.0;
double prob = 1.0;
// calculate error for above position using the currently available measurements
for (const WiFiMeasurement& m : mes.entries) {
// skip non-FHWS APs
if (!isFHWS_AP(m.getAP().getMAC())) {continue;}
// get model's rssi for the given location
const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);
// skip APs unknown to the model
if (rssi_model != rssi_model) {
std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
continue;
}
// get scan's rssi
const float rssi_scan = m.getRSSI();
// likelyhood
const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
// adjust
prob *= p;
}
const double err = -prob;
return err;
};
std::minstd_rand gen;
std::uniform_real_distribution<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
std::uniform_real_distribution<float> distZ(mapBBox.getMin().z, mapBBox.getMax().z);
// initializer for the optimizer: random position within the map's bbox
auto init = [&] (const int childIdx, float* params) {
params[0] = distX(gen);
params[1] = distY(gen);
params[2] = distZ(gen);
};
float params[3];
K::NumOptAlgoGenetic<float> opt(3);
opt.setPopulationSize(200);
opt.setMaxIterations(20);
opt.calculateOptimum(func, params, init);
std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
path.add(Point3(params[0], params[1], params[2]));
// draw a smoothed version of th epath
plot->pathEst.clear();
for (const Point3 p : path.getAverage(1)) {
const K::GnuplotPoint3 gp3(p.x, p.y, p.z);
plot->pathEst.add(gp3);
}
plot->plot();
}
}
// // TODO
// void abc(const std::string& fpFile) {
// // load fingerprints
// calib = new WiFiFingerprints(fpFile);
// // how to handle VAPs
// VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// // the optimizer
// opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);
// }
// static void dumpWiFiCenterForPath(coconst std::string& fPath) {
// std::cout << "dump WiFi for " << fPath << std::endl;
// Offline::FileReader fr(fPath);
// WiFiModel logDistC
// for (const auto wifi : fr.wifi) {
// std::cout << wifi.ts << ":" << wifi.data.entries.size() << std::endl;
// }
// }
};
#endif // EVALWIFI_H

29
Helper.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef LEHELPER_H
#define LEHELPER_H
#include <fstream>
#include <Indoor/geo/EarthPos.h>
class LeHelper {
public:
static void writeGoogleLine(std::vector<EarthPos>& poss) {
const std::string file = "/apps/android/workspace/OTHER2017/osm/googleLine.js";
std::ofstream out(file);
out << "function getLines() {return [\n";
out.precision(15);
for (const EarthPos& pos : poss) {
out << "\t{" << "lat:" << pos.lat << ", lng:" << pos.lon << "},\n";
}
out << "];}";
out.flush();
out.close();
}
};
#endif // HELPER_H

46
Plotty.h
View File

@@ -1,19 +1,33 @@
#ifndef PLOTTY_H
#define PLOTTY_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.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>
class Plotty {
public:
Floorplan::IndoorMap* map;
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints points;
K::GnuplotSplotElementColorPoints cpoints;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementLines pathReal;
K::GnuplotSplotElementLines pathEst;
K::GnuplotSplotElementLines mapOutline;
public:
Plotty(Floorplan::IndoorMap* map) {
Plotty(Floorplan::IndoorMap* map) : map(map) {
//gp << "set view equal xy\n";
@@ -25,24 +39,38 @@ public:
gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
// draw floorplan
splot.add(&lines);
mapOutline.setColorHex("#888888");
splot.add(&mapOutline);
for (Floorplan::Floor* floor : map->floors) {
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
lines.addSegment(p1, p2);
mapOutline.addSegment(p1, p2);
}
}
for (Floorplan::Stair* s : floor->stairs) {
for (const Floorplan::StairPart& sp : s->getParts()) {
const K::GnuplotPoint3 p1(sp.start.x, sp.start.y, sp.start.z + floor->atHeight);
const K::GnuplotPoint3 p2(sp.end.x, sp.end.y, sp.end.z + floor->atHeight);
lines.addSegment(p1, p2);
mapOutline.addSegment(p1, p2);
}
}
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
gp << "set object polygon from ";
for (size_t i = 0; i < poly->poly.points.size(); ++i) {
if (i > 0) {gp << " to ";}
const Point2 pt = poly->poly.points[i];
gp << pt.x << "," << pt.y << "," << floor->atHeight << " ";
}
gp << " fs solid fc rgb " << ( poly->outdoor ? "'#ddffdd'" : "'#dddddd'");
gp << "\n";
}
}
splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#0000ff");
splot.add(&pathEst);
splot.add(&points);
points.setPointType(7);
@@ -93,6 +121,14 @@ public:
gp << "set title '" << title << "'\n";
}
void setGroundTruth(const std::vector<int> indices) {
const std::vector<Point3> path = FloorplanHelper::getGroundTruth(map, indices);
pathReal.clear();
for (const Point3& p : path) {
pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z));
}
}
void plot() {
gp.draw(splot);
gp.flush();

37
Settings.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef SETTINGS_H
#define SETTINGS_H
#include <string>
#include <vector>
namespace Settings {
const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/";
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";
const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL37.xml";
const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat";
namespace GroundTruth {
// IPIN2017 [toni]
//const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
//const std::vector<int> path2 = {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> path3 = {31, 32, 33, 34, 35, 36, 37, 38};
// OTHER2017 [frank]
const std::vector<int> path1 = {40, 41, 42, 43, 44, 45, 1, 0, 46, 47, 48, 49, 50, 51, 52};
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};
}
}
#endif // SETTINGS_H

73
TestMapEarthReg.h Normal file
View File

@@ -0,0 +1,73 @@
#ifndef TESTMAPEARTHREG_H
#define TESTMAPEARTHREG_H
/**
* test map<-> earth registration
* by converting the map to lon/lat and exporting to google-maps
*/
void testMapEarthReg(const std::string& fMap) {
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(fMap);
std::ofstream outOSM("/apps/android/workspace/OTHER2017/osm/osm.txt");
outOSM << "lat lon title icon iconSize iconOffset\n";
outOSM.precision(20);
// our markers
std::ofstream outGooglePOI("/apps/android/workspace/OTHER2017/osm/googlePOI.js");
outGooglePOI << "function getPOI() {return [\n";
outGooglePOI.precision(20);
int cnt = 0;
for (Floorplan::EarthPosMapPos* em : map->earthReg.correspondences) {
//mapCenter += em->posOnMap_m.xy();
//earthCenter.lat += em->posOnEarth.lat;
//earthCenter.lon += em->posOnEarth.lon;
outOSM << em->posOnEarth.lat << "\t" << em->posOnEarth.lon << "\t" << "title" << "\t" << "osm.png" << "\t" << "16,16" << "\t" << "-8,-8" << "\n";
outGooglePOI << "\t{lat:" << em->posOnEarth.lat << ", lng:" << em->posOnEarth.lon << "},\n";
++cnt;
}
outGooglePOI << "];}";
outGooglePOI.flush();
outOSM.flush();
//const EarthMapping em1(Point3(mapCenter.x, mapCenter.y, 0), earthCenter, 0);
// construct mapper
const EarthMapping em(map);
// export building's first floor
std::ofstream outGooglePoly("/apps/android/workspace/OTHER2017/osm/googlePoly.js");
outGooglePoly << "function getPoly() {return [\n";
outGooglePoly.precision(20);
// export to google maps lon/lat
Floorplan::Floor* f1 = map->floors[1];
for (Floorplan::FloorOutlinePolygon* poly : f1->outline) {
if (poly->method != Floorplan::OutlineMethod::ADD) {continue;}
if (poly->outdoor) {continue;}
outGooglePoly << "[\n";
for (int i = 0; i <= poly->poly.points.size(); ++i) {
const Point2 p = poly->poly.points[i % poly->poly.points.size()];
const EarthPos ep = em.mapToWorld(Point3(p.x, p.y, 0));
outGooglePoly << "\t{lat:" << ep.lat << ", lng:" << ep.lon << "},\n";
}
outGooglePoly << "],";
}
outGooglePoly << "];}";
outGooglePoly.flush();
int i = 0; (void) i;
}
#endif // TESTMAPEARTHREG_H

45
main.cpp
View File

@@ -13,24 +13,53 @@
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <Indoor/geo/EarthMapping.h>
#include "Settings.h"
#include "EvalCompareOpt.h"
#include "EvalApOpt.h"
#include "EvalWalk.h"
#include "EvalData.h"
#include "EvalWiFi.h"
#define BREAK raise(SIGTRAP);
int main(void) {
const std::string fMap = "/apps/android/workspace/OTHER2017/data/SHL35.xml";
const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat";
// test gps within data files
if (1 == 0) {
EvalData::dumpGPSforPath(Settings::path1a); BREAK;
EvalData::dumpGPSforPath(Settings::path1b); BREAK;
EvalData::dumpGPSforPath(Settings::path2a); BREAK;
EvalData::dumpGPSforPath(Settings::path2b); BREAK;
}
// test wifi within data files
if (1 == 1) {
//EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2);
EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path1);
//ew1.fixedParams(-40, 2.5, -8); BREAK;
ew1.fixedParams(-64.5905, 1.25988, -2.47863); BREAK;
}
// run walking
// run earthmapping
//testMapEarthReg(fMap);
// EvalApOpt eval(fMap, fCalib);
// eval.optAll();
{
EvalCompareOptAllFixed allFixed(fMap, fCalib);
//allFixed.fixedPosFixedParamsForAll();
//allFixed.fixedPosOptParamsForAll();
//allFixed.fixedPosOptParamsForEach();
// run evaluation
if (1 == 0) {
EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib);
allFixed.fixedPosFixedParamsForAll();
allFixed.fixedPosOptParamsForAll();
allFixed.fixedPosOptParamsForEach();
allFixed.optPosOptParamsForEach();
}