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

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
grid/factory/v2/GridFactory.h
View File

@@ -18,18 +18,7 @@
#include <functional>
/** listen for events during the build process */
class GridFactoryListener {
public:
virtual void onGridBuildUpdateMajor(const std::string& what) = 0;
virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0;
virtual void onGridBuildUpdateMinor(const std::string& what) = 0;
virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0;
};
#include "GridFactoryListener.h"
template <typename T> class GridFactory {
@@ -156,7 +145,7 @@ public:
GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
bbox.grow(0.012345);
bbox.grow(0.42345);
if (intersects(bbox, floor)) {continue;}
// add to the grid
@@ -170,7 +159,7 @@ public:
}
// connect the g
connectAdjacent(z_cm);
connectAdjacent(floor, z_cm);
}
@@ -186,6 +175,9 @@ public:
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
// cleanup
stairs.finalize();
}
@@ -273,7 +265,7 @@ public:
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const float z_cm) {
void connectAdjacent(const Floorplan::Floor* floor, const float z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::tick();
@@ -285,7 +277,7 @@ public:
if (n1.z_cm != z_cm) {continue;}
// connect the node with its neighbors
connectAdjacent(n1);
connectAdjacent(floor, n1);
}
@@ -293,8 +285,13 @@ public:
}
/** connect the given node with its neighbors */
void connectAdjacent(T& n1) {
/**
* connect the given node with its neighbors.
* even though a node has a neighbor, it might still be blocked by an obstacle:
* e.g. a 45° wall directly between nodes. a neighbor exists, but is unreachable due to the wall.
* we thus perform an additional intersection check with all obstacles within the floor the node n1 belongs to
*/
void connectAdjacent(const Floorplan::Floor* floor, T& n1) {
const int gridSize_cm = grid.getGridSize_cm();
@@ -313,6 +310,7 @@ public:
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
if (isBlocked(floor, n1, *n2)) {continue;} // is there a (e.g. small) obstacle between the two?
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
@@ -479,7 +477,46 @@ private:
}
}
return false;
}
/**
* normally, the algorithm will not try to connect two nodes that are neighbors but have an obstacle between them.
* however, especially for 45° obstacles it might happen that a neighbor exists but is blocked by an obstacle.
* we thus need this additional check to ensure everything is fine... even though it needs performance...
*/
static inline bool isBlocked(const Floorplan::Floor* floor, const GridPoint& n1, const GridPoint& n2) {
// (obstacles use meter, while the nodes are in centimeter!
const Point2 p1_m = n1.inMeter().xy();
const Point2 p2_m = n2.inMeter().xy();
const Line2 lineNodes(p1_m, p2_m);
// process each obstacle
for (Floorplan::FloorObstacle* fo : floor->obstacles) {
// depends on the type of obstacle
if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
const Line2 lineObstacle(line->from, line->to);
if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;}
} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
throw Exception("should not happen");
} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
// DOORS ARE NOT AN OBSTACLE
} else {
throw Exception("TODO: not yet implemented obstacle type");
}
}
return false;
}
/** does the bbox intersect with any of the floor's walls? */

19
grid/factory/v2/GridFactoryListener.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef GRIDFACTORYLISTENER_H
#define GRIDFACTORYLISTENER_H
#include <string>
/** listen for events during the build process */
class GridFactoryListener {
public:
virtual void onGridBuildUpdateMajor(const std::string& what) = 0;
virtual void onGridBuildUpdateMajor(const int cnt, const int cur) = 0;
virtual void onGridBuildUpdateMinor(const std::string& what) = 0;
virtual void onGridBuildUpdateMinor(const int cnt, const int cur) = 0;
};
#endif // GRIDFACTORYLISTENER_H

43
grid/factory/v2/Importance.h
View File

@@ -1,8 +1,11 @@
#ifndef IMPORTANCE_H
#define IMPORTANCE_H
#include "../../../geo/Units.h"
#include "../../Grid.h"
#include "GridFactoryListener.h"
#include "../../../misc/KNN.h"
#include "../../../misc/KNNArray.h"
@@ -35,9 +38,14 @@ public:
}
/** attach importance-factors to the grid */
template <typename T> static void addImportance(Grid<T>& g) {
template <typename T> static void addImportance(Grid<T>& g, GridFactoryListener* l = nullptr) {
Log::add(name, "adding importance information to all nodes");// at height " + std::to_string(z_cm));
Log::add(name, "adding importance information to all nodes");
if (l) {
l->onGridBuildUpdateMajor(2, 0);
l->onGridBuildUpdateMajor("adding importance information");
l->onGridBuildUpdateMinor("performing initial setups");
}
// get an inverted version of the grid
Grid<T> inv(g.getGridSize_cm());
@@ -51,15 +59,13 @@ public:
// construct KNN search
KNN<Grid<T>, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 12;
// create list of all door-nodes
std::vector<T> doors;
// create list of all stair-nodes
std::vector<T> stairs;
// process each node
for (T& n1 : g) {
@@ -83,9 +89,26 @@ public:
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
if (l) {
l->onGridBuildUpdateMajor(2, 1);
l->onGridBuildUpdateMinor("calculating importance for each node");
}
std::cout << "dunno why, but the KNN for stairs searches extremely slow!" << std::endl;
// process each node again
for (T& n1 : g) {
for (int i = 0; i < g.getNumNodes(); ++i) {
// log
if (i % (g.getNumNodes() / 20) == 0) {
if (l) {
l->onGridBuildUpdateMinor(g.getNumNodes(), i);
}
}
// get the node
T& n1 = g[i];
// get the distance to the nearest wall
const float distToWall_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
@@ -101,12 +124,16 @@ public:
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 3.5f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
// use wall avoidance
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m);
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
}
// sanity check
Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
}
}

24
grid/factory/v2/Stairs.h
View File

@@ -22,6 +22,9 @@ private:
/** calculation helper */
Helper<T> helper;
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
std::ofstream outStairs;
std::ofstream outDelete;
@@ -77,6 +80,7 @@ public:
}
~Stairs() {
finalize();
outStairs.close();
outDelete.close();
}
@@ -201,10 +205,6 @@ public:
}
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
// add all stair nodes or replace them with already existing ones
for (Intermediate& iNode : stairNodes) {
@@ -288,13 +288,23 @@ public:
}
}
// delete all pending nodes and perform a cleanup
for (T* n : toDelete) {grid.remove(*n);}
grid.cleanup();
// finalize after ALL stairs were added. much faster and should be safe!
//finalize();
}
void finalize() {
// delete all pending nodes and perform a cleanup
if (!toDelete.empty()) {
for (T* n : toDelete) {grid.remove(*n);}
toDelete.clear();
grid.cleanup();
}
}
static float minZ(const std::vector<XYZ>& points) {
auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
auto it = std::min_element(points.begin(), points.end(), comp);