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

13
code/Settings.h
View File

@@ -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;
}

105
code/main.cpp
View File

@@ -399,7 +399,6 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
{
// Do update step
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(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<float> 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<float> 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<float> 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<float> 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,
};

37
code/misc.h
View File

@@ -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<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>
std::vector<T> asVector(const std::array<T, Size>& src)
{