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

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