diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index 3afaa25..af2a3be 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -34,7 +34,6 @@ FILE(GLOB HEADERS Settings.h FtmKalman.h main.h - mainFtm.h trilateration.h Plotta.h misc.h @@ -46,7 +45,6 @@ FILE(GLOB SOURCES ../../Indoor/lib/tinyxml/tinyxml2.cpp ../../Indoor/lib/Recast/*.cpp main.cpp - mainFtm.cpp mainTrilat.cpp mainProb.cpp Eval.cpp diff --git a/code/main.cpp b/code/main.cpp index e2db731..7ba79d3 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -411,8 +411,6 @@ int main(int argc, char** argv) return mainTrilat(argc, argv); } - //mainFtm(argc, argv); - //return 0; CombinedStats statsAVG; CombinedStats statsMedian; CombinedStats statsSTD; diff --git a/code/mainFtm.cpp b/code/mainFtm.cpp deleted file mode 100644 index d77e6f7..0000000 --- a/code/mainFtm.cpp +++ /dev/null @@ -1,278 +0,0 @@ -#include "mainFtm.h" - -#include "mesh.h" -#include "filter.h" -#include "Settings.h" -//#include "meshPlotter.h" -#include "Plotty.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include - -#include "FtmKalman.h" - -#include - -static Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string folder) { - - // reading file - std::string currDir = std::filesystem::current_path().string(); - - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[numFile]); - - // ground truth - std::vector gtPath = setup.gtPath; - Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); - Stats::Statistics errorStats; - - //calculate distance of path - std::vector::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(time(NULL)); - auto evalDir = std::filesystem::path(Settings::errorDir); - evalDir.append(folder); - - if (!std::filesystem::exists(evalDir)) { - std::filesystem::create_directory(evalDir); - } - - std::ofstream errorFile; - errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); - - - // wifi - auto kalmanMap = std::make_shared>(); - kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev) }); - - // mesh - NM::NavMeshSettings set; - MyNavMesh mesh; - MyNavMeshFactory fac(&mesh, set); - fac.build(map); - - const Point3 srcPath0(9.8, 24.9, 0); // fixed start pos - - // add shortest-path to destination - //const Point3 dst(51, 45, 1.7); - //const Point3 dst(25, 45, 0); - //NM::NavMeshDijkstra::stamp(mesh, dst); - - // debug show - //MeshPlotter dbg; - //dbg.addFloors(map); - //dbg.addOutline(map); - //dbg.addMesh(mesh); - ////dbg.addDijkstra(mesh); - //dbg.draw(); - - Plotty plot(map); - plot.buildFloorplan(); - plot.setGroundTruth(gtPath); - plot.plot(); - - // particle-filter - const int numParticles = 5000; - //auto init = std::make_unique(&mesh, srcPath0); // known position - auto init = std::make_unique(&mesh); // uniform distribution - auto eval = std::make_unique(); - eval->assignProps = true; - - //auto trans = std::make_unique(mesh); - auto trans = std::make_unique(); - - auto resample = std::make_unique>(); - auto estimate = std::make_unique>(); - - // setup - MyFilter pf(numParticles, std::move(init)); - pf.setEvaluation(std::move(eval)); - pf.setTransition(std::move(trans)); - pf.setResampling(std::move(resample)); - pf.setEstimation(std::move(estimate)); - //pf.setNEffThreshold(0.85); - pf.setNEffThreshold(0.0); - - // sensors - MyControl ctrl; - MyObservation obs; - - StepDetection sd; - PoseDetection pd; - TurnDetection td(&pd); - RelativePressure relBaro; - ActivityDetector act; - relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); - Timestamp lastTimestamp = Timestamp::fromMS(0); - - - // parse each sensor-value within the offline data - for (const Offline::Entry& e : fr.getEntries()) { - - const Timestamp ts = Timestamp::fromMS(e.ts); - - if (e.type == Offline::Sensor::WIFI_FTM) { - auto ftm = fr.getWifiFtm()[e.idx].data; - - float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset; - float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset - - auto& kalman = kalmanMap->at(ftm.getAP().getMAC()); - float predictDist = kalman.predict(ts, ftmDist); - - //ftm.setFtmDist(predictDist); - - obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm); - } - - //if (ctrl.numStepsSinceLastEval > 0) - //if (ts - lastTimestamp >= Timestamp::fromMS(500)) - if (obs.wifi.size() == 4) - { - - obs.currentTime = ts; - ctrl.currentTime = ts; - -// if(ctrl.numStepsSinceLastEval > 0){ -// pf.updateTransitionOnly(&ctrl); -// } - MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs); - ctrl.afterEval(); - Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1); - lastTimestamp = ts; - - ctrl.lastEstimate = est.pos.pos; - - - // draw wifi ranges - for (auto& ftm : obs.wifi) - { - int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID; - - if (nucid == 1) - { - Point3 apPos = Settings::data.CurrentPath.NUCs.find(ftm.first)->second.position; - // plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist()); - } - } - - obs.wifi.clear(); - - //plot - //dbg.showParticles(pf.getParticles()); - //dbg.setCurPos(est.pos.pos); - //dbg.setGT(gtPos); - //dbg.addEstimationNode(est.pos.pos); - //dbg.addGroundTruthNode(gtPos); - //dbg.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); - //dbg.draw(); - - plot.showParticles(pf.getParticles()); - plot.setCurEst(est.pos.pos); - plot.setGroundTruth(gtPos); - - plot.addEstimationNode(est.pos.pos); - plot.setActivity((int) act.get()); - plot.splot.getView().setCamera(0, 0); - //plot.splot.getView().setEqualXY(true); - - plot.plot(); - - - //std::this_thread::sleep_for(500ms); - - // error calc -// 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"; - } - } - - - - // get someting on console - std::cout << "Statistical Analysis Filtering: " << std::endl; - std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; - - // save the statistical data in file - errorFile << "========================================================== \n"; - errorFile << "Average of all statistical data: \n"; - errorFile << "Median: " << errorStats.getMedian() << "\n"; - errorFile << "Average: " << errorStats.getAvg() << "\n"; - errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n"; - errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n"; - errorFile.close(); - - return errorStats; -} - -int mainFtm(int argc, char** argv) { - - Stats::Statistics statsAVG; - Stats::Statistics statsMedian; - Stats::Statistics statsSTD; - Stats::Statistics statsQuantil; - Stats::Statistics tmp; - - std::string evaluationName = "prologic/tmp"; - - for(int i = 0; i < 1; ++i){ - for(int j = 0; j < 1; ++j){ - tmp = run(Settings::data.CurrentPath, j, evaluationName); - statsMedian.add(tmp.getMedian()); - statsAVG.add(tmp.getAvg()); - statsSTD.add(tmp.getStdDev()); - statsQuantil.add(tmp.getQuantile(0.75)); - } - - std::cout << "Iteration " << i << " completed" << std::endl; - } - - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data: " << std::endl; - std::cout << "Median: " << statsMedian.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - - - //std::this_thread::sleep_for(std::chrono::seconds(60)); - - - return 0; -} diff --git a/code/mainFtm.h b/code/mainFtm.h deleted file mode 100644 index c59939c..0000000 --- a/code/mainFtm.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once - -int mainFtm(int argc, char** argv); -