FHWS/museumLoc
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

code for journal presentation

Browse Source
This commit is contained in:
toni
2018-05-15 09:32:39 +02:00
parent 97ce18f5f1
commit a536b1daec
7 changed files with 165 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
main.cpp
View File

@@ -93,8 +93,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.7);
//const Point3 srcPath2(62, 38, 1.8);
//const Point3 srcPath3(62, 38, 1.8);
const Point3 srcPath2(62, 38, 1.8);
const Point3 srcPath3(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
@@ -115,17 +115,18 @@ 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 trans = std::make_unique<MyPFTrans>(mesh, WiFiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
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>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
@@ -154,6 +155,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
@@ -184,6 +186,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (ctrl.numStepsSinceLastEval > 0) {
obs.currentTime = ts;
ctrl.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// }
@@ -192,7 +196,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
lastTimestamp = ts;
ctrl.lastEstimate = est.pos.pos;
//plot
//dbg.showParticles(pf.getParticles());
@@ -207,22 +211,16 @@ 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.setActivity((int) act.get());
//plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
errorStats.add(err_m);
errorFile << err_m << "\n";
errorFile << ts.ms() << " " << err_m << "\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();
}
}
}
@@ -241,14 +239,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
errorFile.close();
//save the .gp buffer into a file
// std::ofstream plotFile;
// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
// dbg.saveToFile(plotFile);
// plotFile.close();
/* plot in gp file */
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
//save also a png image, just for a better overview
// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot();
return errorStats;
}
@@ -261,13 +260,38 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
Settings::DataSetup set = Settings::data.Path1;
std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
std::string evaluationName = "museum/Path3_Museum_KLD_SimpleDistance";
for(int i = 0; i < 1; ++i){
for(int i = 0; i < 100; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(set, j, evaluationName);
//TODO: in transition die distance über KLD noch einkommentieren als Test
// for(int j = 0; j < Settings::data.Path0.training.size(); ++j){
// tmp = run(Settings::data.Path0, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path1.training.size(); ++j){
// tmp = run(Settings::data.Path1, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path2.training.size(); ++j){
// tmp = run(Settings::data.Path2, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
@@ -299,6 +323,6 @@ int main(int argc, char** argv) {
finalStatisticFile.close();
std::this_thread::sleep_for(std::chrono::seconds(60));
//std::this_thread::sleep_for(std::chrono::seconds(60));
}