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

4
CMakeLists.txt
View File

@@ -36,6 +36,7 @@ FILE(GLOB SOURCES
./*.cpp
./*/*.cpp
./*/*/*.cpp
./*/*/*/*.cpp
../KLib/inc/tinyxml/*.cpp
)
@@ -59,8 +60,9 @@ ADD_DEFINITIONS(
-Wall
-Werror=return-type
-Wextra
-Wpedantic
-g
-g3
-O0
-DWITH_TESTS

12
geo/Point3.h
View File

@@ -24,6 +24,18 @@ struct Point3 {
Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
float length() const {return std::sqrt(x*x + y*y + z*z);}
float length(const float norm) const {
return std::pow(
(std::pow(std::abs(x),norm) +
std::pow(std::abs(y),norm) +
std::pow(std::abs(z),norm)
), 1.0f/norm);
}
};
#endif // POINT3_H

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

2
main.cpp
View File

@@ -15,7 +15,7 @@ int main(int argc, char** argv) {
#ifdef WITH_TESTS
::testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "*bbox*";
::testing::GTEST_FLAG(filter) = "*TestAll*";
return RUN_ALL_TESTS();
#endif

36
math/MiniMat2.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef MINIMAT2_H
#define MINIMAT2_H
/**
* very simple 2x2 matrix
*/
struct MiniMat2 {
/** store eigenvalues */
struct EV {float e1,e2;};
/** data */
float a,b,c,d;
/** ctor */
MiniMat2() : a(0), b(0), c(0), d(0) {;}
/** get the matrix' eigenvalues */
EV getEigenvalues() const {
const float T = a+d;
const float D = a*d - b*c;
EV res;
res.e1 = T/2 + std::sqrt(T*T/4-D);
res.e2 = T/2 - std::sqrt(T*T/4-D);
return res;
}
/** add (x,y) * (x,y)T to the matrix */
void addSquared(const float x, const float y) {
a += (x*x); b += (x*y);
c += (x*y); d += (y*y);
}
};
#endif // MINIMAT2_H

30
misc/Debug.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef DEBUG_H
#define DEBUG_H
#include <string>
#include <iostream>
#include <iomanip>
class Log {
public:
static void add(const char* comp, const std::string what) {
addComp(comp);
std::cout << what << std::endl;
}
static void add(const std::string& component, const std::string what) {
addComp(component.c_str());
std::cout << what << std::endl;
}
private:
static void addComp(const char* component) {
std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
}
};
#endif // DEBUG_H

129
nav/dijkstra/Dijkstra.h Normal file
View File

@@ -0,0 +1,129 @@
#ifndef DIJKSTRA_H
#define DIJKSTRA_H
#include <vector>
#include <algorithm>
#include <unordered_set>
#include "DijkstraStructs.h"
#include "../../misc/Debug.h"
#include <KLib/Assertions.h>
template <typename T> class Dijkstra {
/** all allocated nodes for the user-data inputs */
std::unordered_map<const T*, DijkstraNode<T>*> nodes;
/** all already processed edges */
std::unordered_set<DijkstraEdge<T>> usedEdges;
/** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */
std::vector<DijkstraNode<T>*> toBeProcessedNodes;
public:
/** get (or create) a new node for the given user-node */
DijkstraNode<T>* getNode(const T* userNode) {
if (nodes.find(userNode) == nodes.end()) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
nodes[userNode] = dn;
}
return nodes[userNode];
}
/** get the edge (bi-directional) between the two given nodes */
DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {
return DijkstraEdge<T>(n1, n2);
}
/** get the dijkstra-pendant for the given user-node */
DijkstraNode<T>* getNode(const T& userNode) {
return nodes[&userNode];
}
/** build shortest path from start to end using the provided wrapper-class */
template <typename Access> void build(const T& start, const T& end, const Access& acc) {
// NOTE: end is currently ignored!
// runs until all nodes were evaluated
Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start);
// cleanup
toBeProcessedNodes.clear();
usedEdges.clear();
nodes.clear();
// run from start
const T* cur = &start;
// create a node for the start element
DijkstraNode<T>* dnStart = getNode(cur);
dnStart->cumWeight = 0;
// add this node to the processing list
toBeProcessedNodes.push_back(dnStart);
// until we are done
while(!toBeProcessedNodes.empty()) {
// get the next to-be-processed node
DijkstraNode<T>* dnSrc = toBeProcessedNodes[0];
// and remove him from the list
toBeProcessedNodes.erase(toBeProcessedNodes.begin());
// process each neighbor of the current element
for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
// get the neighbor itself
const T* dst = acc.getNeighbor(*dnSrc->element, i);
// get the distance-weight to the neighbor
const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
_assertTrue(weight >= 0, "edge-weight must not be negative!");
// get-or-create a node for the neighbor
DijkstraNode<T>* dnDst = getNode(dst);
// get-or-create the edge describing the connection
DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
// was this edge already processed? -> skip it
if (usedEdges.find(edge) != usedEdges.end()) {continue;}
// otherwise: remember it
usedEdges.insert(edge);
// and add the node for later processing
toBeProcessedNodes.push_back(dnDst);
// update the weight to the destination?
const float potentialWeight = dnSrc->cumWeight + weight;
if (potentialWeight < dnDst->cumWeight) {
dnDst->cumWeight = potentialWeight;
dnDst->previous = dnSrc;
}
}
// sort the nodes by distance-from-start (shortest first)
auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
std::sort(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
}
Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
// cleanup
toBeProcessedNodes.clear();
usedEdges.clear();
}
};
#endif // DIJKSTRA_H

77
nav/dijkstra/DijkstraStructs.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef DIJKSTRANODE_H
#define DIJKSTRANODE_H
/**
* wrapper around a user data structure
* adds additional fields needed for dijkstra calculation
*/
template <typename T> struct DijkstraNode {
/** pos infinity */
static constexpr float INF = +99999999;
/** the user-element this node describes */
const T* element;
/** the previous dijkstra node (navigation path) */
DijkstraNode<T>* previous;
/** the weight from the start up to this element */
float cumWeight;
// /** ctor */
// DijkstraNode() : element(nullptr), previous(), cumWeight(INF) {;}
/** ctor */
DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
/** equal? (bi-dir) */
bool operator == (const DijkstraNode<T>& other) {
return element == other.element;
}
};
/**
* data structure describing the connection between two nodes
* only used to track already processed connections!
*/
template <typename T> struct DijkstraEdge {
/** the edge's source */
const DijkstraNode<T>* src;
/** the edge's destination */
const DijkstraNode<T>* dst;
/** ctor */
DijkstraEdge(const DijkstraNode<T>* src, const DijkstraNode<T>* dst) : src(src), dst(dst) {;}
/** equal? (bi-dir) */
bool operator == (const DijkstraEdge& other) const {
return ((dst == other.dst) && (src == other.src)) ||
((src == other.dst) && (dst == other.src));
}
};
//template <typename T> struct DijkstraEdgeWeighted : public DijkstraEdge<T> {
// /** the edge's weight */
// float weight;
// DijkstraEdgeWeighted(const DijkstraNode<T>* src, const DijkstraNode<T>* dst, const float weight) : DijkstraEdge<T>(src,dst), weight(weight) {;}
//};
namespace std {
template <typename T> struct hash<DijkstraEdge<T>>{
size_t operator()(const DijkstraEdge<T>& e) const {
return hash<size_t>()( (size_t)e.src^(size_t)e.dst);
}
};
}
#endif // DIJKSTRANODE_H

2
tests/Tests.h
View File

@@ -6,7 +6,7 @@
#include <gtest/gtest.h>
static inline std::string getDataFile(const std::string& name) {
return "/apps/workspaces/Indoor/tests/data/" + name;
return "/mnt/data/workspaces/Indoor/tests/data/" + name;
}
#endif

26
tests/floorplan/TestFloorplanFactorySVG.cpp
View File

@@ -1,26 +1,26 @@
#ifdef WITH_TESTS
//#include "../Tests.h"
//#include "../../floorplan/FloorplanFactorySVG.h"
//#include <cstdlib>
#include "../Tests.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include <cstdlib>
//TEST(FloorplanFactorySVG, parse) {
TEST(FloorplanFactorySVG, parse) {
// const std::string filename = getDataFile("test.svg");
// FloorplanFactorySVG factory(filename, 1.0);
const std::string filename = getDataFile("test.svg");
FloorplanFactorySVG factory(filename, 1.0);
// Floor f1 = factory.getFloor("1");
// ASSERT_EQ(30, f1.getObstacles().size());
Floor f1 = factory.getFloor("1");
ASSERT_EQ(30, f1.getObstacles().size());
// Floor f2 = factory.getFloor("2");
// ASSERT_EQ(30, f2.getObstacles().size());
Floor f2 = factory.getFloor("2");
ASSERT_EQ(30, f2.getObstacles().size());
// Floor f3 = factory.getFloor("1_2");
// ASSERT_EQ(12, f3.getObstacles().size());
Floor f3 = factory.getFloor("1_2");
ASSERT_EQ(12, f3.getObstacles().size());
//}
}

2
tests/grid/GridImportance.cpp
View File

@@ -22,7 +22,7 @@ TEST(GridImportance, a) {
GridImportance gi;
gi.addImportance(g, 20);
plot(g);
Plot p; p.build(g).fire();
}

69
tests/grid/Plot.h
View File

@@ -1,6 +1,8 @@
#ifndef PLOT_H
#define PLOT_H
#include "../../grid/Grid.h"
#include <set>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
@@ -18,49 +20,62 @@ public:
};
template <int gridSize_cm, typename T> void plot(Grid<gridSize_cm, T>& g) {
class Plot {
public:
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementColorPoints points;
K::GnuplotSplotElementLines lines;
gp << "set ticslevel 0\n";
gp << "set view equal xyz\n";
gp << "set cbrange[0.5:1.0]\n";
gp << "set palette gray negative\n";
template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
std::set<uint64_t> done;
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";
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);
std::set<uint64_t> done;
for (int n = 0; n < n1.getNumNeighbors(); ++n) {
const GP& n2 = n1.getNeighbor(n, g);
if (done.find(g.getUID(n2)) == 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);
++cnt;
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);
for (const T& n2 : g.neighbors(n1)) {
//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
// const GP& n2 = n1.getNeighbor(n, g);
if (done.find(g.getUID(n2)) == 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);
++cnt;
}
}
done.insert(g.getUID(n1));
}
done.insert(g.getUID(n1));
points.setPointSize(1);
//splot.add(&lines);
splot.add(&points);
return *this;
}
points.setPointSize(1);
//splot.add(&lines);
splot.add(&points);
Plot& fire() {
gp.draw(splot);
gp.flush();
sleep(1000);
return *this;
}
gp.draw(splot);
gp.flush();
sleep(100);
}
};
#endif // PLOT_H

69
tests/grid/TestAll.cpp Normal file
View File

@@ -0,0 +1,69 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../grid/factory/GridImportance.h"
#include "../../grid/factory/GridFactory.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "Plot.h"
TEST(TestAll, Nav) {
Grid<20, GP> g;
// 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.5);
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
}
} tmp(g);
GridFactory<20, GP> gf(g);
FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
Floor f1 = fpf.getFloor("1");
Floor f2 = fpf.getFloor("2");
Stairs s1_2 = fpf.getStairs("1_2");
gf.addFloor(f1, 20);
gf.addFloor(f2, 340);
gf.addStairs(s1_2, 20, 340);
gf.removeIsolated();
GridImportance gi;
gi.addImportance(g, 20);
gi.addImportance(g, 340);
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));
d.build(start, end, tmp);
// 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));
dn = dn->previous;
}
Plot p;
p.build(g);
p.splot.add(&path);
p.fire();
}
#endif

18
tests/grid/TestGrid.cpp
View File

@@ -1,17 +1,11 @@
#ifdef WITH_TESTS
#include "Plot.h"
#include "../Tests.h"
#include "../../grid/Grid.h"
#include "../../grid/GridPoint.h"
#include "../../grid/GridNode.h"
class GP : public GridNode, public GridPoint {
public:
GP() : GridNode(), GridPoint() {;}
GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
};
TEST(Grid, add) {
Grid<20, GP> grid;
@@ -191,11 +185,11 @@ TEST(Grid, bbox) {
Grid<1, GP> grid;
int idx1 = grid.add(GP( 0, 0, 0));
int idx2 = grid.add(GP( 0, 1, 0));
int idx3 = grid.add(GP( 0,-1, 0));
int idx4 = grid.add(GP( 1, 0, 0));
int idx5 = grid.add(GP(-1, 0, 0));
grid.add(GP( 0, 0, 0));
grid.add(GP( 0, 1, 0));
grid.add(GP( 0,-1, 0));
grid.add(GP( 1, 0, 0));
grid.add(GP(-1, 0, 0));
BBox3 bb = grid.getBBox();

2
tests/grid/TestGridFactory.cpp
View File

@@ -28,7 +28,7 @@ TEST(GridFactory, create) {
gfInv.addInverted(g, 20);
gfInv.addInverted(g, 340);
// plot(gInv);
//plot(gInv);
}

51
tests/nav/dijkstra/TestDijkstra.cpp Normal file
View File

@@ -0,0 +1,51 @@
#ifdef WITH_TESTS
#include "../../Tests.h"
#include "../../../grid/Grid.h"
#include "../../../nav/dijkstra/Dijkstra.h"
#include "../../grid/Plot.h"
TEST(Dijkstra, build) {
Grid<1, GP> grid;
int idx1 = grid.add(GP( 0, 0, 0));
int idx2 = grid.add(GP( 0, 1, 0));
int idx3 = grid.add(GP( 0,-1, 0));
int idx4 = grid.add(GP( 1, 0, 0));
int idx5 = grid.add(GP(-1, 0, 0));
grid.connectBiDir(idx1, idx2);
grid.connectBiDir(idx1, idx3);
grid.connectBiDir(idx1, idx4);
grid.connectBiDir(idx1, idx5);
class TMP {
Grid<1, GP>& grid;
public:
TMP(Grid<1, 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 {return ((Point3)n1 - (Point3)n2).length();}
} tmp(grid);
Dijkstra<GP> d;
d.build(grid[idx5], grid[idx3], tmp);
// start node must be "idx5"
DijkstraNode<GP>* n = d.getNode(grid[idx5]);
ASSERT_EQ(&grid[idx5], n->element); ASSERT_EQ(nullptr, n->previous); ASSERT_EQ(0, n->cumWeight);
// "idx1" (the center) is reached via idx5
DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
ASSERT_EQ(&grid[idx1], n2->element); ASSERT_EQ(&grid[idx5], n2->previous->element);
// "idx3" (the target) is reached via idx1 (the center)
DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
ASSERT_EQ(&grid[idx3], n3->element); ASSERT_EQ(&grid[idx1], n3->previous->element);
}
#endif