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

many changes :P

Browse Source
This commit is contained in:
k-a-z-u
2016-01-21 20:01:20 +01:00
parent a7dc0cabbb
commit 12084fe147
29 changed files with 2900 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

199
grid/Grid.h
View File

@@ -9,6 +9,9 @@
#include "GridNode.h"
#include <iostream>
#include <KLib/Assertions.h>
#include "../geo/BBox3.h"
/**
* grid of the given grid-size, storing some value which
* extends GridPoint
@@ -47,6 +50,11 @@ public:
*/
int add(const T& elem) {
assertAligned(elem); // assert that the to-be-added element is aligned to the grid
return addUnaligned(elem);
}
/** add the given (not necessarly aligned) element to the grid */
int addUnaligned(const T& elem) {
const int idx = nodes.size(); // next free index
const UID uid = getUID(elem); // get the UID for this new element
nodes.push_back(elem); // add it to the grid
@@ -55,27 +63,39 @@ public:
return idx; // done
}
/** connect (uni-dir) i1 -> i2 */
void connectUniDir(const int idx1, const int idx2) {
connectUniDir(nodes[idx1], nodes[idx2]);
}
/** connect (uni-dir) i1 -> i2 */
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!");
}
/**
* connect (bi-directional) the two provided nodes
* @param idx1 index of the first element
* @param idx2 index of the second element
*/
void connect(const int idx1, const int idx2) {
T& n1 = nodes[idx1]; // get the first element
T& n2 = nodes[idx2]; // get the second element
connect(n1, n2);
void connectBiDir(const int idx1, const int idx2) {
connectBiDir(nodes[idx1], nodes[idx2]);
}
/**
* connect (bi-directional) the two provided nodes
* @param n1 the first node
* @param n2 the second node
*/
void connect(T& n1, T& n2) {
n1._neighbors[n1._numNeighbors] = n2._idx; // add them both as neighbors
n2._neighbors[n2._numNeighbors] = n1._idx;
++n1._numNeighbors; // increment the neighbor-counter
++n2._numNeighbors;
void connectBiDir(T& n1, T& n2) {
connectUniDir(n1, n2);
connectUniDir(n2, n1);
}
/** get the number of contained nodes */
@@ -93,6 +113,24 @@ public:
return e._numNeighbors;
}
/** get the n-th neighbor for the given node */
T& getNeighbor(const int nodeIdx, const int nth) const {
const T& node = nodes[nodeIdx];
return getNeighbor(node, nth);
}
/** get the n-th neighbor for the given node */
T& getNeighbor(const T& node, const int nth) const {
const T& neighbor = nodes[node._neighbors[nth]];
return (T&) neighbor;
}
/** do we have a center-point the given point belongs to? */
bool hasNodeFor(const GridPoint& p) const {
const UID uid = getUID(p);
return (hashes.find(uid) != hashes.end());
}
/** get the center-node the given Point belongs to */
const T& getNodeFor(const GridPoint& p) {
const UID uid = getUID(p);
@@ -123,10 +161,151 @@ public:
}
/** array access */
const T& operator [] (const int idx) const {
T& operator [] (const int idx) {
return nodes[idx];
}
/** disconnect the two nodes (bidirectional) */
void disconnectBiDir(const int idx1, const int idx2) {
disconnectBiDir(nodes[idx1], nodes[idx2]);
}
/** disconnect the two nodes (bidirectional) */
void disconnectBiDir(T& n1, T& n2) {
disconnectUniDir(n1, n2);
disconnectUniDir(n2, n1);
}
/** remove the connection from n1 to n2 (not the other way round!) */
void disconnectUniDir(T& n1, T& n2) {
for (int n = 0; n < n1._numNeighbors; ++n) {
if (n1._neighbors[n] == n2._idx) {
arrayRemove(n1._neighbors, n, n1._numNeighbors);
--n1._numNeighbors;
return;
}
}
}
/** remove the given array-index by moving all follwing elements down by one */
template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
for (int i = idxToRemove+1; i < arrayLen; ++i) {
arr[i-1] = arr[i];
}
}
/**
* mark the given node for deletion
* see: cleanup()
*/
void remove(const int idx) {
remove(nodes[idx]);
}
void remove(T& node) {
// disconnect from all neighbors
while (node._numNeighbors) {
disconnectBiDir(node._idx, node._neighbors[0]);
}
// remove from hash-list
hashes.erase(getUID(node));
// mark for deleteion (see: cleanup())
node._idx = -1;
}
/**
* remove all nodes, marked for deletion.
* BEWARE: this will invalidate all indices used externally!
*/
void cleanup() {
for (size_t i = 0; i < nodes.size(); ++i) {
if (nodes[i]._idx == -1) {
nodes.erase(nodes.begin()+i);
moveDown(i);
--i;
}
}
}
void moveDown(const int idx) {
for (size_t i = 0; i < nodes.size(); ++i) {
if (nodes[i]._idx >= idx) {--nodes[i]._idx;}
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];}
};
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]);
}
NeighborForEach neighbors(const T& node) {
return NeighborForEach(*this, node._idx);
}
/** get the grid's bounding-box. EXPENSIVE! */
BBox3 getBBox() const {
BBox3 bb;
for (const T& n : nodes) {
bb.add( Point3(n.x_cm, n.y_cm, n.z_cm) );
}
return bb;
}
int kdtree_get_point_count() const {
return nodes.size();
}
template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { return false; }
inline float kdtree_get_pt(const size_t idx, const int dim) const {
const T& p = nodes[idx];
if (dim == 0) {return p.x_cm;}
if (dim == 1) {return p.y_cm;}
if (dim == 2) {return p.z_cm;}
throw 1;
}
inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
const float d0 = p1[0] - nodes[idx_p2].x_cm;
const float d1 = p1[1] - nodes[idx_p2].y_cm;
const float d2 = p1[2] - nodes[idx_p2].z_cm;
return (d0*d0) + (d1*d1) + (d2*d2);
}
private:
/** asssert that the given element is aligned to the grid */

18
grid/GridNode.h
View File

@@ -4,6 +4,9 @@
#include "GridNodeBBox.h"
#include "GridPoint.h"
template<int, typename> class Grid;
/**
* INTERNAL DATASTRUCTURE
* this data-structure is internally used by the Grid
@@ -17,17 +20,26 @@ class GridNode {
/** INTERNAL: array-index */
int _idx = -1;
/** INTERNAL: store neighbors (via index) */
/** INTERNAL: number of neighbors */
int _numNeighbors = 0;
/** INTERNAL: number of neighbors */
int _neighbors[10] = {};
/** INTERNAL: store neighbors (via index) */
int _neighbors[12] = {};
public:
GridNode() {;}
/** get the node's index within its grid */
int getIdx() const {return _idx;}
/** 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);
}
};

36
grid/GridNodeBBox.h
View File

@@ -2,43 +2,21 @@
#define GRIDNODEBBOX_H
#include "GridPoint.h"
#include "../geo/Line2D.h"
#include "../geo/BBox2.h"
/**
* describes the 2D (one floor)
* bounding-box for one node on the grid
*/
struct GridNodeBBox {
/** smaller half */
float x1_cm, y1_cm;
/** larger half */
float x2_cm, y2_cm;
struct GridNodeBBox : public BBox2 {
/** ctor */
GridNodeBBox(const GridPoint& center, const int gridSize_cm) {
x1_cm = center.x_cm - gridSize_cm/2; // smaller half
y1_cm = center.y_cm - gridSize_cm/2;
x2_cm = center.x_cm + gridSize_cm/2; // larger half
y2_cm = center.y_cm + gridSize_cm/2;
}
/** equal? */
bool operator == (const GridNodeBBox& o) const {
return (x1_cm == o.x1_cm) && (y1_cm == o.y1_cm) && (x2_cm == o.x2_cm) && (y2_cm == o.y2_cm);
}
/** does the BBox intersect with the given line? */
bool intersects (const Line2D& l) const {
Line2D l1(x1_cm, y1_cm, x2_cm, y1_cm); // upper
Line2D l2(x1_cm, y2_cm, x2_cm, y2_cm); // lower
Line2D l3(x1_cm, y1_cm, x1_cm, y2_cm); // left
Line2D l4(x2_cm, y1_cm, x2_cm, y2_cm); // right
return l.getSegmentIntersection(l1) ||
l.getSegmentIntersection(l2) ||
l.getSegmentIntersection(l3) ||
l.getSegmentIntersection(l4);
p1.x = center.x_cm - gridSize_cm/2; // smaller half
p1.y = center.y_cm - gridSize_cm/2;
p2.x = center.x_cm + gridSize_cm/2; // larger half
p2.y = center.y_cm + gridSize_cm/2;
}
};

6
grid/GridPoint.h
View File

@@ -6,13 +6,13 @@
struct GridPoint {
/** x-position (in centimeter) */
const float x_cm;
float x_cm;
/** y-position (in centimeter) */
const float y_cm;
float y_cm;
/** z-position (in centimeter) */
const float z_cm;
float z_cm;
/** empty ctor */

182
grid/factory/GridFactory.h
View File

@@ -3,6 +3,8 @@
#include <string>
#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.h"
#include "../../geo/Units.h"
#include "../GridNodeBBox.h"
#include "../Grid.h"
@@ -22,6 +24,8 @@ public:
/** add the given floor at the provided height (in cm) */
void addFloor(const Floor& floor, const float z_cm) {
// build grid-points
for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
@@ -35,7 +39,181 @@ public:
}
}
int i = 0;
connectAdjacent(z_cm);
}
void connectAdjacent(const float z_cm) {
// connect adjacent grid-points
for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
T& n1 = (T&) grid[idx];
if (n1.z_cm != z_cm) {continue;} // ugly... different floor -> skip
// square around each point
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
int ox = n1.x_cm + x;
int oy = n1.y_cm + y;
GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
if (grid.hasNodeFor(p)) {
T& n2 = (T&) grid.getNodeFor(p);
grid.connectUniDir(n1, n2);
}
}
}
}
}
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
for (const Stair& s : stairs) {
for (int i = 0; i < grid.getNumNodes(); ++i) {
// potential starting-point for the stair
T& n = (T&) grid[i];
// real starting point for the stair?
if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grap? -> construct stair
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStair(n, n2);
}
int i = 0;
}
}
}
}
/** build a stair (z-transition) from n1 to n2 */
void buildStair(T& n1, T& n2) {
//TODO: ensure n1 is below n2
const float zDiff = n2.z_cm - n1.z_cm;
const float xDiff = n2.x_cm - n1.x_cm;
const float yDiff = n2.y_cm - n1.y_cm;
int idx1 = n1.getIdx();
int idx2 = -1;
const int idx3 = n2.getIdx();
// move upards in gridSize steps
for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
// calculate the percentage of reached upwards-distance
const float percent = z/zDiff;
// adjust (x,y) accordingly (interpolate)
int x = n1.x_cm + xDiff * percent;
int y = n1.y_cm + yDiff * percent;
// snap (x,y) to the grid???
//x = std::round(x / gridSize_cm) * gridSize_cm;
//y = std::round(y / gridSize_cm) * gridSize_cm;
// create a new node add it to the grid, and connect it with the previous one
idx2 = grid.addUnaligned(T(x,y,z));
grid.connectBiDir(idx1, idx2);
idx1 = idx2;
}
// add the last segment
if (idx2 != -1) {
grid.connectBiDir(idx2, idx3);
}
}
/** add the inverted version of the given z-layer */
void addInverted(const Grid<gridSize_cm, T>& gIn, const float z_cm) {
// get the original grid's bbox
BBox3 bb = gIn.getBBox();
// build new grid-points
for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
// does the input-grid contain such a point?
GridPoint gp(x_cm, y_cm, z_cm);
if (gIn.hasNodeFor(gp)) {continue;}
// add to the grid
grid.add(T(x_cm, y_cm, z_cm));
}
}
}
// TODO: how to determine the starting index?!
// IDEAS: find all segments:
// start at a random point, add all connected points to the set
// start at a NEW random point ( not part of the already processed points), add connected points to a new set
// repeat until all points processed
// how to handle multiple floor layers?!?!
// run after all floors AND staircases were added??
// OR: random start, check segment size, < 50% of all nodes? start again
void removeIsolated() {
// get largest connected region
std::set<int> set;
do {
const int idxStart = rand() % grid.getNumNodes();
set.clear();
getConnected(idxStart, set);
} while (set.size() < 0.5 * grid.getNumNodes());
// remove all other
for (int i = 0; i < grid.getNumNodes(); ++i) {
if (set.find(i) == set.end()) {grid.remove(i);}
}
// clean the grid
grid.cleanup();
}
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];
set.insert(n1.getIdx());
for (T& n2 : grid.neighbors(n1)) {
if (set.find(n2.getIdx()) == set.end()) {
getConnected(n2.getIdx(), set);
}
}
}
@@ -43,7 +221,7 @@ private:
/** does the bbox intersect with any of the floor's walls? */
bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
for (const Line2D l : floor.getObstacles()) {
for (const Line2& l : floor.getObstacles()) {
if (bbox.intersects(l)) {return true;}
}
return false;

84
grid/factory/GridImportance.h Normal file
View File

@@ -0,0 +1,84 @@
#ifndef GRIDIMPORTANCE_H
#define GRIDIMPORTANCE_H
#include "../Grid.h"
#include "GridFactory.h"
#include "../../misc/KNN.h"
#include <KLib/math/distribution/Normal.h>
class GridImportance {
public:
/** attach importance-factors to the grid */
template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
// get an inverted version of the grid
Grid<gridSize_cm, T> inv;
GridFactory<gridSize_cm, T> fac(inv);
fac.addInverted(g, z_cm);
// construct KNN search
KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
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];
float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
knn.get(point, 10, indices, squaredDist);
const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
const float imp2 = door( indices );
n1.imp = (imp1 + imp2)/2;
}
}
float door( size_t* indices ) {
// build covariance
//if (dist1_m > 1.0) {return 1;}
//return 1.0 - std::abs(dist1_m - dist2_m);
return 1;
}
float importance(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;
// static K::NormalDistribution d1(1.0, 0.75);
// //static K::NormalDistribution d2(3.0, 0.75);
// 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;}
}
};
#endif // GRIDIMPORTANCE_H