Added trilateration code

code/Plotty.h

@@ -98,6 +98,7 @@ public:
 
 	K::GnuplotSplotElementLines pathReal;
 	K::GnuplotSplotElementLines pathEst;
+    K::GnuplotSplotElementLines pathEst2;
     K::GnuplotSplotElementColorPoints particles;
 
 	K::GnuplotSplotElementLines mapOutlineGlass;
@@ -155,6 +156,7 @@ public:
 
 		splot.add(&pathReal); pathReal.getStroke().setWidth(2); pathReal.getStroke().getColor().setHexStr("#000000");
 		splot.add(&pathEst); pathEst.getStroke().setWidth(2); pathEst.getStroke().getColor().setHexStr("#0000ff");
+        splot.add(&pathEst2); pathEst2.getStroke().setWidth(2); pathEst2.getStroke().getColor().setHexStr("#ff0000");
 
 		splot.add(&pm3doutline);
 
@@ -478,6 +480,11 @@ public:
         pathEst.add(est);
     }
 
+    void addEstimationNode2(const Point3 pos) {
+        K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
+        pathEst2.add(est);
+    }
+
 
 	void setTitle(const std::string& title) {
 		gp << "set title '" << title << "'\n";

code/mainTrilat.cpp

@@ -0,0 +1,251 @@
+
+#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 "Settings.h"
+
+#include "Plotty.h"
+#include "Plotta.h"
+
+
+#include "trilateration.h"
+#include "misc.h"
+
+
+
+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();
+
+    
+
+    std::vector<Point2> 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 bool UseFTM = false;
+    const int movAvgWnd = 10;
+    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)
+    {
+        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::vector<float> avgDists;
+
+                for (size_t i = 0; i < 4; i++)
+                {
+                    float dist = wifi.ftmDists[i];
+
+                    if (!isnan(dist))
+                    {
+                        movAvgsFtm[i].add(dist);
+                    }
+
+
+                    if (movAvgsFtm[i].getNumUsed() == 0)
+                    {
+                        avgDists.push_back(0);
+                    }
+                    else
+                    {
+                        avgDists.push_back(movAvgsFtm[i].get());
+                    }                   
+                }
+
+                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
+
+                plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
+                plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
+
+                // draw wifi ranges           
+                for (size_t i = 0; i < 4; i++)
+                {
+                    plot.addCircle(i + 1, apPositions[i], avgDists[i]);
+                }
+
+                // Error
+                distErrorFtm = gtPos.getDistance(estPos);
+                errorStats.ftm.add(distErrorFtm);
+            }
+
+
+            // RSSI
+            {
+                std::vector<float> avgDists;
+
+                for (size_t i = 0; i < 4; i++)
+                {
+                    float dist = wifi.rssiDists[i];
+
+                    if (!isnan(dist))
+                    {
+                        movAvgsRssi[i].add(dist);
+                    }
+
+
+                    if (movAvgsRssi[i].getNumUsed() == 0)
+                    {
+                        avgDists.push_back(0);
+                    }
+                    else
+                    {
+                        avgDists.push_back(movAvgsRssi[i].get());
+                    }
+                }
+
+                Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
+
+                plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
+
+                // Error
+                distErrorRssi = gtPos.getDistance(estPos);
+                errorStats.rssi.add(distErrorRssi);
+            }
+            
+            //std::cout << wifi.ts.ms() << " " << distError << "\n";
+            
+            errorValuesFtm.push_back(distErrorFtm);
+            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();
+        }
+
+        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 mainTrilat(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/trilateration.h

@@ -0,0 +1,95 @@
+#pragma once
+
+#include <cmath>
+#include <vector>
+
+#include <eigen3/Eigen/Eigen>
+
+#include "..\Indoor\geo\Point2.h"
+#include "..\Indoor\geo\Point3.h"
+
+namespace Trilateration
+{
+    // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp
+
+    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
+    {
+        // To locate position on a 2d plan, have to get at least 3 becaons,
+        // otherwise return false.
+        if (positions.size() < 3)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 2);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, r1, r2;
+        
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y;
+                x2 = positions[j].x, y2 = positions[j].y;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                    (pow(y1, 2) - pow(y2, 2)) -
+                    (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+        return Point2(location.x(), location.y());
+    }
+
+    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
+    {
+        // To locate position in a 3D space, have to get at least 4 becaons
+        if (positions.size() < 4)
+            assert(false);
+        if (positions.size() != distances.size())
+            assert(false);
+
+        // Define the matrix that we are going to use
+        size_t count = positions.size();
+        size_t rows = count * (count - 1) / 2;
+        Eigen::MatrixXd m(rows, 3);
+        Eigen::VectorXd b(rows);
+
+        // Fill in matrix according to the equations
+        size_t row = 0;
+        double x1, x2, y1, y2, z1, z2, r1, r2;
+
+        for (size_t i = 0; i < count; ++i) {
+            for (size_t j = i + 1; j < count; ++j) {
+                x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
+                x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
+                r1 = distances[i];
+                r2 = distances[j];
+                m(row, 0) = x1 - x2;
+                m(row, 1) = y1 - y2;
+                m(row, 2) = z1 - z2;
+                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
+                          (pow(y1, 2) - pow(y2, 2)) +
+                          (pow(z1, 2) - pow(z2, 2)) -
+                          (pow(r1, 2) - pow(r2, 2))) / 2;
+                row++;
+            }
+        }
+
+        // Then calculate to solve the equations, using the least square solution
+        Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+        return Point3(location.x(), location.y(), location.z());
+    }
+}