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

94
grid/Grid.h
View File

@@ -2,15 +2,16 @@
#define GRID_H
#include <vector>
#include <iostream>
#include <unordered_map>
#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"
#include <iostream>
#include <KLib/Assertions.h>
#include "../geo/BBox3.h"
#include "../misc/Debug.h"
/**
* grid of the given grid-size, storing some value which
@@ -18,7 +19,11 @@
*/
template <int gridSize_cm, typename T> class Grid {
typedef uint64_t UID;
static constexpr const char* name = "Grid";
#include "GridNeighborIterator.h"
typedef uint64_t UID;
private:
@@ -72,10 +77,7 @@ public:
void connectUniDir(T& n1, const T& n2) {
n1._neighbors[n1._numNeighbors] = n2._idx;
++n1._numNeighbors;
if (n1._numNeighbors > 12) {
int i = 0;
}
_assertBetween(n1._numNeighbors, 0, 12, "number of neighbors out of bounds!");
_assertBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
}
/**
@@ -154,14 +156,21 @@ public:
*
*/
UID getUID(const GridPoint& p) const {
const uint64_t x = std::round(p.x_cm / gridSize_cm);
const uint64_t y = std::round(p.y_cm / gridSize_cm);
const uint64_t z = std::round(p.z_cm / gridSize_cm);
const uint64_t x = std::round(p.x_cm / (float)gridSize_cm);
const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
return (z << 40) | (y << 20) | (x << 0);
}
/** array access */
T& operator [] (const int idx) {
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
return nodes[idx];
}
/** const array access */
const T& operator [] (const int idx) const {
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
return nodes[idx];
}
@@ -223,48 +232,57 @@ public:
*/
void cleanup() {
for (size_t i = 0; i < nodes.size(); ++i) {
Log::add(name, "running grid cleanup");
// check every single node
for (int i = (int)nodes.size() - 1; i >= 0; --i) {
// is this node marked as "deleted"? (idx == -1)
if (nodes[i]._idx == -1) {
nodes.erase(nodes.begin()+i);
moveDown(i);
--i;
// remove this node
deleteNode(i);
++i;
}
}
// rebuild hashes
Log::add(name, "rebuilding UID hashes");
hashes.clear();
for (size_t idx = 0; idx < nodes.size(); ++idx) {
hashes[getUID(nodes[idx])] = idx;
}
}
void moveDown(const int idx) {
private:
/** hard-delete the given node */
void deleteNode(const int idx) {
// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
// remove him from the node list (reclaim its memory and its index)
nodes.erase(nodes.begin()+idx);
// decrement the index for all of the following nodes and adjust neighbor references
for (size_t i = 0; i < nodes.size(); ++i) {
if (nodes[i]._idx >= idx) {--nodes[i]._idx;}
// decrement the higher indices (reclaim the free one)
if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
// adjust the neighbor references (decrement by one)
for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
}
}
}
class NeighborIter : std::iterator<std::input_iterator_tag, int> {
private:
Grid<gridSize_cm, T>& grid;
int nodeIdx;
int nIdx;
public:
NeighborIter(Grid<gridSize_cm, T>& grid, const int nodeIdx, const int nIdx) : grid(grid), nodeIdx(nodeIdx), nIdx(nIdx) {;}
NeighborIter& operator++() {++nIdx; return *this;}
NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
bool operator==(const NeighborIter& rhs) {return nodeIdx == rhs.nodeIdx && nIdx == rhs.nIdx;}
bool operator!=(const NeighborIter& rhs) {return nodeIdx != rhs.nodeIdx || nIdx != rhs.nIdx;}
T& operator*() {return (T&) grid.nodes[nodeIdx]._neighbors[nIdx];}
};
public:
class NeighborForEach {
private:
Grid<gridSize_cm, T>& grid;
int nodeIdx;
public:
NeighborForEach(Grid<gridSize_cm, T>& grid, const int nodeIdx) : grid(grid), nodeIdx(nodeIdx) {;}
NeighborIter begin() {return NeighborIter(grid, nodeIdx, 0);}
NeighborIter end() {return NeighborIter(grid, nodeIdx, grid[nodeIdx]._numNeighbors);}
};
NeighborForEach neighbors(const int idx) {
return neighbors(nodes[idx]);
@@ -287,7 +305,7 @@ public:
return nodes.size();
}
template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { return false; }
template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { (void) bb; return false; }
inline float kdtree_get_pt(const size_t idx, const int dim) const {
const T& p = nodes[idx];

66
grid/GridNeighborIterator.h Normal file
View File

@@ -0,0 +1,66 @@
#ifndef GRIDNEIGHBORITERATOR_H
#define GRIDNEIGHBORITERATOR_H
/** allows iterating over all neighbors of one node */
class NeighborIter : std::iterator<std::input_iterator_tag, int> {
private:
/** the grid the src-node belongs to */
const Grid<gridSize_cm, T>& grid;
/** index of the source-node within its grid */
const int srcNodeIdx;
/** index of the current neighbor [0:10] */
int nIdx;
public:
/** ctor */
NeighborIter(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
/** next neighbor */
NeighborIter& operator++() {++nIdx; return *this;}
/** next neighbor */
NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
/** compare with other iterator */
bool operator==(const NeighborIter& rhs) {return srcNodeIdx == rhs.srcNodeIdx && nIdx == rhs.nIdx;}
/** compare with other iterator */
bool operator!=(const NeighborIter& rhs) {return srcNodeIdx != rhs.srcNodeIdx || nIdx != rhs.nIdx;}
/** get the neighbor the iterator currently points to */
T& operator*() {return (T&) grid.getNeighbor(srcNodeIdx, nIdx);}
};
/** allows for-each iteration over all neighbors of one node */
class NeighborForEach {
private:
/** the grid the src-node belongs to */
const Grid<gridSize_cm, T>& grid;
/** index of the source-node within its grid */
const int srcNodeIdx;
public:
/** ctor */
NeighborForEach(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx) :
grid(grid), srcNodeIdx(srcNodeIdx) {;}
/** starting point */
NeighborIter begin() {return NeighborIter(grid, srcNodeIdx, 0);}
/** end point */
NeighborIter end() {return NeighborIter(grid, srcNodeIdx, grid[srcNodeIdx]._numNeighbors);}
};
#endif // GRIDNEIGHBORITERATOR_H

20
grid/GridNode.h
View File

@@ -13,22 +13,24 @@ template<int, typename> class Grid;
* to store additional information for each node besides
* the user's requested data-structure
*/
class GridNode {
struct GridNode {
private:
template<int, typename> friend class Grid;
/** INTERNAL: array-index */
int _idx = -1;
int _idx;
/** INTERNAL: number of neighbors */
int _numNeighbors = 0;
int _numNeighbors;
/** INTERNAL: store neighbors (via index) */
int _neighbors[12] = {};
int _neighbors[10];
public:
GridNode() {;}
GridNode() : _idx(-1), _numNeighbors(0), _neighbors() {;}
/** get the node's index within its grid */
int getIdx() const {return _idx;}
@@ -36,10 +38,10 @@ public:
/** get the number of neighbors for this node */
int getNumNeighbors() const {return _numNeighbors;}
/** get the n-th neighbor for this node */
template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
return grid.getNeighbor(_idx, nth);
}
// /** get the n-th neighbor for this node */
// template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
// return grid.getNeighbor(_idx, nth);
// }
};

7
grid/GridPoint.h
View File

@@ -2,6 +2,7 @@
#define GRIDPOINT_H
#include <cmath>
#include "../geo/Point3.h"
struct GridPoint {
@@ -35,6 +36,12 @@ struct GridPoint {
return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f;
}
/** cast to Point3 */
operator Point3() const {return Point3(x_cm, y_cm, z_cm);}
/** cast to string */
operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
};

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;}
}
};