From 23f516243e79e2d963f35bbef0515212d2988adb Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 26 Nov 2019 14:28:01 +0100 Subject: [PATCH] PLot changes --- code/Settings.h | 13 +++--- code/main.cpp | 105 ++++++++++++++++++++++++++++++------------------ code/misc.h | 37 +++++++++++++++++ 3 files changed, 111 insertions(+), 44 deletions(-) diff --git a/code/Settings.h b/code/Settings.h index 8a4934a..22bed9e 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -12,6 +12,9 @@ namespace Settings { const std::string plotDataDir = "../plots/data/"; const std::string outputDir = "../output/"; + const bool PlotCircles = false; + const bool PlotToPng = false; + const bool UseKalman = false; const bool UseRSSI = false; @@ -412,14 +415,14 @@ namespace Settings { dataDir + "Pixel2/path21/17527721931557.csv", dataDir + "Pixel2/path21/18001426901565.csv", dataDir + "Pixel2/path21/18177010770775.csv", - - dataDir + "Pixel3/path21/15786943753685.csv", + + // dataDir + "Pixel3/path21/15786943753685.csv", // bug dataDir + "Pixel3/path21/16075928276171.csv", dataDir + "Pixel3/path21/16268193326887.csv", - dataDir + "Pixel3/path21/16424251083631.csv", + // dataDir + "Pixel3/path21/16424251083631.csv", // bug }, { - // NUC, ID Pos X Y Z offset loss kalman stddev + // NUC, ID Pos X Y Z offset loss kalman stddev { NUC1, {1, { 67 , 33 , 0.8}, -2.00, 3.000, 3.0f} }, // NUC 1 { NUC2, {2, { 27 , 49.6, 0.8}, -2.00, 3.000, 3.0f} }, // NUC 2 { NUC3, {3, { 15 , 32.4, 0.8}, -2.00, 3.000, 3.0f} }, // NUC 3 @@ -466,6 +469,6 @@ namespace Settings { }; } data; - static DataSetup CurrentPath = data.Path22; + static DataSetup CurrentPath = data.Path21; } diff --git a/code/main.cpp b/code/main.cpp index 9679384..c80ac87 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -399,7 +399,6 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str { // Do update step Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); // TODO //gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()), @@ -504,44 +503,52 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str estPos = est.pos.pos; ctrl.lastEstimate = estPos; - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); - plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); - // Error - distErrorFtm = gtPos.getDistance(estPos.xy()); - errorStats.ftm.add(distErrorFtm); - - // draw wifi ranges - plot.clearDistanceCircles(); - - for (size_t i = 0; i < obs.ftm.size(); i++) + if (Settings::UseRSSI) { - WiFiMeasurement wifi2 = obs.ftm[i]; - - Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; - - K::GnuplotColor color; - switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID) - { - case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; - case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; - case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break; - case 6: color = K::GnuplotColor::fromRGB(0, 255, 255); break; - default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; - } - - float plotDist = wifi2.getFtmDist(); - if (Settings::UseRSSI) - { - float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss; - float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, wifi2.getRSSI()); - plotDist = rssiDist; - } - - plot.addDistanceCircle(apPos.xy(), plotDist, color); + distErrorRssi = gtPos.getDistance(estPos.xy()); + errorStats.rssi.add(distErrorRssi); + } + else + { + distErrorFtm = gtPos.getDistance(estPos.xy()); + errorStats.ftm.add(distErrorFtm); } + // draw wifi ranges + if (Settings::PlotCircles) + { + plot.clearDistanceCircles(); + + for (size_t i = 0; i < obs.ftm.size(); i++) + { + WiFiMeasurement wifi2 = obs.ftm[i]; + + Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; + + K::GnuplotColor color; + switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID) + { + case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; + case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; + case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break; + case 6: color = K::GnuplotColor::fromRGB(0, 255, 255); break; + default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; + } + + float plotDist = wifi2.getFtmDist() + Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ftm_offset; + if (Settings::UseRSSI) + { + float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, wifi2.getRSSI()); + plotDist = rssiDist; + } + + plot.addDistanceCircle(apPos.xy(), plotDist, color); + } + } + obs.wifi.clear(); obs.ftm.clear(); @@ -562,7 +569,25 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //distsPlot.add("ftmDists", ftmDistances); //distsPlot.frame(); + // Png Output + if (Settings::PlotToPng) + { + plot.gp.setTerminal("png", K::GnuplotSize(1280, 720)); + auto pngPath = outputDir / "png" / "frame.png"; + + // clear folder + //std::filesystem::remove_all(pngPath); + forceDirectories(pngPath.parent_path()); + //std::filesystem::create_directory(pngPath); + + plot.gp.setOutput(appendFileSuffixToPath(pngPath, ts.ms()).string()); + } + + // Plotting + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); plot.showParticles(pf.getParticles()); plot.setCurEst(estPos); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); @@ -574,15 +599,13 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //plot.splot.getView().setCamera(0, 0); //plot.splot.getView().setEqualXY(true); + + plot.plot(); //std::this_thread::sleep_for(100ms); } } - - //std::cin.get(); - - printErrorStats(errorStats); std::ofstream plot_out; @@ -597,6 +620,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorPlot.frame(); + + std::cout << "Press any key to continue ..." << std::endl; + std::cin.get(); + // MATLAB output //{ // std::ofstream matlab_error_out; @@ -710,7 +737,7 @@ int main(int argc, char** argv) //Settings::data.Path10, //Settings::data.Path11 //Settings::data.Path20, - //Settings::data.Path21, + Settings::data.Path21, Settings::data.Path22, }; diff --git a/code/misc.h b/code/misc.h index 7bd1513..d5648bb 100644 --- a/code/misc.h +++ b/code/misc.h @@ -12,6 +12,43 @@ #include "Settings.h" +static std::string timeForFilename(const std::time_t& time) +{ + std::stringstream ss; + ss << std::put_time(std::localtime(&time), "%F_%T"); // ISO 8601 without timezone information. + auto s = ss.str(); + std::replace(s.begin(), s.end(), ':', '-'); + return s; +} + +static std::string currentTimeForFilename() +{ + auto time = std::time(nullptr); + return timeForFilename(time); +} + +static std::filesystem::path appendCurrentTimeToFilename(const std::filesystem::path& fileName) +{ + return fileName.stem().string() + "_" + currentTimeForFilename() + fileName.extension().string(); +} + +template +inline std::filesystem::path appendFileSuffixToPath(const std::filesystem::path& path, T value) +{ + std::string filename = path.stem().string() + "_" + std::to_string(value) + path.extension().string(); + return path.parent_path() / filename; +} + + +static bool forceDirectories(const std::filesystem::path& path) +{ + if (!std::filesystem::exists(path)) { + return std::filesystem::create_directories(path); + } + return true; +} + + template std::vector asVector(const std::array& src) {