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

work in progress.

Browse Source
This commit is contained in:
toni
2016-04-05 18:06:23 +02:00
parent 5f81da109a
commit aeaa4cb59d
7 changed files with 107 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
code/eval/SmoothingEvalBase.h
View File

@@ -34,6 +34,7 @@
#include "../frank/OrientationSensorReader.h"
static int smoothing_time_delay = 0;
float stepSize = 0.71;
class SmoothingEvalBase {
@@ -237,7 +238,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
@@ -268,8 +269,8 @@ public:
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 10 scans due to uniform distribution start
if (++cnt > 10) {
// 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);
@@ -319,20 +320,28 @@ public:
// ========================= //
//File
std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
//std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
stats.reset();
bf->reset();
for(int i = pfHistory.size() - 1; i > 0; i -= smoothing_time_delay){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
MyState estBF;
int currentParticleSetToSmoothAtTimet = 0;
while(currentParticleSetToSmoothAtTimet != pfHistory.size() - 1){
//iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT)
for(int i = pfHistory.size() - 1; i >= currentParticleSetToSmoothAtTimet; i -= smoothing_time_delay){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[i]);
}
const MyState estBF = bf->update(pfHistory[i]);
const Point3 curSmoothedEst = estBF.pCur;
smoothedEst.push_back(curSmoothedEst);
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[i]);
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[currentParticleSetToSmoothAtTimet]);
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
@@ -346,7 +355,7 @@ public:
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(tsHistory[i]);
vis.setTimestamp(tsHistory[currentParticleSetToSmoothAtTimet]);
vis.addGroundTruth(gtw);
vis.addEstPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
@@ -367,9 +376,11 @@ public:
// prevent gnuplot errors
usleep(1000*33);
++currentParticleSetToSmoothAtTimet;
}
statsout2.close();
//statsout2.close();
}