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

added tex template

Browse Source
This commit is contained in:
toni
2016-03-29 15:32:09 +02:00
parent 21e26e6209
commit f52947fae9
30 changed files with 13110 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
code/eval/FixedLagEvalBase.h
View File

@@ -19,6 +19,7 @@
#include "../particles/MyInitializer.h"
#include "../particles/smoothing/MySmoothingTransition.h"
#include "../particles/smoothing/MySmoothingTransitionSimple.h"
#include "../particles/smoothing/MySmoothingTransitionExperimental.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
@@ -68,6 +69,8 @@ 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:
FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
@@ -84,7 +87,7 @@ public:
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
vis.gp << "unset cbrange\n";
}
@@ -236,7 +239,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
@@ -273,12 +276,14 @@ public:
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
}
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
if(cnt > 19){
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
@@ -291,7 +296,7 @@ public:
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
for(int i = 0; i < MiscSettings::lag; i += MiscSettings::fixedLagGap){
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);
}
@@ -309,18 +314,20 @@ public:
statsSmoothing.add(errSmoothed);
std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl;
// plot
//plot
vis.clearStates();
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
vis.setEstAndShould(curEst, curGT);
vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
@@ -343,6 +350,42 @@ public:
}
statsout.close();
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
//vis.setEstAndShould(curEst, curGT);
//vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//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*33);
}
};