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

added new data-structures

Browse Source 
added new test-cases
added flexible dijkstra calculation
added debugging log
modified: plotting, grid-generation, grid-importance,
refactoring
This commit is contained in:
FrankE
2016-01-22 18:47:06 +01:00
parent 12084fe147
commit cdf97322f8
21 changed files with 720 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
grid/factory/GridFactory.h
View File

@@ -9,10 +9,16 @@
#include "../GridNodeBBox.h"
#include "../Grid.h"
#include "../../misc/Debug.h"
template <int gridSize_cm, typename T> class GridFactory {
/** logging name */
static constexpr const char* name = "GridFac";
private:
/** the grid to build into */
Grid<gridSize_cm, T>& grid;
@@ -24,6 +30,7 @@ public:
/** add the given floor at the provided height (in cm) */
void addFloor(const Floor& floor, const float z_cm) {
Log::add(name, "adding floor at height " + std::to_string(z_cm));
// build grid-points
for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
@@ -43,8 +50,11 @@ public:
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const float z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm));
// connect adjacent grid-points
for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
@@ -79,6 +89,8 @@ public:
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm));
for (const Stair& s : stairs) {
for (int i = 0; i < grid.getNumNodes(); ++i) {
@@ -100,7 +112,7 @@ public:
buildStair(n, n2);
}
int i = 0;
}
}
@@ -182,16 +194,21 @@ public:
void removeIsolated() {
Log::add(name, "searching for isolated nodes");
// get largest connected region
std::set<int> set;
do {
const int idxStart = rand() % grid.getNumNodes();
set.clear();
Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
getConnected(idxStart, set);
Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
} while (set.size() < 0.5 * grid.getNumNodes());
// remove all other
Log::add(name, "removing the isolated nodes");
for (int i = 0; i < grid.getNumNodes(); ++i) {
if (set.find(i) == set.end()) {grid.remove(i);}
}
@@ -206,10 +223,14 @@ private:
/** recursively get all connected nodes and add them to the set */
void getConnected(const int idx, std::set<int>& set) {
T& n1 = (T&) grid[idx];
// get the node behind idx
const T& n1 = (T&) grid[idx];
// add him to the current region
set.insert(n1.getIdx());
for (T& n2 : grid.neighbors(n1)) {
// get all his (unprocessed) neighbors and add them to the region
for (const T& n2 : grid.neighbors(n1)) {
if (set.find(n2.getIdx()) == set.end()) {
getConnected(n2.getIdx(), set);
}

116
grid/factory/GridImportance.h
View File

@@ -4,16 +4,31 @@
#include "../Grid.h"
#include "GridFactory.h"
#include "../../misc/KNN.h"
#include "../../math/MiniMat2.h"
#include "../../misc/Debug.h"
#include <KLib/math/distribution/Normal.h>
/**
* add an importance factor to each node within the grid.
* the importance is calculated based on several facts:
* - nodes that belong to a door or narrow path are more important
* - nodes directly located at walls are less important
*/
class GridImportance {
private:
static constexpr const char* name = "GridImp";
public:
/** attach importance-factors to the grid */
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
// get an inverted version of the grid
Grid<gridSize_cm, T> inv;
GridFactory<gridSize_cm, T> fac(inv);
@@ -22,29 +37,28 @@ public:
// construct KNN search
KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
// the number of neighbors to use
static constexpr int numNeighbors = 8;
for (int idx = 0; idx < g.getNumNodes(); ++idx) {
// process each point
T& n1 = (T&) g[idx];
// // get its nearest neighbor
// size_t idxNear;
// float distSquared;
// float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
// knn.getNearest(point, idxNear, distSquared);
// // calculate importante
// const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
// n1.imp = imp;
size_t indices[10];
float squaredDist[10];
// get the 10 nearest neighbors and their distance
size_t indices[numNeighbors];
float squaredDist[numNeighbors];
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
knn.get(point, 10, indices, squaredDist);
knn.get(point, numNeighbors, indices, squaredDist);
const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
const float imp2 = door( indices );
n1.imp = (imp1 + imp2)/2;
// get the neighbors
std::vector<T*> neighbors;
for (int i = 0; i < numNeighbors; ++i) {
neighbors.push_back(&inv[indices[i]]);
}
addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
addDoor(n1, neighbors);
}
@@ -52,31 +66,67 @@ public:
}
float door( size_t* indices ) {
// build covariance
/** add importance to nSrc if it is part of a door */
template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {
MiniMat2 m;
Point3 center = nSrc;
// calculate the centroid of the nSrc's nearest-neighbors
Point3 centroid(0,0,0);
for (const T* n : neighbors) {
centroid = centroid + (Point3)*n;
}
centroid /= neighbors.size();
// if nSrc is too far from the centroid, this does not make sense
if ((centroid-center).length() > 60) {return;}
// build covariance of the nearest-neighbors
int used = 0;
for (const T* n : neighbors) {
Point3 d = (Point3)*n - center;
if (d.length() > 100) {continue;} // radius search
m.addSquared(d.x, d.y);
++used;
}
// we need at least two points for the covariance
if (used < 2) {return;}
// check eigenvalues
MiniMat2::EV ev = m.getEigenvalues();
// ensure e1 > e2
if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
// door?
if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; }
//if (dist1_m > 1.0) {return 1;}
//return 1.0 - std::abs(dist1_m - dist2_m);
return 1;
}
float importance(float dist_m) {
/** get the importance of the given node depending on its nearest wall */
template <typename T> void addImportance(T& nSrc, float dist_m) {
static K::NormalDistribution d1(0.0, 0.5);
//if (dist_m > 1.5) {dist_m = 1.5;}
return 1.0 - d1.getProbability(dist_m) * 0.5;
// avoid sticking too close to walls (unlikely)
static K::NormalDistribution avoidWalls(0.0, 0.3);
// static K::NormalDistribution d1(1.0, 0.75);
// //static K::NormalDistribution d2(3.0, 0.75);
// favour walking near walls (likely)
static K::NormalDistribution sticToWalls(0.9, 0.5);
// favour walking far away (likely)
static K::NormalDistribution farAway(2.2, 0.5);
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance
nSrc.imp *= 1.0
- avoidWalls.getProbability(dist_m) * 0.35 // avoid walls
+ sticToWalls.getProbability(dist_m) * 0.15 // walk near walls
+ farAway.getProbability(dist_m) * 0.20 // walk in the middle
;
// if (dist_m > 3.0) {dist_m = 3.0;}
// return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);
// if (dist_m < 0.5) {return 0.8;}
// if (dist_m < 1.5) {return 1.2;}
// if (dist_m < 2.5) {return 0.8;}
// else {return 1.2;}
}
};