FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added several grid-walks

Browse Source 
added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...
This commit is contained in:
FrankE
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
tests/grid/Plot.h
View File

@@ -10,14 +10,16 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include "../../floorplan/Floor.h"
class GP : public GridNode, public GridPoint {
public:
float imp = 1.0f;
float impPath = 1.0f;
float distToTarget = 1.0;
public:
GP() : GridNode(), GridPoint() {;}
GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
};
class Plot {
@@ -27,22 +29,76 @@ public:
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementColorPoints points;
K::GnuplotSplotElementColorPoints nodes;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementLines floors;
template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
/** ctor */
Plot() {
gp << "set ticslevel 0\n";
gp << "set view equal xyz\n";
gp << "set cbrange[0.5:1.5]\n";
gp << "set palette gray negative\n";
//gp << "set hidden3d front\n";
splot.add(&nodes);
splot.add(&floors);
floors.setLineWidth(2);
floors.setColorHex("#008800");
}
template <int gridSize_cm, typename T> Plot& showGrid(Grid<gridSize_cm, T>& g) {
addEdges(g);
addNodes(g);
return *this;
}
template <int gridSize_cm, typename T> Plot& addEdges(Grid<gridSize_cm, T>& g) {
// prevent adding edges twice
std::set<size_t> done;
for (GP& n1 : g) {
for (const T& n2 : g.neighbors(n1)) {
size_t edge = g.getUID(n1) ^ g.getUID(n2);
if (done.find(edge) == done.end()) {
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
lines.addSegment(p1, p2);
done.insert(edge);
}
}
}
return *this;
}
template <int gridSize_cm, typename T> Plot& addNodes(Grid<gridSize_cm, T>& g) {
for (GP& n1 : g) {
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
nodes.add(p1, 1.0);
}
return *this;
}
template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
std::set<uint64_t> done;
int cnt = 0;
for (int i = 0; i < g.getNumNodes(); ++i) {
const GP& n1 = g[i];
points.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), n1.imp);
//const float color = std::pow(n1.distToTarget,0.1);
const float color = n1.imp * n1.impPath;
nodes.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), color);
for (const T& n2 : g.neighbors(n1)) {
//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
@@ -59,15 +115,28 @@ public:
}
points.setPointSize(1);
//splot.add(&lines);
splot.add(&points);
nodes.setPointSize(1.0);
return *this;
}
/** remove all floors from the plot */
Plot& resetFloors() {
floors.clear();
return *this;
}
/** add the given floor to the plot */
Plot& addFloor(Floor& f, const float z_cm) {
for (const Line2& l : f.getObstacles()) {
K::GnuplotPoint3 p1(l.p1.x, l.p1.y, z_cm+10);
K::GnuplotPoint3 p2(l.p2.x, l.p2.y, z_cm+10);
floors.addSegment(p1, p2);
}
return *this;
}
Plot& fire() {
gp.draw(splot);
gp.flush();

30
tests/grid/TestAll.cpp
View File

@@ -6,6 +6,7 @@
#include "../../grid/factory/GridFactory.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../grid/walk/GridWalkWeighted.h"
#include "Plot.h"
@@ -29,38 +30,55 @@ TEST(TestAll, Nav) {
} tmp(g);
GridFactory<20, GP> gf(g);
FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
// load floorplan
FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
Floor f1 = fpf.getFloor("1");
Floor f2 = fpf.getFloor("2");
Stairs s1_2 = fpf.getStairs("1_2");
// build the grid
gf.addFloor(f1, 20);
gf.addFloor(f2, 340);
gf.addStairs(s1_2, 20, 340);
gf.removeIsolated();
// calculate node importance based on the floorplan (walls, ...)
GridImportance gi;
gi.addImportance(g, 20);
gi.addImportance(g, 340);
//gi.addImportance(g, 20);
//gi.addImportance(g, 340);
// calculate dijkstra using aforementioned importance
Dijkstra<GP> d;
const GP& start = g.getNodeFor(GridPoint(500,200,20));
//const GP& end = g.getNodeFor(GridPoint(1400,1400,20));
const GP& end = g.getNodeFor(GridPoint(1200,200,340));
//const GP& end = g.getNodeFor(GridPoint(1300,1300,20));
d.build(start, end, tmp);
// add the path's importance to the grid
gi.addImportance(g, d.getNode(start), d.getNode(end));
// plot path
K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
DijkstraNode<GP>* dn = d.getNode(end);
while (dn->previous != nullptr) {
path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm));
path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
dn = dn->previous;
}
// // walk
// GridWalkWeighted<GP> walk;
// Log::add("test", "walking");
// for (int i = 0; i < 5000; ++i) {
// walk.getDestination(g, &start, 1.0, Headings::UP);
// }
// Log::add("test", "done");
Plot p;
p.build(g);
//p.showGrid(g);
p.addFloor(f1, 20);
p.addFloor(f2, 340);
p.splot.add(&path);
p.fire();

127
tests/grid/TestWalk.cpp Normal file
View File

@@ -0,0 +1,127 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "Plot.h"
#include "../../grid/factory/GridImportance.h"
#include "../../grid/factory/GridFactory.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../grid/walk/GridWalkState.h"
#include "../../grid/walk/GridWalkWeighted.h"
#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"
TEST(Walk, plot) {
Grid<20, GP> g;
GridFactory<20, GP> gf(g);
bool use3D = true;
// load floorplan
FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
Floor f1 = fpf.getFloor("1");
Floor f2 = fpf.getFloor("2");
Stairs s1_2 = fpf.getStairs("1_2");
// build the grid
gf.addFloor(f1, 20);
if (use3D) {
gf.addFloor(f2, 340);
gf.addStairs(s1_2, 20, 340);
}
gf.removeIsolated();
// calculate node importance based on the floorplan (walls, ...)
GridImportance gi;
gi.addImportance(g, 20);
gi.addImportance(g, 340);
// dijkstra mapper
class TMP {
Grid<20, GP>& grid;
public:
TMP(Grid<20, GP>& grid) : grid(grid) {;}
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2.0);
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
}
} tmp(g);
// // calculate shortest path
// Dijkstra<GP> d;
const GP* start = &g.getNodeFor(GridPoint(700,200,20));
const GP* end;
if (use3D) {
end = &g.getNodeFor(GridPoint(1200,200,340));
} else {
end = &g.getNodeFor(GridPoint(1300,1300,20));
}
// d.build(end, start, tmp);
// gi.addDistanceToTarget(g, d);
// // add the path's importance to the grid
// gi.addImportance(g, d.getNode(start), d.getNode(end));
// // plot path
// K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
// DijkstraNode<GP>* dn = d.getNode(start);
// while (dn->previous != nullptr) {
// path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
// dn = dn->previous;
// }
// walk
GridWalkLightAtTheEndOfTheTunnel<GP> walk(g, tmp, *end);
std::minstd_rand gen;
std::normal_distribution<float> dist(0.3, 0.3);
K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000");
// setup starting states
std::vector<GridWalkState<GP>> states;
for (int i = 0; i < 1000; ++i) {
states.push_back( GridWalkState<GP>( start, Headings::UP ) );
}
Plot p;
//p.build(g);
p.addFloor(f1, 20);
if (use3D) {
p.addFloor(f2, 340);
}
//p.splot.add(&path);
p.splot.add(&pStates);
p.nodes.setPointSize(0.2);
if (!use3D) {
p.gp << "set view 0,0\n";
}
p.gp << "set view equal xy\n";
// walk random distances
for (int i = 0; i < 5000; ++i) {
pStates.clear();
for (GridWalkState<GP>& state : states) {
state = walk.getDestination(g, state, std::abs(dist(gen)));
pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
}
p.gp.draw(p.splot);
p.gp.flush();
usleep(1000*80);
}
}
#endif