FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

started working on the tex-part

Browse Source 
started working on eval-graphics
ned helper methods
tested some new aspects
some fixes and changes
added some graphics
new test-floorplan
many cleanups
This commit is contained in:
FrankE
2016-02-03 21:17:15 +01:00
parent 8a57b4cdbd
commit c5a5acbbf6
40 changed files with 69163 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
code/eval/Eval.h
View File

@@ -18,50 +18,53 @@ public:
Eval() {
// Eval() {
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
// pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
// MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
// MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
// //GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
// GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
// //GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
// //GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
// sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
// srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
// srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
// gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
}
// }
// //wifi also uniform dist 0/1 fuer bereiche die OK sind?
// //steps hochzaehlen weil mehr als einer in einer transition??
// //increase regional average region
// void setEval1() {
// runName = "TODO";
void setEval1() {
// // the particle filter's evaluation method
// std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
// eval.get()->setUsage(true, false, false, true, true);
// pf->setEvaluation( std::move(eval) );
// // resampling step?
// pf->setNEffThreshold(1.0);
// pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
runName = "TODO";
// // state estimation step
// //pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
// //pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
// pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.33f)));
// the particle filter's evaluation method
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval.get()->setUsage(true, false, false, true, true);
pf->setEvaluation( std::move(eval) );
// resampling step?
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
}
// }

98
code/eval/Eval1.h Normal file
View File

@@ -0,0 +1,98 @@
#ifndef EVAL1_H
#define EVAL1_H
#include "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 <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>
class Eval1 : public EvalBase {
public:
Eval1() {
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
std::vector<int> wp = path1; std::reverse(wp.begin(), wp.end());
MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) );
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) );
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// path1
//sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
// path2
//sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
//sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward
//srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");
// path3
// sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
// srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
// sr = new SensorReader("./measurements/path3/2/1454345622819.csv"); // backward
// srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt");
// srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, wp);
}
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
//steps hochzaehlen weil mehr als einer in einer transition??
//increase regional average region
void setEval1() {
runName = "TODO";
// the particle filter's evaluation method
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval.get()->setUsage(true, true, true, true, true); // TODO: STEP TURN
pf->setEvaluation( std::move(eval) );
// resampling step?
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
}
};
#endif // EVAL1_H

111
code/eval/EvalBase.h
View File

@@ -47,9 +47,16 @@ protected:
std::string runName;
GroundTruthWay gtw;
std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// OLD
//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
public:
@@ -66,13 +73,17 @@ public:
}
static GridPoint conv(const Point3& p) {
return GridPoint(p.x, p.y, p.z);
}
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;
for (int id : ids) {
auto it = waypoints.find(id);
assert(it != waypoints.end());
if(it == waypoints.end()) {throw "not found";}
path.push_back(it->second);
}
@@ -138,6 +149,11 @@ public:
// 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> pathEst;
@@ -148,7 +164,8 @@ public:
K::Statistics<double> stats;
int cnt = 0;
// process each sensor reading
// process each single sensor reading
while(sr->hasNext()) {
// get the next sensor reading from the CSV
@@ -187,58 +204,39 @@ public:
}
// 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 * 0.71;
}
// scheduled transition every 500 ms
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
// 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);
//Steps are sorted in the list by timestamp.
//If the current observation timestamp is bigger/equal
//to the current step timestamp, use this step as observation
//and remove it from the list.
//The new first timestamp in the list will be then be the next one (timestamp-wise)
StepObservation so;
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
so.step = true;
so.ts = current_time;
}
obs.step = &so;
step_observations.pop_front();
}
else {
so.step = false;
so.ts = current_time;
obs.step = &so;
}
TurnObservation to;
//same principal as for steps is applied for turns
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
to = turn_observations.front();
obs.turn = &to;
turn_observations.pop_front();
}
else {
to.delta_heading = 0.0;
to.delta_motion = 0.0;
obs.turn = &to;
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(nullptr, obs);
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
const int offset = 0; // 750
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
// skip the first 8 scans due to uniform distribution start
@@ -251,28 +249,31 @@ public:
// plot
vis.clearStates();
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
const K::Particle<MyState>& p = pf->getParticles()[i];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
vis.show();;
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.1;
vis.gp << "set arrow 111 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
}
}
{
// vis.setShowParticles(false);
// vis.setShowTime(false);
// vis.setShowCurPos(false);
// vis.debugProcess(0, pathEst, gtw, pf, layers);
// std::ofstream out("/tmp/" + runName + ".data");
// out << vis.getDataset();
// out.close();
}
sleep(1000);
}

95
code/eval/PaperPlot.h Normal file
View File

@@ -0,0 +1,95 @@
#ifndef PAPERPLOT_H
#define PAPERPLOT_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot {
public:
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementColorPoints nodes;
public:
PaperPlot() {
floors.setLineWidth(2);
plot.add(&nodes);
plot.add(&floors);
gp << "set ticslevel 0\n";
//gp << "set zrange [0:0]\n";
}
void show() {
gp.draw(plot);
gp.flush();;
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f, const LengthF height) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
floors.addSegment(p1, p2);
}
}
/** add the grid to the plot */
template <typename T> void addGrid(Grid<T>& grid) {
// std::set<uint64_t> used;
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = n1.imp;
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
for (const T& n1 : grid) {
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
const float color = n1.imp;
//const float color = n1.distToTarget/max;
//const float color = 0;
nodes.add(p1, color);
// for (const T& n2 : grid.neighbors(n1)) {
// const uint64_t idx = n1.getIdx() * n2.getIdx();
// if (used.find(idx) == used.end()) {
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
// gridEdges.addSegment(p1, p2);
// used.insert(idx);
// }
// }
}
}
};
#endif // PAPERPLOT_H

141
code/eval/PaperPlot2D.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef PAPERPLOT2D_H
#define PAPERPLOT2D_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
#include <Indoor/floorplan/Floor.h>
#include <Indoor/geo/Length.h>
class PaperPlot2D {
public:
struct Size {
float w;
float h;
Size(const float w, const float h) : w(w), h(h) {;}
};
public:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines floors;
K::GnuplotPlotElementColorPoints nodes;
std::string file;
public:
PaperPlot2D(const std::string& file, Size s) : file(file) {
toFile(file, s);
setup();
}
PaperPlot2D() {
setup();
}
private:
void setup() {
floors.setLineWidth(1.5);
nodes.setPointType(7);
plot.add(&nodes);
plot.add(&floors);
gp << "unset border\n";
gp << "unset colorbox\n";
gp << "set tics scale 0,0\n"; // HACK! "unset tics\n" segfaults current gnuplot version...
gp << "set format x ' '\n";
gp << "set format y ' '\n";
gp << "set size ratio -1\n";
}
void toFile(const std::string& file, const Size s) {
gp << "set output '" << file << "'\n";
gp << "set terminal eps size " << s.w << "," << s.h << "\n";
}
public:
void setRanges(const float x1, const float x2, const float y1, const float y2) {
gp << "set xrange [" << x1 << ":" << x2 << "]\n";
gp << "set yrange [" << y1 << ":" << y2 << "]\n";
}
void show() {
gp.draw(plot);
if (file.length() != 0) {
std::string dataFile = file + ".dat";
std::ofstream os(dataFile.c_str());
os << gp.getBuffer();
os.close();
}
gp.flush();
}
/** add all obstacles of the given floor to the provided height */
void addFloor(const Floor& f) {
// add each wall
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint2 p1(l.p1.x, l.p1.y);
const K::GnuplotPoint2 p2(l.p2.x, l.p2.y);
floors.addSegment(p1, p2);
}
}
// void removeGrid() {
// gp << "unset object\n";
// }
/** add the grid to the plot */
template <typename T, typename Colorizer> void addGrid(Grid<T>& grid, const Colorizer col) {
// get the min/max value
float max = -999999;
float min = +999999;
for (const T& n1 : grid) {
const float val = col.get(n1);
//const float val = n1.distToTarget;
if (val > max) {max = val;}
if (val < min) {min = val;}
}
gp << "set cbrange["<<min<<":"<<max<<"]\n";
// for (const T& n1 : grid) {
// const K::GnuplotPoint2 p1(n1.x_cm, n1.y_cm);
// const float color = n1.imp;
// //const float color = n1.distToTarget/max;
// //const float color = 0;
// nodes.add(p1, color);
// }
int i = 0;
for (const T& n1 : grid) {
if (col.skip(n1)) {continue;}
gp << "set object " << (++i) << " rectangle center " << n1.x_cm << "," << n1.y_cm << " size 20,20 fs solid noborder fc palette cb " << col.get(n1) << "\n";
}
}
};
#endif // PAPERPLOT2D_H

4
code/eval/PaperVisDijkstra.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef PAPERVISDIJKSTRA_H
#define PAPERVISDIJKSTRA_H
#endif // PAPERVISDIJKSTRA_H

190
code/eval/PaperVisImportance.h Normal file
View File

@@ -0,0 +1,190 @@
#ifndef PAPERVISIMPORTANCE_H
#define PAPERVISIMPORTANCE_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>
#include "PaperPlot.h"
#include "PaperPlot2D.h"
#include "../MyGridNode.h"
#include "../Settings.h"
#include "../DijkstraMapper.h"
PaperPlot2D::Size s1 = PaperPlot2D::Size(2,4);
class PaperVisImportance {
public:
static float clamp(const float in, const float min, const float max) {
if (in < min) {return min;}
if (in > max) {return max;}
return in;
}
// use node-importance as grid-color
struct ColorizerImp {
float get(const MyGridNode& n) const {return n.imp;}
bool skip(const MyGridNode& n) const {return false;}
};
// use node-distance as grid-color
struct ColorizerDist {
float get(const MyGridNode& n) const {return n.distToTarget;}
bool skip(const MyGridNode& n) const {return false;}
};
// use num-visited as grid-color
struct ColorizeHeat {
int maxCnt; int cutoff;
ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
bool skip(const MyGridNode& n) const {return n.cnt < cutoff;} // skip to reduce plot size
};
static void createImportance() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
{
PaperPlot2D plot("floorplan_importance.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.addGrid(grid, ColorizerImp());
plot.show();
}
}
static void createPath() {
// load the floorplan
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
Floor f0 = fpFac.getFloor("test1");
const LengthF h0 = LengthF::cm(0);
// add the floorplan to the grid
Grid<MyGridNode> grid(20);
GridFactory<MyGridNode> gridFac(grid);
gridFac.addFloor(f0, h0.cm());
// remove all isolated nodes not attached to 300,300,floor0
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
// start and end
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
// build all shortest path to reach th target
Dijkstra<MyGridNode> dijkstra;
DijkstraMapper accImp(grid);
DijkstraMapperNormal accNormal(grid);
// path without importance
dijkstra.build(gnStart, gnStart, accNormal);
DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// stamp importance information onto the grid-nodes
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
// path WITH importance
dijkstra.build(gnStart, gnStart, accImp);
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// build plot
K::GnuplotPlotElementLines gpPath1; gpPath1.setLineWidth(2); gpPath1.setColorHex("#444444");
K::GnuplotPlotElementLines gpPath2; gpPath2.setLineWidth(2); gpPath2.setColorHex("#000000");
for (DijkstraNode<MyGridNode>* dn : pathNormal) {
gpPath1.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
for (DijkstraNode<MyGridNode>* dn : pathImp) {
gpPath2.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
}
// plot the 2 paths
{
PaperPlot2D plot("floorplan_paths.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.addFloor(f0);
plot.plot.add(&gpPath1); gpPath1.setCustomAttr("dashtype 3");
plot.plot.add(&gpPath2);
plot.show();
}
// stamp distance information onto the grid
// attach a corresponding weight-information to each user-grid-node
for (MyGridNode& node : grid) {
const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
node.distToTarget = dn->cumWeight;
}
// walk
GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);
for (int i = 0; i < 30000; ++i) {
if (i % 250 == 0) {std::cout << i << std::endl;}
const MyGridNode& nStart = gnEnd;
GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
}
// plot the heat-map
{
PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1);
plot.setRanges(0,2100, 0,5100);
plot.gp << "set palette gray negative\n";
plot.addFloor(f0);
plot.addGrid(grid, ColorizeHeat(7000, 50));
plot.show();
}
}
};
#endif // PAPERVISIMPORTANCE_H