Added plotta, added probabilistic code

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})
 

code/Plotta.h

@@ -0,0 +1,149 @@
+#pragma once;
+
+
+#include <string>
+#include <filesystem>
+#include <vector>
+#include <unordered_map>
+#include <iostream>
+
+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<std::string, DataItem> 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<typename T>
+        void add(std::string name, const std::vector<T>& value)
+        {
+            plottastream stream;
+
+            stream << name << " = ";
+            stream << value;
+
+            DataItem item;
+            item.name = name;
+            item.dataStr = stream.str();
+
+            data.insert_or_assign(item.name, item);
+        }
+
+        template<typename T>
+        void add(std::string name, const std::initializer_list<T>& 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<typename Iterator>
+    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<typename T>
+    plottastream& operator<<(plottastream& stream, const std::vector<T>& list)
+    {
+        return writeNumpyArray(stream, list.begin(), list.end());
+    }
+
+    template<typename T>
+    plottastream& operator<<(plottastream& stream, const std::initializer_list<T>& list)
+    {
+        return writeNumpyArray(stream, list.begin(), list.end());
+    }
+
+
+    template<typename T>
+    static plottastream& operator<<(plottastream& stream, const T& value)
+    {
+        // use std::strngstream conversion methods for any T
+        static_cast<std::stringstream&>(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<float>{ value.x, value.y };
+    }
+}

code/Settings.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <cassert>
+
 #include <Indoor/grid/GridPoint.h>
 #include <Indoor/data/Timestamp.h>
 #include <Indoor/sensors/radio/VAPGrouper.h>
@@ -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<std::string> training;
         std::unordered_map<MACAddress, NUCSettings> NUCs;
         std::vector<int> 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
+                { 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, 0, 3.0f} }, // NUC 2
+                { 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, 0, 3.0f} }, // NUC 3
+                { 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, 0, 3.0f} }, // NUC 4
+                { 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 }
         };

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"
+    }
+  ]
+}

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 <sys/stat.h>
 
@@ -250,6 +251,11 @@ static Stats::Statistics<float> 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<float> 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<float> 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<float> statsAVG;
     Stats::Statistics<float> statsMedian;
     Stats::Statistics<float> statsSTD;

code/main.h

@@ -1,2 +1,5 @@
 #pragma once
 
+int mainTrilat(int argc, char** argv);
+
+int mainProp(int argc, char** argv);

code/mainProb.cpp

@@ -0,0 +1,316 @@
+#include "main.h"
+
+#include <array>
+#include <memory>
+#include <thread>
+#include <filesystem>
+#include <chrono>
+#include <iostream>
+
+#include <Indoor/math/stats/Statistics.h>
+
+#include <Indoor/floorplan/v2/FloorplanReader.h>
+#include <Indoor/sensors/offline/FileReader.h>
+#include <Indoor/sensors/offline/Sensors.h>
+#include <Indoor/sensors/radio/model/LogDistanceModel.h>
+#include <Indoor/geo/Heading.h>
+#include <Indoor/geo/Point2.h>
+#include <Indoor/data/Timestamp.h>
+#include <Indoor/math/MovingAVG.h>
+
+#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<float, 4> dist, std::array<Point2, 4> apPos, std::vector<std::pair<Point2, float>>& density, std::pair<Point2, float>& 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<float>::getProbability(0, sigma, d);
+                    P *= p;
+                }
+            }
+
+            density.push_back({ pos, P });
+        }
+    }
+
+    auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair<Point2, float> a, std::pair<Point2, float> b) {
+        return a.second < b.second;
+    });
+
+    maxElem = *maxElement;
+}
+
+static CombinedStats<float> 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<int> gtPath = setup.gtPath;
+    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
+    CombinedStats<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;
+
+    // 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<Kalman, 4> 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<Point2, 4> 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<WifiMeas> data = filterOfflineData(fr);
+
+    const float sigma = 3.5;
+    const int movAvgWnd = 15;
+    std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
+    std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
+
+    std::vector<float> errorValuesFtm, errorValuesRssi;
+    std::vector<int> timestamps;
+
+    for (const WifiMeas& wifi : data)
+    {
+        Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py");
+
+        Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(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<float, 4> 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<std::pair<Point2, float>> density;
+                std::pair<Point2, float> 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<float> 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<float, 4> 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<std::pair<Point2, float>> density;
+                std::pair<Point2, float> 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<float> 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<float> statsAVG;
+    CombinedStats<float> statsMedian;
+    CombinedStats<float> statsSTD;
+    CombinedStats<float> statsQuantil;
+    CombinedStats<float> 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;
+}

code/mainTrilat.cpp

@@ -38,8 +38,7 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     Offline::FileReader fr(setup.training[walkIdx]);
 
     // ground truth
-    std::vector<int> gtPath = setup.gtPath;
+    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath);
-    Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
     CombinedStats<float> errorStats;
 
     //calculate distance of path
@@ -61,11 +60,11 @@ static CombinedStats<float> 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<Point2> apPositions{
         Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(),
@@ -74,6 +73,8 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
         Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(),
     };
 
+    plotta.add("apPos", apPositions);
+
     std::vector<WifiMeas> data = filterOfflineData(fr);
 
     const bool UseFTM = false;
@@ -84,10 +85,13 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     std::vector<float> errorValuesFtm, errorValuesRssi;
     std::vector<int> timestamps;
 
+    std::vector<Point2> gtPath, estPathFtm, estPathRssi;
+
     for (const WifiMeas& wifi : data)
     {
         Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(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<float> 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<float> 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<float> 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");
+            plotta.add("t", timestamps);
-            test.add("t", timestamps);
+            plotta.add("errorFtm", errorValuesFtm);
-            test.add("errorFtm", errorValuesFtm);
+            plotta.add("errorRssi", errorValuesRssi);
-            test.add("errorRssi", errorValuesRssi);
+            plotta.frame();
-            test.frame();
         }
 
         plot.plot();
@@ -186,6 +191,11 @@ static CombinedStats<float> 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";
     

code/misc.h

@@ -0,0 +1,127 @@
+#pragma once
+
+#include <array>
+#include <vector>
+
+#include <Indoor/math/stats/Statistics.h>
+
+
+#include <Indoor/data/Timestamp.h>
+#include <Indoor/sensors/offline/FileReader.h>
+#include <Indoor/sensors/radio/model/LogDistanceModel.h>
+
+#include "Settings.h"
+
+template<typename T, int Size>
+std::vector<T> asVector(const std::array<T, Size>& src)
+{
+    std::vector<T> result;
+    result.assign(src.begin(), src.end());
+    return result;
+}
+
+struct WifiMeas {
+    Timestamp ts;
+    std::array<float, 4> ftmDists;
+    std::array<float, 4> 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<typename T>
+struct CombinedStats {
+    Stats::Statistics<T> ftm;
+    Stats::Statistics<T> rssi;
+};
+
+
+
+static std::vector<WifiMeas> filterOfflineData(const Offline::FileReader& fr)
+{
+    std::vector<WifiMeas> 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<std::string> 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();
+    }
+};