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

added fixed interval smoothing

Browse Source
This commit is contained in:
toni
2016-03-17 19:24:45 +01:00
parent 8d2be0f8a0
commit 89bb0b8b7a
17 changed files with 1010 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
code/eval/EvalBase.h
View File

@@ -29,6 +29,9 @@
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
#include <Indoor/misc/Time.h>
class EvalBase {
protected:
@@ -64,9 +67,11 @@ protected:
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
float stepSize = 0.71;
public:
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
// build the grid
Helper::buildTheGrid(grid, floors);
@@ -173,9 +178,11 @@ public:
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<float> statsTime;
K::Statistics<double> stats;
int cnt = 0;
std::vector<float> errors;
// process each single sensor reading
while(sr->hasNext()) {
@@ -225,7 +232,7 @@ public:
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
ctrl.walked_m = obs.step->steps * stepSize;
}
// process all occurred steps
@@ -241,6 +248,8 @@ public:
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
auto tick1 = Time::tick();
lastTransitionTS = se.ts;
// timed updates
@@ -252,16 +261,25 @@ public:
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 750;
const int offset = 0;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
auto tick2 = Time::tick();
float diffTime = Time::diffMS(tick1, tick2) * 1.25f;
statsTime.add(diffTime);
std::cout << "#" << statsTime.getAvg() << "\t" << diffTime << std::endl;
// skip the first 10 scans due to uniform distribution start
if (++cnt > 10) {
pathEst.push_back(curEst);
const float err = diff.length();
stats.add(err);
std::cout << stats.asString() << std::endl;
errors.push_back(err);
// skip the first 24 scans due to uniform distribution start (12 seconds)
if (++cnt > 24) {
stats.add(err);
std::cout << stats.asString() << std::endl;
}
// plot
@@ -287,17 +305,55 @@ public:
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*333);
static int dspCnt = 0;
if (++dspCnt > 0) {
vis.show();
usleep(1000*33); // prevent gnuplot errors
dspCnt = 0;
}
}
}
sleep(1000);
// append error for each run to a file
std::ofstream oTError("/tmp/errors.txt", std::ios_base::app);
oTError << runName << "\n\t"; stats.appendTo(oTError); oTError << "\n\n";
oTError.close();
// detailled error-description
std::ofstream oError("/tmp/err_" + runName + ".dat");
for (float f : errors) {oError << f << "\n";}
oError.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_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
oPlot.close();
}