toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added astar to smoothing transition

Browse Source
This commit is contained in:
toni
2016-04-11 10:57:29 +02:00
parent 5aad08e17c
commit de8783b9ca
13 changed files with 146 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
code/eval/SmoothingEvalBase.h
View File

@@ -184,7 +184,8 @@ public:
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<double> stats;
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
int cnt = 0;
//stats file
@@ -271,14 +272,14 @@ public:
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
pathEst.push_back(curEst);
const float err = diff.length();
stats.add(err);
std::cout << stats.asString() << std::endl;
pathEst.push_back(curEst);
const float err = diff.length();
statsFiltering.add(err);
std::cout << statsFiltering.asString() << std::endl;
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
// plot
@@ -321,7 +322,7 @@ public:
//File
//std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
stats.reset();
//stats.reset();
bf->reset();
MyState estBF;
@@ -345,8 +346,8 @@ public:
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
stats.add(errSmoothed);
std::cout << stats.asString() << std::endl;
statsSmoothing.add(errSmoothed);
std::cout << statsSmoothing.asString() << std::endl;
// plot
@@ -382,6 +383,44 @@ public:
//statsout2.close();
// append error for each run to a file
std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app);
oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n";
oTError.close();
// append error for each run to a file
std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app);
oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n";
oSError.close();
// plot-data
std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close();
std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close();
std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close();
std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
oPlot << "set terminal eps size 3.4,2\n";
oPlot << "set output '" << runName << ".eps'\n";
oPlot << "set termoption dashlength 0.5\n";
oPlot << "set ticslevel 0\n";
oPlot << "set view equal xy\n";
oPlot << "set zrange [0:2200]\n";
oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n";
oPlot << "set view 72,33\n";
oPlot << "unset border\n";
oPlot << "unset xtics\n";
oPlot << "unset ytics\n";
oPlot << "unset ztics\n";
oPlot << "splot \\\n";
oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n";
oPlot << "'path_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
oPlot.close();
}
};