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

aktuell spielerein

Browse Source
This commit is contained in:
toni
2018-08-01 10:08:33 +02:00
parent 1e52b377dc
commit 38042f8dad
5 changed files with 145 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
main.cpp
View File

@@ -28,6 +28,7 @@
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
#include <sys/stat.h>
@@ -45,6 +46,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats;
//calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
double distance = 0;
for(int i = 1; i < gtEntries.size(); ++i){
distance += gtEntries[i].value.getDistance(gtEntries[i-1].value);
}
std::cout << "Distance of Path: " << distance << std::endl;
// error file
const long int t = static_cast<long int>(time(NULL));
auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
@@ -63,7 +73,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// WiFiModelPerFloor WiFiModelPerFloor(map);
// WiFiModelPerBBox WiFiModelPerBBox(map);
WiFiModel* WiFiModel = nullptr;
WiFiModel* wifiModel = nullptr;
// with optimization
if(Settings::WiFiModel::optimize){
@@ -76,22 +86,23 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// use a regional optimization scheme (one per floor)
WiFiOptimizerPerFloor opt(map);
WiFiOptimizer::PerFloor opt(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
WiFiOptimizer::LogDistCeiling ldc(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
// 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);
wifiModel = opt.optimizeAll(ldc.MIN_2_FPS);
wifiModel->saveXML(setup.wifiModel);
} else {
/** NOTE: Funktioniert fürs Museum VIEL VIEL Besser */
// use one model per AP for the whole map
WiFiModel = new WiFiModelLogDistCeiling(map);
wifiModel = new WiFiModelLogDistCeiling(map);
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
@@ -99,10 +110,10 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
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);
((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry);
}
WiFiModel->saveXML(setup.wifiModel);
wifiModel->saveXML(setup.wifiModel);
}
@@ -111,15 +122,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map);
WiFiModel = fac.loadXML(setup.wifiModel);
wifiModel = fac.loadXML(setup.wifiModel);
}
} else {
// without optimization
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");
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");
}
@@ -156,15 +167,16 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
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, *WiFiModel);
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::ParticleFilterResamplingKDE<MyState, MyNavMeshTriangle>>(mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
@@ -187,6 +199,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
int i = 0;
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
@@ -248,19 +262,35 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.showParticles(pf.getParticles());
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
//plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.setActivity((int) act.get());
plot.plot();
//plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
// float err_m = gtPos.getDistance(est.pos.pos);
// errorStats.add(err_m);
// errorFile << ts.ms() << " " << err_m << "\n";
//error calc with penalty for wrong floor
double errorFactor = 3.0;
Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
float err_m = gtPosError.getDistance(estError);
errorStats.add(err_m);
errorFile << ts.ms() << " " << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
plot.gp << "set terminal png size 1280,720\n";
plot.gp.setOutput("/tmp/videoSparkasse/" + std::to_string(i++) + ".png");
int degree = ((30 - i) % 360);
if (degree < 0){degree += 360;}
plot.gp << "set view 63,"<< degree << "\n";
plot.gp << "set autoscale xy\n";
plot.gp << "set autoscale z\n";
plot.plot();
}
}
@@ -289,6 +319,33 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot();
plot.closeStream();
// //save file for radius_sub
// std::ofstream radiusFile;
// radiusFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_radius.csv");
// //provide the maximum value for radius_sub (kld * wifiquality)
// double max = *std::max_element(((MyPFTrans*) pf.getTransition())->listRadiusSub.begin() , ((MyPFTrans*) pf.getTransition())->listRadiusSub.end());
// std::cout << "Max Value radius_sub: " << max << std::endl;
// double sum = 0;
// int pos = ((MyPFTrans*) pf.getTransition())->listRadiusSub.size() * 0.75;
// double quantilValue = 0;
// int num = 0;
// for(double val : ((MyPFTrans*) pf.getTransition())->listRadiusSub){
// sum += val;
// if(num == pos){quantilValue = val;}
// ++num;
// radiusFile << val << "\n";
// }
// double avg = sum / num;
// std::cout << "Average radius_sub: " << avg << std::endl;
// std::cout << "75 Quantil radius_sub: " << quantilValue << std::endl;
// radiusFile.close();
return errorStats;
}
@@ -300,7 +357,8 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
std::string evaluationName = "museum/tmp";
//Path2_Museum_KDE_SimpleResampling
std::string evaluationName = "museum/videoSparkasse";
for(int i = 0; i < 1; ++i){
@@ -330,7 +388,7 @@ int main(int argc, char** argv) {
// statsQuantil.add(tmp.getQuantile(0.75));
// }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
for(int j = 0; j < 1; ++j){
tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());