diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index ff9697b..ac533f1 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -35,6 +35,9 @@ FILE(GLOB HEADERS FtmKalman.h main.h mainFtm.h + trilateration.h + Plotta.h + misc.h ) @@ -43,6 +46,8 @@ FILE(GLOB SOURCES ../../Indoor/lib/Recast/*.cpp main.cpp mainFtm.cpp + mainTrilat.cpp + mainProb.cpp ) @@ -113,5 +118,12 @@ TARGET_LINK_LIBRARIES( # pthread ) +if ("${CMAKE_GENERATOR}" MATCHES "Visual Studio*") + get_filename_component(smartCmdArgJsonFile "ftmprologic.args.json" ABSOLUTE) + MESSAGE(STATUS "Path to json file: " ${smartCmdArgJsonFile}) + + set_property(TARGET ProLogic PROPERTY VS_GLOBAL_SmartCmdArgJsonFile ${smartCmdArgJsonFile}) +endif() + SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) diff --git a/code/Plotta.h b/code/Plotta.h new file mode 100644 index 0000000..fb3fe9b --- /dev/null +++ b/code/Plotta.h @@ -0,0 +1,149 @@ +#pragma once; + + +#include +#include +#include +#include +#include + +namespace Plotta +{ + namespace fs = std::filesystem; + class plottastream : public std::stringstream { }; + + + struct Plotta + { + private: + struct DataItem + { + std::string name; + std::string dataStr; + }; + + const fs::path scriptFile; + const fs::path dataFile; + + std::unordered_map data; + + + public: + Plotta(const fs::path& scriptFile, const fs::path& dataFile) + : scriptFile(scriptFile), dataFile(dataFile) + { + + } + + void frame() + { + // send data + std::ofstream stream; + stream.open(dataFile); + + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + + stream << "# genertated at " << std::put_time(&tm, "%d.%m.%Y %T") << "\n"; + stream << "import numpy as np" << "\n\n"; + + for (const auto& item : data) + { + stream << item.second.dataStr << "\n"; + } + + stream.close(); + } + + template + void add(std::string name, const std::vector& value) + { + plottastream stream; + + stream << name << " = "; + stream << value; + + DataItem item; + item.name = name; + item.dataStr = stream.str(); + + data.insert_or_assign(item.name, item); + } + + template + void add(std::string name, const std::initializer_list& value) + { + plottastream stream; + + stream << name << " = "; + stream << value; + + DataItem item; + item.name = name; + item.dataStr = stream.str(); + + data.insert_or_assign(item.name, item); + } + + private: + + void start() + { + // TODO + // start python script + // connect + assert(false); + } + }; + + template + static plottastream& writeNumpyArray(plottastream& stream, Iterator begin, Iterator end) + { + stream << "np.array(["; + + for (Iterator it = begin; it != end; ++it) + { + stream << *it; + if (it != end - 1) + { + stream << ", "; + } + } + + stream << "])"; + return stream; + } + + template + plottastream& operator<<(plottastream& stream, const std::vector& list) + { + return writeNumpyArray(stream, list.begin(), list.end()); + } + + template + plottastream& operator<<(plottastream& stream, const std::initializer_list& list) + { + return writeNumpyArray(stream, list.begin(), list.end()); + } + + + template + static plottastream& operator<<(plottastream& stream, const T& value) + { + // use std::strngstream conversion methods for any T + static_cast(stream) << value; + return stream; + } + + template<> + static plottastream& operator<<(plottastream& stream, const float& value) + { + return stream << (isnan(value) ? "float(\"nan\")" : std::to_string(value)); + } + + template<> + static plottastream& operator<<(plottastream& stream, const Point2& value) + { + return stream << std::initializer_list{ value.x, value.y }; + } +} \ No newline at end of file diff --git a/code/Settings.h b/code/Settings.h index ed4dd15..785a4e4 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -1,5 +1,7 @@ #pragma once +#include + #include #include #include @@ -93,7 +95,7 @@ namespace Settings { const std::string dataDir = "../measurements/data/"; const std::string errorDir = "../measurements/error/"; - const bool UseKalman = true; + const bool UseKalman = false; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -102,6 +104,27 @@ namespace Settings { const MACAddress NUC3("1c:1b:b5:ef:a2:9a"); const MACAddress NUC4("1c:1b:b5:ec:d1:82"); + static int nucIndex(const MACAddress& addr) + { + if (addr == Settings::NUC1) return 0; + if (addr == Settings::NUC2) return 1; + if (addr == Settings::NUC3) return 2; + if (addr == Settings::NUC4) return 3; + else assert(false); + } + + static MACAddress nucFromIndex(int idx) + { + switch (idx) + { + case 0: return NUC1; + case 1: return NUC2; + case 2: return NUC3; + case 3: return NUC4; + default: assert(false); + } + } + struct NUCSettings { int ID = 0; @@ -119,6 +142,11 @@ namespace Settings { std::vector training; std::unordered_map NUCs; std::vector gtPath; + + NUCSettings nucInfo(int idx) const + { + return NUCs.at(nucFromIndex(idx)); + } }; /** all configured datasets */ @@ -233,10 +261,10 @@ namespace Settings { dataDir + "Pixel2/path5/1560158988785_6_6.csv" }, { - { NUC1, {1, { 8.1, 18.7, 0.8}, 2.00, 0, 3.0f} }, // NUC 1 - { NUC2, {2, { 8.4, 27.3, 0.8}, 1.25, 0, 3.0f} }, // NUC 2 - { NUC3, {3, {21.3, 19.3, 0.8}, 2.75, 0, 3.0f} }, // NUC 3 - { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 0, 3.0f} }, // NUC 4 + { NUC1, {1, { 8.1, 18.7, 0.8}, 2.00, 3.375, 3.0f} }, // NUC 1 + { NUC2, {2, { 8.4, 27.3, 0.8}, 1.25, 3.375, 3.0f} }, // NUC 2 + { NUC3, {3, {21.3, 19.3, 0.8}, 2.75, 3.250, 3.0f} }, // NUC 3 + { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 3.375, 3.0f} }, // NUC 4 }, { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 } }; diff --git a/code/ftmprologic.args.json b/code/ftmprologic.args.json new file mode 100644 index 0000000..1ae93f0 --- /dev/null +++ b/code/ftmprologic.args.json @@ -0,0 +1,10 @@ +{ + "FileVersion": 2, + "Id": "eafb5522-91a6-3893-907d-2546127e18fd", + "Items": [ + { + "Id": "883c12c1-2ab8-42bc-8db6-3b0feb8f53ff", + "Command": "trilat" + } + ] +} \ No newline at end of file diff --git a/code/main.cpp b/code/main.cpp index 6fcd305..bb6716f 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,4 +1,4 @@ -//#include "main.h" +#include "main.h" #include "mesh.h" #include "filter.h" @@ -28,6 +28,7 @@ #include "FtmKalman.h" #include "mainFtm.h" +#include "misc.h" #include @@ -250,6 +251,11 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: plot.buildFloorplan(); plot.setGroundTruth(gtPath); plot.setView(30, 0); + // APs Positions + plot.addCircle(100000 + 0, Settings::data.CurrentPath.nucInfo(0).position.xy(), 0.05); + plot.addCircle(100000 + 1, Settings::data.CurrentPath.nucInfo(1).position.xy(), 0.05); + plot.addCircle(100000 + 2, Settings::data.CurrentPath.nucInfo(2).position.xy(), 0.05); + plot.addCircle(100000 + 3, Settings::data.CurrentPath.nucInfo(3).position.xy(), 0.05); plot.plot(); // particle-filter @@ -413,7 +419,7 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: plot.plot(); //plot.closeStream(); - //std::this_thread::sleep_for(500ms); + std::this_thread::sleep_for(100ms); // error calc // float err_m = gtPos.getDistance(est.pos.pos); @@ -449,10 +455,20 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: int main(int argc, char** argv) { + CmdArguments args(argc, argv); + + if (args.hasFlag("prob")) + { + return mainProp(argc, argv); + } + else if (args.hasFlag("trilat")) + { + return mainTrilat(argc, argv); + } + //mainFtm(argc, argv); //return 0; - Stats::Statistics statsAVG; Stats::Statistics statsMedian; Stats::Statistics statsSTD; diff --git a/code/main.h b/code/main.h index 3f59c93..09e4b6c 100644 --- a/code/main.h +++ b/code/main.h @@ -1,2 +1,5 @@ #pragma once +int mainTrilat(int argc, char** argv); + +int mainProp(int argc, char** argv); diff --git a/code/mainProb.cpp b/code/mainProb.cpp new file mode 100644 index 0000000..62e9820 --- /dev/null +++ b/code/mainProb.cpp @@ -0,0 +1,316 @@ +#include "main.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "FtmKalman.h" + +#include "Settings.h" + +#include "Plotty.h" +#include "Plotta.h" + +#include "misc.h" + +static float kalman_procNoiseDistStdDev = 1.2f; // standard deviation of distance for process noise +static float kalman_procNoiseVelStdDev = 0.1f; // standard deviation of velocity for process noise + +static void poorMansKDE(const BBox3& bbox, float sigma, std::array dist, std::array apPos, std::vector>& density, std::pair& maxElem) +{ + density.clear(); + + const float stepsize = 0.2; + + const float minX = bbox.getMin().x - 5; + const float minY = bbox.getMin().y - 5; + + const float maxX = bbox.getMax().x + 5; + const float maxY = bbox.getMax().y + 5; + + for (float y = minY; y < maxY; y += stepsize) + { + for (float x = minX; x < maxX; x += stepsize) + { + const Point2 pos(x, y); + + float P = 1.0f; + + for (size_t i = 0; i < 4; i++) + { + // TODO: Was mit nan machen? + if (!isnan(dist[i])) + { + float d = pos.getDistance(apPos[i]) - dist[i]; + float p = Distribution::Normal::getProbability(0, sigma, d); + P *= p; + } + } + + density.push_back({ pos, P }); + } + } + + auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair a, std::pair b) { + return a.second < b.second; + }); + + maxElem = *maxElement; +} + +static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::string folder) +{ + // reading file + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); + Offline::FileReader fr(setup.training[walkIdx]); + + // ground truth + std::vector gtPath = setup.gtPath; + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); + CombinedStats 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; + + // 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.setView(30, 0); + plot.plot(); + + // wifi + std::array ftmKalmanFilters{ + Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) + }; + + std::array apPositions{ + Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(), + Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(), + Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(), + Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(), + }; + + std::vector data = filterOfflineData(fr); + + const float sigma = 3.5; + const int movAvgWnd = 15; + std::array, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; + std::array, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; + + std::vector errorValuesFtm, errorValuesRssi; + std::vector timestamps; + + for (const WifiMeas& wifi : data) + { + Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py"); + + Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + float distErrorFtm = 0; + float distErrorRssi = 0; + + //if (wifi.numSucessMeas() == 4) + { + // FTM + { + std::array dists = wifi.ftmDists; + + if (Settings::UseKalman) + { + for (size_t i = 0; i < 4; i++) + { + if (!isnan(dists[i])) + { + dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); + } + } + } + + BBox3 bbox = FloorplanHelper::getBBox(map); + std::vector> density; + std::pair maxElement; + + poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + + Point2 estPos = maxElement.first; + + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); + + // Error + distErrorFtm = gtPos.getDistance(estPos); + errorStats.ftm.add(distErrorFtm); + + + //std::vector densityX, densityY, densityZ; + //for (const auto& item : density) + //{ + // densityX.push_back(item.first.x); + // densityY.push_back(item.first.y); + // densityZ.push_back(item.second); + //} + + //test.add("densityX", densityX); + //test.add("densityY", densityY); + //test.add("densityZ", densityZ); + } + + // RSSI + { + std::array dists = wifi.rssiDists; + + if (Settings::UseKalman) + { + for (size_t i = 0; i < 4; i++) + { + if (!isnan(dists[i])) + { + dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); + } + } + } + + + BBox3 bbox = FloorplanHelper::getBBox(map); + std::vector> density; + std::pair maxElement; + + poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + + Point2 estPos = maxElement.first; + + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); + + // Error + distErrorRssi = gtPos.getDistance(estPos); + errorStats.rssi.add(distErrorRssi); + + + //std::vector densityX, densityY, densityZ; + //for (const auto& item : density) + //{ + // densityX.push_back(item.first.x); + // densityY.push_back(item.first.y); + // densityZ.push_back(item.second); + //} + + //test.add("densityX", densityX); + //test.add("densityY", densityY); + //test.add("densityZ", densityZ); + } + + //std::cout << wifi.ts.ms() << " " << distError << "\n"; + + errorValuesFtm.push_back(distErrorFtm); + errorValuesRssi.push_back(distErrorRssi); + timestamps.push_back(wifi.ts.ms()); + + test.add("t", timestamps); + test.add("errorFtm", errorValuesFtm); + test.add("errorRssi", errorValuesRssi); + test.frame(); + } + + plot.plot(); + //Sleep(250); + printf(""); + } + + std::cout << "Walk error:" << "\n"; + std::cout << "[m] " << " mean \t stdDev median" << "\n"; + + std::cout << "FTM " << errorStats.ftm.getAvg() << "\t" << errorStats.ftm.getStdDev() << "\t" << errorStats.ftm.getMedian() << "\n"; + std::cout << "RSSI " << errorStats.rssi.getAvg() << "\t" << errorStats.rssi.getStdDev() << "\t" << errorStats.rssi.getMedian() << "\n"; + std::cout << std::endl; + + return errorStats; +} + + + +int mainProp(int argc, char** argv) +{ + // global stats + CombinedStats statsAVG; + CombinedStats statsMedian; + CombinedStats statsSTD; + CombinedStats statsQuantil; + CombinedStats tmp; + + std::string evaluationName = "prologic/tmp"; + + for (size_t walkIdx = 0; walkIdx < 6; walkIdx++) + { + std::cout << "Executing walk " << walkIdx << "\n"; + for (int i = 0; i < 1; ++i) + { + std::cout << "Start of iteration " << i << "\n"; + + tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName); + + statsAVG.ftm.add(tmp.ftm.getAvg()); + statsMedian.ftm.add(tmp.ftm.getMedian()); + statsSTD.ftm.add(tmp.ftm.getStdDev()); + statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); + + statsAVG.rssi.add(tmp.rssi.getAvg()); + statsMedian.rssi.add(tmp.rssi.getMedian()); + statsSTD.rssi.add(tmp.rssi.getStdDev()); + statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75)); + + std::cout << "Iteration " << i << " completed" << std::endl; + } + } + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data FTM: " << std::endl; + std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data RSSI: " << std::endl; + std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + + return 0; +} \ No newline at end of file diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp index fc34650..103d2e4 100644 --- a/code/mainTrilat.cpp +++ b/code/mainTrilat.cpp @@ -38,8 +38,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Offline::FileReader fr(setup.training[walkIdx]); // ground truth - std::vector gtPath = setup.gtPath; - Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); + Interpolator gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath); CombinedStats errorStats; //calculate distance of path @@ -61,11 +60,11 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Plotty plot(map); plot.buildFloorplan(); - plot.setGroundTruth(gtPath); + plot.setGroundTruth(setup.gtPath); plot.setView(30, 0); plot.plot(); - + Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py"); std::vector apPositions{ Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(), @@ -74,6 +73,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(), }; + plotta.add("apPos", apPositions); + std::vector data = filterOfflineData(fr); const bool UseFTM = false; @@ -84,10 +85,13 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; + std::vector gtPath, estPathFtm, estPathRssi; + for (const WifiMeas& wifi : data) { Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + gtPath.push_back(gtPos); float distErrorFtm = 0; float distErrorRssi = 0; @@ -132,6 +136,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str // Error distErrorFtm = gtPos.getDistance(estPos); errorStats.ftm.add(distErrorFtm); + estPathFtm.push_back(estPos); } @@ -166,6 +171,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str // Error distErrorRssi = gtPos.getDistance(estPos); errorStats.rssi.add(distErrorRssi); + estPathRssi.push_back(estPos); } //std::cout << wifi.ts.ms() << " " << distError << "\n"; @@ -174,11 +180,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorValuesRssi.push_back(distErrorRssi); timestamps.push_back(wifi.ts.ms()); - Plotta::Plotta test("test", "C:\\Temp\Plotta\\data.py"); - test.add("t", timestamps); - test.add("errorFtm", errorValuesFtm); - test.add("errorRssi", errorValuesRssi); - test.frame(); + plotta.add("t", timestamps); + plotta.add("errorFtm", errorValuesFtm); + plotta.add("errorRssi", errorValuesRssi); + plotta.frame(); } plot.plot(); @@ -186,6 +191,11 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str printf(""); } + plotta.add("gtPath", gtPath); + plotta.add("estPathFtm", estPathFtm); + plotta.add("estPathRssi", estPathRssi); + plotta.frame(); + std::cout << "Walk error:" << "\n"; std::cout << "[m] " << " mean \t stdDev median" << "\n"; diff --git a/code/misc.h b/code/misc.h new file mode 100644 index 0000000..e4309a8 --- /dev/null +++ b/code/misc.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include + +#include + + +#include +#include +#include + +#include "Settings.h" + +template +std::vector asVector(const std::array& src) +{ + std::vector result; + result.assign(src.begin(), src.end()); + return result; +} + +struct WifiMeas { + Timestamp ts; + std::array ftmDists; + std::array rssiDists; + + WifiMeas() + : ts(Timestamp::fromMS(0)), ftmDists{ NAN, NAN, NAN, NAN }, rssiDists{ NAN, NAN, NAN, NAN } + { } + + int numSucessMeas() const { + int count = 0; + + if (!isnan(ftmDists[0])) count++; + if (!isnan(ftmDists[1])) count++; + if (!isnan(ftmDists[2])) count++; + if (!isnan(ftmDists[3])) count++; + + return count; + } + +}; + +template +struct CombinedStats { + Stats::Statistics ftm; + Stats::Statistics rssi; +}; + + + +static std::vector filterOfflineData(const Offline::FileReader& fr) +{ + std::vector result; + WifiMeas currentItem; + + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) + { + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } + + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); + + // init last ts + if (currentItem.ts.isZero()) + { + currentItem.ts = ts; + } + + // new time step? + if ((ts - currentItem.ts) > Timestamp::fromMS(10)) + { + result.push_back(currentItem); + currentItem = {}; + } + + currentItem.ts = ts; + + // DISTANCE + auto ftm = fr.getWifiFtm()[e.idx].data; + + const MACAddress& mac = ftm.getAP().getMAC(); + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, ftm.getRSSI()); + + int nucIndex = Settings::nucIndex(mac); + currentItem.ftmDists[nucIndex] = ftmDist; + currentItem.rssiDists[nucIndex] = rssiDist; + } + + return result; +} + + +struct CmdArguments +{ + std::string executableFilename; + std::vector arguments; + + CmdArguments() + { + } + + CmdArguments(int argc, char** argv) + { + if (argc > 0) + { + executableFilename = std::string(argv[0]); + for (size_t i = 1; i < argc; i++) + { + arguments.push_back(std::string(argv[i])); + } + } + } + + bool hasFlag(const std::string& arg) const + { + return std::find(arguments.begin(), arguments.end(), arg) != arguments.end(); + } +}; \ No newline at end of file