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

Added forgotten code

Browse Source
This commit is contained in:
Markus Bullmann
2019-05-01 10:45:53 +02:00
parent a7be6b8d62
commit b99d519aa7
8 changed files with 467 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
code/main.cpp
View File

@@ -6,8 +6,10 @@
#include "navMesh/meshPlotter.h"
#include "Plotty.h"
#include <chrono>
#include <memory>
#include <thread>
#include <experimental/filesystem>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
@@ -24,7 +26,22 @@
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <sys/stat.h>
#define ESTIMATE_ALGO 3
enum class Type {
UNKNOWN,
GROUND_INDOOR,
GROUND_OUTDOOR,
STAIR,
HANDRAIL,
DOOR,
WALL,
WINDOW,
OBJECT,
ERROR,
};
Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
@@ -42,17 +59,27 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
//error file
const long int t = static_cast<long int>(time(NULL));
const std::string evalDir = Settings::errorDir + folder;
struct stat statStruct;
stat(evalDir.c_str(), &statStruct);
if(!S_ISDIR(statStruct.st_mode)){
if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){
Assert::doThrow("Eval folder couldn't be created!");
}
}
std::ofstream errorFile;
errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
evalDir.append(folder);
if (!std::experimental::filesystem::exists(evalDir)) {
std::experimental::filesystem::create_directory(evalDir);
}
std::ofstream errorFile;
std::ofstream estimationFile;
std::string outputBasePath = evalDir.string() + "/";
#if ESTIMATE_ALGO == 1
outputBasePath += "KDE_";
#elif ESTIMATE_ALGO == 2
outputBasePath += "WAVG_";
#else
outputBasePath += "MAXP_";
#endif
outputBasePath += std::to_string(numFile) + "_" + std::to_string(t);
errorFile.open(outputBasePath + "error.csv");
estimationFile.open(outputBasePath + "est.csv");
// wifi
WiFiModelLogDistCeiling WiFiModel(map);
@@ -115,17 +142,22 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// particle-filter
const int numParticles = 5000;
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
//auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
#if ESTIMATE_ALGO == 1
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1, 1));
#elif ESTIMATE_ALGO == 2
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
#else
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
#endif
// setup
MyFilter pf(numParticles, std::move(init));
@@ -207,22 +239,46 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.plot();
//plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
errorStats.add(err_m);
errorFile << ts.ms() << " " << err_m << "\n";
estimationFile << est.pos.pos.x << " " << est.pos.pos.y << " " << est.pos.pos.z << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
if(ts.ms() == 13410 || ts.ms() == 20802){
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
std::string filePath = evalDir.string() + "/";
#if ESTIMATE_ALGO == 1
filePath += "KDE_";
#elif ESTIMATE_ALGO == 2
filePath += "WAVG_";
#else
filePath += "MAXP_";
#endif
filePath += std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp";
plot.saveSVG(filePath);
//plotFile.open(filePath + ".gp");
// plot.saveToFile(plotFile);
// plotFile.close();
}
//std::this_thread::sleep_for(std::chrono::milliseconds(500));
if (ts.ms() == 20802)
{
break;
}
}
}
@@ -241,6 +297,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
errorFile.close();
estimationFile.close();
//save the .gp buffer into a file
// std::ofstream plotFile;
// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
@@ -248,7 +306,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// plotFile.close();
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_" + std::to_string(obs.currentTime.ms()) + ".gp");
plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_" + std::to_string(obs.currentTime.ms()) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
@@ -266,7 +324,7 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
Settings::DataSetup set = Settings::data.Path0;
Settings::DataSetup set = Settings::data.Path1;
std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
for(int i = 0; i < 1; ++i){