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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/museumLoc

Browse Source
This commit is contained in:
Markus Bullmann
2018-07-31 15:38:14 +02:00
parent 3aaa470422 1e52b377dc
commit 713494a675
7 changed files with 241 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
main.cpp
View File

@@ -20,11 +20,16 @@
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/model/WiFiModels.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerPerFloor.h>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <sys/stat.h>
Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
@@ -54,32 +59,67 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// wifi
WiFiModelLogDistCeiling WiFiModel(map);
// WiFiModelLogDistCeiling WiFiModel(map);
// WiFiModelPerFloor WiFiModelPerFloor(map);
// WiFiModelPerBBox WiFiModelPerBBox(map);
WiFiModel* WiFiModel = nullptr;
// with optimization
if(Settings::WiFiModel::optimize){
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
WiFiModel.addAP(ap.mac, entry);
}
WiFiModel.saveXML(setup.wifiModel);
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
if (Settings::WiFiModel::useRegionalOpt) {
// use a regional optimization scheme (one per floor)
WiFiOptimizerPerFloor opt(map);
// add all fingerprints to the optimizer (optimizer will add them to the correct floor/model)
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
WiFiModel = opt.optimizeAll();
WiFiModel->saveXML(setup.wifiModel);
} else {
// use one model per AP for the whole map
WiFiModel = new WiFiModelLogDistCeiling(map);
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
((WiFiModelLogDistCeiling*)WiFiModel)->addAP(ap.mac, entry);
}
WiFiModel->saveXML(setup.wifiModel);
}
} else {
WiFiModel.loadXML(setup.wifiModel);
// load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map);
WiFiModel = fac.loadXML(setup.wifiModel);
}
} else {
// without optimization
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
WiFiModel = new WiFiModelLogDistCeiling(map);
((WiFiModelLogDistCeiling*)WiFiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}
@@ -92,8 +132,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.7);
//const Point3 srcPath2(62, 38, 1.8);
//const Point3 srcPath3(62, 38, 1.8);
const Point3 srcPath2(62, 38, 1.8);
const Point3 srcPath3(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
@@ -114,13 +154,14 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// particle-filter
const int numParticles = 5000;
auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
//auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh);
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(*WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh, *WiFiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, Settings::KDE::gridSize, Settings::KDE::bandwidth);
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
@@ -154,6 +195,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
@@ -184,6 +226,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (ctrl.numStepsSinceLastEval > 0) {
obs.currentTime = ts;
ctrl.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// }
@@ -192,7 +236,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
lastTimestamp = ts;
ctrl.lastEstimate = est.pos.pos;
//plot
//dbg.showParticles(pf.getParticles());
@@ -207,22 +251,17 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.plot();
plot.setActivity((int) act.get());
plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
errorStats.add(err_m);
errorFile << err_m << "\n";
errorFile << ts.ms() << " " << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
if(ts.ms() == 13410 || ts.ms() == 20802){
std::ofstream plotFile;
plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
}
}
}
@@ -241,14 +280,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
errorFile.close();
//save the .gp buffer into a file
// std::ofstream plotFile;
// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
// dbg.saveToFile(plotFile);
// plotFile.close();
/* plot in gp file */
std::ofstream plotFile;
plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
//save also a png image, just for a better overview
// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot();
return errorStats;
}
@@ -261,13 +301,38 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
Settings::DataSetup set = Settings::data.Path1;
std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
std::string evaluationName = "museum/tmp";
for(int i = 0; i < 1; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(set, j, evaluationName);
//TODO: in transition die distance über KLD noch einkommentieren als Test
// for(int j = 0; j < Settings::data.Path0.training.size(); ++j){
// tmp = run(Settings::data.Path0, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path1.training.size(); ++j){
// tmp = run(Settings::data.Path1, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path2.training.size(); ++j){
// tmp = run(Settings::data.Path2, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
@@ -299,6 +364,6 @@ int main(int argc, char** argv) {
finalStatisticFile.close();
std::this_thread::sleep_for(std::chrono::seconds(60));
//std::this_thread::sleep_for(std::chrono::seconds(60));
}