FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PLot changes

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-26 14:28:01 +01:00
parent b5fb632cc2
commit 23f516243e
3 changed files with 111 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
code/Settings.h
View File

@@ -12,6 +12,9 @@ namespace Settings {
const std::string plotDataDir = "../plots/data/"; const std::string plotDataDir = "../plots/data/";
const std::string outputDir = "../output/"; const std::string outputDir = "../output/";
const bool PlotCircles = false;
const bool PlotToPng = false;
const bool UseKalman = false; const bool UseKalman = false;
const bool UseRSSI = false; const bool UseRSSI = false;
@@ -413,13 +416,13 @@ namespace Settings {
dataDir + "Pixel2/path21/18001426901565.csv", dataDir + "Pixel2/path21/18001426901565.csv",
dataDir + "Pixel2/path21/18177010770775.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/16075928276171.csv",
dataDir + "Pixel3/path21/16268193326887.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 { 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 { 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 { NUC3, {3, { 15 , 32.4, 0.8}, -2.00, 3.000, 3.0f} }, // NUC 3
@@ -466,6 +469,6 @@ namespace Settings {
}; };
} data; } data;
static DataSetup CurrentPath = data.Path22; static DataSetup CurrentPath = data.Path21;
} }

93
code/main.cpp
View File

@@ -399,7 +399,6 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
{ {
// Do update step // Do update step
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy(); Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
// TODO // TODO
//gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()), //gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
@@ -504,41 +503,49 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
estPos = est.pos.pos; estPos = est.pos.pos;
ctrl.lastEstimate = estPos; ctrl.lastEstimate = estPos;
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
// Error // Error
distErrorFtm = gtPos.getDistance(estPos.xy()); if (Settings::UseRSSI)
errorStats.ftm.add(distErrorFtm); {
distErrorRssi = gtPos.getDistance(estPos.xy());
errorStats.rssi.add(distErrorRssi);
}
else
{
distErrorFtm = gtPos.getDistance(estPos.xy());
errorStats.ftm.add(distErrorFtm);
}
// draw wifi ranges // draw wifi ranges
plot.clearDistanceCircles(); if (Settings::PlotCircles)
for (size_t i = 0; i < obs.ftm.size(); i++)
{ {
WiFiMeasurement wifi2 = obs.ftm[i]; plot.clearDistanceCircles();
Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; for (size_t i = 0; i < obs.ftm.size(); i++)
K::GnuplotColor color;
switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID)
{ {
case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; WiFiMeasurement wifi2 = obs.ftm[i];
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(); Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position;
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); 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);
}
} }
@@ -562,7 +569,25 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
//distsPlot.add("ftmDists", ftmDistances); //distsPlot.add("ftmDists", ftmDistances);
//distsPlot.frame(); //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 // 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.showParticles(pf.getParticles());
plot.setCurEst(estPos); plot.setCurEst(estPos);
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
@@ -574,15 +599,13 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
//plot.splot.getView().setCamera(0, 0); //plot.splot.getView().setCamera(0, 0);
//plot.splot.getView().setEqualXY(true); //plot.splot.getView().setEqualXY(true);
plot.plot(); plot.plot();
//std::this_thread::sleep_for(100ms); //std::this_thread::sleep_for(100ms);
} }
} }
//std::cin.get();
printErrorStats(errorStats); printErrorStats(errorStats);
std::ofstream plot_out; std::ofstream plot_out;
@@ -597,6 +620,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
errorPlot.frame(); errorPlot.frame();
std::cout << "Press any key to continue ..." << std::endl;
std::cin.get();
// MATLAB output // MATLAB output
//{ //{
// std::ofstream matlab_error_out; // std::ofstream matlab_error_out;
@@ -710,7 +737,7 @@ int main(int argc, char** argv)
//Settings::data.Path10, //Settings::data.Path10,
//Settings::data.Path11 //Settings::data.Path11
//Settings::data.Path20, //Settings::data.Path20,
//Settings::data.Path21, Settings::data.Path21,
Settings::data.Path22, Settings::data.Path22,
}; };

37
code/misc.h
View File

@@ -12,6 +12,43 @@
#include "Settings.h" #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<typename T>
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<typename T, int Size> template<typename T, int Size>
std::vector<T> asVector(const std::array<T, Size>& src) std::vector<T> asVector(const std::array<T, Size>& src)
{ {