added presentation, related code, and forgotten tex changes

This commit is contained in:
2016-07-01 10:22:29 +02:00
parent 1d64cedd29
commit f63ae7fe85
47 changed files with 4941 additions and 8 deletions

View File

@@ -31,6 +31,8 @@ public:
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {

View File

@@ -38,6 +38,7 @@ protected:
Grid<MyGridNode> grid;
Helper::FHWSFloors floors;
Vis vis;
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
@@ -94,7 +95,7 @@ public:
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
static GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> path;

View File

@@ -18,6 +18,7 @@
#include "eval/PaperVisDijkstra.h"
#include "eval/PaperVisGrid.h"
#include "presentation/presentation.h"
float BarometerEvaluation::barometerSigma = NAN;
@@ -85,9 +86,12 @@ void testModelWalk() {
int main(void) {
Presentation p;
p.xxx();
// testModelWalk();
Eval1 eval;
//Eval1 eval;
// //eval.path2_forward_simple();
// //eval.path2_forward_path();
// //eval.path3_forward_simple();
@@ -113,10 +117,10 @@ int main(void) {
// {Eval1 eval; eval.bergwerk_path3_nexus_multi(); eval.run();}
// {Eval1 eval; eval.bergwerk_path3_nexus_shortest(); eval.run();}
{Eval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();}
//{Eval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();}
//{Eval1 eval; eval.bergwerk_path4_nexus_imp(); eval.run();}
{Eval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();}
{Eval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();}
//{Eval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();}
//{Eval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();}

View File

@@ -0,0 +1,488 @@
#ifndef PRESENTATION_H
#define PRESENTATION_H
#include "../eval/EvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
class Presentation {
struct PlotErr {
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lErr1;
K::GnuplotPlotElementLines lErr2;
PlotErr() {
plot.add(&lErr1);
plot.add(&lErr2);
lErr1.setLineWidth(2); lErr1.setColorHex("#000000");
lErr2.setLineWidth(2); lErr2.setColorHex("#666666");
plot.addCustom("set xrange[0:160]");
plot.addCustom("set yrange[0:12]");
}
void add(float ts, float err1, float err2) {
ts /= 1000;
lErr1.add(K::GnuplotPoint2(ts, err1));
lErr2.add(K::GnuplotPoint2(ts, err2));
}
};
struct PlotEST {
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines walls;
K::GnuplotSplotElementLines path1;
K::GnuplotSplotElementLines path2;
K::GnuplotSplotElementLines groundtruth;
K::GnuplotSplotElementPoints particles;
std::stringstream misc;
PlotEST(Helper::FHWSFloors& floors, GroundTruthWay& gtw) {
plot.addCustom("unset border");
plot.addCustom("unset tics");
plot.addCustom("unset cbrange");
plot.add(&walls);
plot.add(&particles);
plot.add(&groundtruth);
plot.add(&path1);
plot.add(&path2);
walls.setColorHex("#666666");
//groundtruth.setCustomAttr("dashtype 3");
groundtruth.setLineWidth(2);
groundtruth.setColorHex("#009900");
particles.setPointType(7);
particles.setPointSize(0.25f);
particles.setColorHex("#3333ff");
path1.setLineWidth(3); path1.setColorHex("#000000");
path2.setLineWidth(3); path2.setColorHex("#666666");
addFloor(floors.f0, floors.h0.cm());
addFloor(floors.f1, floors.h1.cm());
addFloor(floors.f2, floors.h2.cm());
addFloor(floors.f3, floors.h3.cm());
addGT(gtw);
}
void addFloor(const Floor& f, const float height) {
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height);
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height);
walls.addSegment(p1, p2);
}
}
void addEst1(const std::vector<Point3>& way) {
path1.clear();
for (Point3 p : way) {
K::GnuplotPoint3 gp(p.x, p.y, p.z);
path1.add(gp);
}
}
void addEst2(const std::vector<Point3>& way) {
path2.clear();
for (Point3 p : way) {
K::GnuplotPoint3 gp(p.x, p.y, p.z);
path2.add(gp);
}
}
void addGT(GroundTruthWay& gtw) {
for (auto it : gtw.getWay()) {
K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
groundtruth.add(gp);
}
}
void setCur(Point3 est, Point3 gt) {
misc.str("");
misc << "set arrow 1 from " << est.x << "," << est.y << "," << est.z+150 << " to " << est.x << "," << est.y << "," << est.z << " lw 3\n";
misc << "set arrow 2 from " << gt.x << "," << gt.y << "," << gt.z+150 << " to " << gt.x << "," << gt.y << "," << gt.z << "\n";
}
template <typename T> void addParticles(const T& lst) {
particles.clear();
for (int i = 0; i < (int) lst.size(); i+=15) {
const K::Particle<MyState>& p = lst[i];
const K::GnuplotPoint3 pp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
particles.add(pp);
}
}
};
template <typename T> struct LeShortestPath {
Grid<T>& grid;
DijkstraMapper acc;
Dijkstra<T> dijkstra;
Point3 dst;
LeShortestPath(Grid<T>& grid, Point3 dst) : grid(grid), acc(grid), dst(dst) {
const T& nEnd = grid.getNearestNode( GridPoint(dst.x, dst.y, dst.z) );
dijkstra.build(&nEnd, acc);
}
void addShortestPath(Point3 start, PlotEST& plot) {
const T& nStart = grid.getNearestNode( GridPoint(start.x, start.y, start.z) );
const T& nEnd = grid.getNearestNode( GridPoint(dst.x, dst.y, dst.z) );
DijkstraNode<T>* dnStart = dijkstra.getNode(nStart);
DijkstraNode<T>* dnEnd = dijkstra.getNode(nEnd);
plot.path1.setColorHex("#0000ee");
plot.path1.clear();
DijkstraPath<T> dp(dnStart, dnEnd);
for (DijkstraNode<T>* dn : dp) {
K::GnuplotPoint3 gp(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm);
plot.path1.add(gp);
}
}
};
public:
void xxx() {
float stepSize = 0.71;
K::ParticleFilter<MyState, MyControl, MyObservation>* pf1;
K::ParticleFilter<MyState, MyControl, MyObservation>* pf2;
Grid<MyGridNode> grid(20);
Helper::FHWSFloors floors = Helper::getFloors(grid);
Helper::buildTheGrid(grid, floors);
pf1 = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
pf2 = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
std::unique_ptr<MyEvaluation> eval1 = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval1.get()->setUsage(true, true, true, true, true);
pf1->setEvaluation( std::move(eval1) );
pf1->setNEffThreshold(1.0);
pf1->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
pf1->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
std::unique_ptr<MyEvaluation> eval2 = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval2.get()->setUsage(true, true, true, true, true);
pf2->setEvaluation( std::move(eval2) );
pf2->setNEffThreshold(1.0);
pf2->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
pf2->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//runName = "bergwerk_path4_nexus_shortest";
GroundTruthWay gtw;
SensorReader* sr;
SensorReaderTurn* srt;
SensorReaderStep* srs;
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!
BarometerEvaluation::barometerSigma = 0.10;
sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
gtw = EvalBase::getGroundTruthWay(*sr, floors.gtwp, path4dbl);
MyGridNode& end = (MyGridNode&)grid.getNodeFor( EvalBase::conv(floors.gtwp[path4dbl.back()]) );
{
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
//DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
pf1->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
{
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf2->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
PlotEST pEst(floors, gtw);
PlotEST pShortest(floors, gtw);
PlotErr pErr;
LeShortestPath<MyGridNode> pShortestFac(grid, gtw.back().value);
K::Gnuplot gp;
K::GnuplotMultiplot multiplot(2,1);
multiplot.add(&pErr.plot);
multiplot.add(&pEst.plot);
//multiplot.add(&pShortest.plot);
// sensor numberspShortest
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
//const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
//Create an BarometerSensorReader
BarometerSensorReader baroSensorReader;
//Read all turn Observations
while(srt->hasNext()) {
SensorEntryTurn set = srt->getNext();
TurnObservation to;
to.ts = set.ts;
to.delta_heading = set.delta_heading;
to.delta_motion = set.delta_motion;
turn_observations.push_back(to);
}
//Step Observations
while(srs->hasNext()) {
SensorEntryStep ses = srs->getNext();
StepObservation so;
so.ts = ses.ts;
step_observations.push_back(so);
}
// the to-be-evaluated observation
MyObservation obs;
obs.step = new StepObservation(); obs.step->steps = 0;
obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
// control data
MyControl ctrl;
std::vector<Point3> pathEst1;
std::vector<Point3> pathEst2;
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()) {
// get the next sensor reading from the CSV
const SensorEntry se = sr->getNext();
//start_time needed for time calculation of steps and turns
obs.latestSensorDataTS = se.ts;
if (start_time == -1) {start_time = se.ts;}
int64_t current_time = se.ts - start_time;
switch(se.idx) {
case s_wifi: {
obs.wifi = WiFiSensorReader::readWifi(se);
break;
}
case s_beacons: {
BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
if (!boe.mac.empty()) {
obs.beacons.entries.push_back(boe);
} // add the observed beacon
obs.beacons.removeOld(obs.latestSensorDataTS);
break;
}
case s_barometer: {
obs.barometer = baroSensorReader.readBarometer(se);
break;
}
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
break;
}
}
// process all occurred turns
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 * stepSize;
}
// process all occurred steps
while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
obs.turn->delta_heading += _to.delta_heading;
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
//auto tick1 = Time::tick();
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf1->getTransition())->setCurrentTime(lastTransitionTS);
((MyTransition*)pf2->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est1 = pf1->update(&ctrl, obs);
const MyState est2 = pf2->update(&ctrl, obs);
const Point3 curEst1 = est1.pCur;
const Point3 curEst2 = est2.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 0;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff1 = curEst1 - curGT;
const Point3 diff2 = curEst2 - curGT;
const float err1 = diff1.length();
const float err2 = diff2.length();
//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
pathEst1.push_back(curEst1);
pathEst2.push_back(curEst2);
//const float err = diff.length();
//errors.push_back(err);
// plot
pEst.setCur(curEst1, curGT);
pEst.addParticles(pf1->getParticles());
pEst.addEst1(pathEst1);
pEst.addEst2(pathEst2);
pShortestFac.addShortestPath(curEst1, pShortest);
pErr.add(se.ts-start_time, err1/100, err2/100);
static int pi = 0;
static int dspCnt = 0;
if (++dspCnt > 0) {
//pEst.gp.draw(pEst.plot);
//pEst.gp.flush();
//pShortest.gp.draw(pShortest.plot);
//pShortest.gp.flush();
float s = 0.75;
gp.setTerminal("pngcairo", K::GnuplotSize(50*s,29*s));
gp.setOutput("/tmp/1/" + std::to_string(pi++) + ".png");
gp << "set multiplot\n";
gp << "set border\n";
gp << "set tics\n";
gp << "set size 1.0, 0.25 \n";
gp << "set origin 0.0, 0.0 \n";
gp << "unset xtics\n";
gp.draw(pErr.plot);
gp << "unset xrange; unset yrange; unset zrange;\n";
gp << "set size 0.68, 1.04 \n";
gp << "set origin -0.08, 0.04 \n";
gp << pEst.misc.str();
gp.draw(pEst.plot);
gp << "unset arrow\n";
gp << "set origin 0.405, 0.04 \n";
gp << pShortest.misc.str();
gp.draw(pShortest.plot);
gp << "unset arrow\n";
gp << "unset multiplot\n";
gp.flush();
usleep(1000*33); // prevent gnuplot errors
dspCnt = 0;
std::cout << pi << std::endl;
/** GIF */
/*
#!/bin/sh
palette="/tmp/palette.png"
filters=""
ffmpeg -i %d.png -vf "$filters,palettegen" -y $palette
ffmpeg -i %d.png -i $palette -lavfi "setpts=3.0*PTS,paletteuse" yy.gif
OR
ffmpeg -f image2 -i %d.png -vf "setpts=3.0*PTS" nyan.flv
*/
}
}
}
}
};
#endif // PRESENTATION_H