diff --git a/code/Plotty.h b/code/Plotty.h index bd53f4a..a301171 100644 --- a/code/Plotty.h +++ b/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"; diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp new file mode 100644 index 0000000..fc34650 --- /dev/null +++ b/code/mainTrilat.cpp @@ -0,0 +1,251 @@ + +#include "main.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "Settings.h" + +#include "Plotty.h" +#include "Plotta.h" + + +#include "trilateration.h" +#include "misc.h" + + + +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(); + + + + std::vector 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 bool UseFTM = false; + const int movAvgWnd = 10; + 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) + { + 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::vector 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 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 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/trilateration.h b/code/trilateration.h new file mode 100644 index 0000000..ce408d9 --- /dev/null +++ b/code/trilateration.h @@ -0,0 +1,95 @@ +#pragma once + +#include +#include + +#include + +#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& positions, const std::vector& 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& positions, const std::vector& 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()); + } +} \ No newline at end of file