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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
FrankE
2016-11-29 21:29:40 +01:00
parent 272382ae3d 7f53f83da0
commit 2255904385
77 changed files with 1818 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Assertions.h
View File

@@ -64,7 +64,10 @@ namespace Assert {
}
template <typename T, typename STR> static inline void isNear(const T v1, const T v2, const T delta, const STR err) {
if (std::abs(v1-v2) > delta) {doThrow(err);}
if (std::abs(v1-v2) > delta) {
std::stringstream ss; ss << "\nexpected " << v1 << " +/- " << delta << " but is " << v2 << "\n";
doThrow(err+ss.str());
}
}
template <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {

13
Exception.h
View File

@@ -4,6 +4,10 @@
#include <exception>
#include <string>
#ifdef ANDROID
#include <QMessageBox>
#endif
class Exception : public std::exception {
private:
@@ -14,7 +18,14 @@ private:
public:
/** ctor */
Exception(const std::string& str) : str(str) {;}
Exception(const std::string& str) : str(str) {
// TODO better solution?
#ifdef ANDROID
QMessageBox::question(nullptr, "Exception", str.c_str(), QMessageBox::Ok);
#endif
}
const char* what() const throw() {return str.c_str();}

30
data/Timestamp.h
View File

@@ -1,6 +1,8 @@
#ifndef TIMESTAMP_H
#define TIMESTAMP_H
#include <chrono>
/**
* helper-class to handle timestamps
*/
@@ -25,16 +27,32 @@ public:
/** get timestamp from the given value which represents seconds */
static inline Timestamp fromSec(const float sec) {return Timestamp(sec*1000);}
/** get timestamp for the current unix-time */
static inline Timestamp fromUnixTime() {
auto now = std::chrono::system_clock::now();
auto duration = now.time_since_epoch();
auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
return Timestamp(millis);
}
/** get timestamp for the current system-time */
static inline Timestamp fromRunningTime() {
static Timestamp startup = fromUnixTime();
return fromUnixTime() - startup;
}
public:
/** get timestamp in milliseconds */
inline uint64_t ms() const {return _ms;}
inline int64_t ms() const {return _ms;}
/** get timestamp in seconds */
inline float sec() const {return _ms/1000.0f;}
public:
/** is this timestamp zero? */
@@ -48,17 +66,23 @@ public:
/** smaller than the given one? */
bool operator < (const Timestamp& o) const {return _ms < o._ms;}
bool operator <= (const Timestamp& o) const {return _ms <= o._ms;}
/** greater than the given one? */
bool operator > (const Timestamp& o) const {return _ms > o._ms;}
bool operator >= (const Timestamp& o) const {return _ms >= o._ms;}
Timestamp operator - (const Timestamp& o) const {return Timestamp(_ms - o._ms);}
Timestamp operator + (const Timestamp& o) const {return Timestamp(_ms + o._ms);}
/** cast to float */
operator float () const {return sec();}
Timestamp operator * (const float val) const {return Timestamp(_ms * val);}
// /** cast to float */
// operator float () const {return sec();}
};

21
floorplan/v2/FloorplanHelper.h
View File

@@ -14,6 +14,27 @@ class FloorplanHelper {
public:
/** align all floorplan values to the given grid size. needed for the grid factory */
static void align(Floorplan::IndoorMap* map, const int gridSize_cm) {
for (Floorplan::Floor* floor : map->floors) {
floor->atHeight = align_m(floor->atHeight, gridSize_cm);
floor->height = align_m(floor->height, gridSize_cm);
for (Floorplan::Stair* stair : floor->stairs) {
for (Floorplan::StairPart& part : ((Floorplan::StairFreeform*)stair)->parts) {
part.start.z = align_m(part.start.z, gridSize_cm);
part.end.z = align_m(part.end.z, gridSize_cm);
}
}
}
}
static inline float align_m(const float val_m, const int gridSize_cm) {
const float val_cm = val_m * 100;
const int snapped = std::round(val_cm / gridSize_cm);
const float res_cm = snapped * gridSize_cm;
return res_cm / 100.0f;
}
/** get a BBox for the whole map */
static BBox3 getBBox(const Floorplan::IndoorMap* map) {

10
floorplan/v2/FloorplanReader.h
View File

@@ -5,6 +5,7 @@
#include "Floorplan.h"
#include "../../misc/Debug.h"
#include "../../Assertions.h"
#include "../../lib/tinyxml/tinyxml2.h"
@@ -18,10 +19,15 @@ namespace Floorplan {
*/
class Reader {
private:
static constexpr const char* name = "FPReader";
public:
/** read an IndoorMap from the given XML-file */
static IndoorMap* readFromFile(const std::string& file) {
Log::add(name, "reading floorplan from file: " + file);
setlocale(LC_NUMERIC, "C");
tinyxml2::XMLDocument doc;
const tinyxml2::XMLError res = doc.LoadFile(file.c_str());
@@ -31,6 +37,7 @@ namespace Floorplan {
/** read an IndoorMap from the given XMl-string */
static IndoorMap* readFromString(const std::string& str) {
Log::add(name, "reading floorplan from string");
setlocale(LC_NUMERIC, "C");
tinyxml2::XMLDocument doc;
const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length());
@@ -54,7 +61,7 @@ namespace Floorplan {
/** parse the <map> node */
static IndoorMap* parseMap(const XMLElem* el) {
std::cout << el->Name() << std::endl;
Log::add(name, "parsing the map");
IndoorMap* map = new IndoorMap();
map->width = el->FloatAttribute("width");
map->depth = el->FloatAttribute("depth");
@@ -76,6 +83,7 @@ namespace Floorplan {
/** parse one <floor> node */
static Floor* parseFloor(const XMLElem* el) {
Floor* floor = new Floor();
Log::add(name, std::string("parsing floor ") + el->Attribute("name"));
floor->atHeight = el->FloatAttribute("atHeight");
floor->height = el->FloatAttribute("height");
floor->name = el->Attribute("name");

30
geo/Angle.h
View File

@@ -5,6 +5,8 @@
#include "../Assertions.h"
#include "Point2.h"
#define PI ((float) M_PI)
struct Angle {
@@ -14,7 +16,7 @@ public:
static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) {
Assert::isFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!");
const float tmp = std::atan2(y2-y1, x2-x1);
return (tmp < 0) ? (tmp + 2*M_PI) : (tmp);
return (tmp < 0) ? (tmp + 2*PI) : (tmp);
}
/** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */
@@ -34,10 +36,10 @@ public:
* - as a change-in-direction between [0:PI]
*/
static float getDiffRAD_2PI_PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds");
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds");
Assert::isBetween(r1, 0.0f, (2*PI), "r1 out of bounds");
Assert::isBetween(r2, 0.0f, (2*PI), "r2 out of bounds");
float tmp = std::abs(r1-r2);
return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp);
return (tmp <= PI) ? (tmp) : (2*PI-tmp);
}
/**
@@ -46,23 +48,23 @@ public:
* - as a change-in-direction between [-PI:+PI]
*/
static float getSignedDiffRAD_2PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); // [0:360] deg
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); // [0:360] deg
Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg
Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg
float diff = r1-r2;
if (diff > +M_PI) {diff = -(2*M_PI - diff);}
else if (diff < -M_PI) {diff = +(2*M_PI + diff);}
Assert::isBetween(diff, (float)-M_PI, (float)(+M_PI), "result out of bounds"); // [-180:+180] deg
if (diff > +PI) {diff = -(2*PI - diff);}
else if (diff < -PI) {diff = +(2*PI + diff);}
Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg
return diff;
}
/** convert degrees to radians */
static constexpr float degToRad(const float deg) {
return deg / 180.0f * M_PI;
static constexpr inline float degToRad(const float deg) {
return deg / 180.0f * PI;
}
/** convert radians to degrees */
static float radToDeg(const float rad) {
return rad * 180.0f / M_PI;
static constexpr inline float radToDeg(const float rad) {
return rad * 180.0f / PI;
}
/** get a pointer vector (length 1) pointing to the given angle (in radians) */
@@ -74,4 +76,6 @@ public:
};
#undef PI
#endif // ANGLE_H

5
geo/BBox2.h
View File

@@ -36,6 +36,11 @@ public:
}
/** returns true if the bbox is not yet configured */
const bool isInvalid() const {
return p1.x == MAX || p1.y == MAX || p2.x == MIN || p2.y == MIN;
}
/** get the bbox's minimum */
const Point2& getMin() const {return p1;}

10
geo/Heading.h
View File

@@ -8,7 +8,7 @@
struct Heading {
#define _2PI (2*M_PI)
#define _2PI (2*(float)M_PI)
private:
@@ -105,10 +105,10 @@ public:
};
namespace Headings {
static const Heading RIGHT = Heading(M_PI*0/2);
static const Heading UP = Heading(M_PI*1/2);
static const Heading LEFT = Heading(M_PI*2/2);
static const Heading DOWN = Heading(M_PI*3/2);
static const Heading RIGHT = Heading((float)M_PI*0.0f/2.0f);
static const Heading UP = Heading((float)M_PI*1.0f/2.0f);
static const Heading LEFT = Heading((float)M_PI*2.0f/2.0f);
static const Heading DOWN = Heading((float)M_PI*3.0f/2.0f);
}
#endif // HEADING_H

4
geo/Point3.h
View File

@@ -105,6 +105,10 @@ struct Point3 {
), 1.0f/norm);
}
std::string asString() const {
return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + ")";
}
private:
static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;}

33
grid/Grid.h
View File

@@ -6,6 +6,8 @@
#include <unordered_map>
#include <algorithm>
#include "../Assertions.h"
#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"
@@ -47,8 +49,11 @@ public:
/** ctor with the grid's size (in cm) */
Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
//static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
//static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
StaticAssert::AinheritsB<T, GridNode>(); // "T must inherit from GridNode!"
StaticAssert::AinheritsB<T, GridPoint>(); // "T must inherit from GridPoint!"
Log::add(name, "empty grid with " + std::to_string(gridSize_cm) + "cm grid-size");
}
/** no-copy */
@@ -395,11 +400,13 @@ public:
/** serialize into the given stream */
void write(std::ostream& out) {
// serialize static
T::staticSerialize(out);
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
const int nodeSize = sizeof(T);
out.write((const char*) &nodeSize, sizeof(nodeSize));
// number of nodes
const int numNodes = nodes.size();
Assert::isTrue(numNodes > 0, "grid says it contains 0 nodes. there must be some error!");
out.write((const char*) &numNodes, sizeof(numNodes));
// serialize
@@ -407,23 +414,37 @@ public:
out.write((const char*) &node, sizeof(T));
}
// serialize static parameters
T::staticSerialize(out);
out.flush();
}
/** deserialize from the given stream */
void read(std::istream& inp) {
// deserialize static
T::staticDeserialize(inp);
Log::add(name, "loading grid from input-stream");
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
int nodeSize;
inp.read((char*) &nodeSize, sizeof(nodeSize));
Assert::equal(nodeSize, (int)sizeof(T), "sizeof(node) of the saved grid does not match sizeof(node) for the code!");
// number of nodes
int numNodes;
inp.read((char*) &numNodes, sizeof(numNodes));
Assert::isTrue(numNodes > 0, "grid-file says it contains 0 nodes. there must be some error!");
// allocate node-space
nodes.resize(numNodes);
// deserialize
inp.read((char*) nodes.data(), numNodes*sizeof(T));
Log::add(name, "deserialized " + std::to_string(nodes.size()) + " nodes");
// deserialize static parameters
T::staticDeserialize(inp);
// update
rebuildHashes();

19
grid/GridPoint.h
View File

@@ -67,7 +67,12 @@ struct GridPoint {
/** cast to string */
operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
operator std::string() const {return asString();}
/** get as string */
std::string asString() const {
return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";
}
/** read-only array access */
float operator [] (const int idx) const {
@@ -79,4 +84,16 @@ struct GridPoint {
};
namespace std {
template<> struct hash<GridPoint> {
int64_t operator() (const GridPoint& gp) const {
return
(((int64_t)gp.x_cm) << 0) +
(((int64_t)gp.y_cm) << 8) +
(((int64_t)gp.z_cm) << 16);
}
};
}
#endif // GRIDPOINT_H

2
grid/factory/GridImportance.h
View File

@@ -130,7 +130,7 @@ public:
}
template <typename T> void addImportance(Grid<T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
template <typename T> void addImportance(Grid<T>& g, const DijkstraNode<T>* start, const DijkstraNode<T>* end) {
// routing path
DijkstraPath<T> path(end, start);

1
grid/factory/v2/Elevators.h
View File

@@ -40,7 +40,6 @@ public:
const int gs_cm = grid.getGridSize_cm();
struct IntPos {
int x_cm;
int y_cm;

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

@@ -136,7 +136,7 @@ public:
/** add the given floor to the grid */
void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
Log::add(name, "adding floor " + floor->name, true);
Log::add(name, "adding floor '" + floor->name + "'", true);
if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}
const BBox2 bbox = getFloorOutlineBBox(floor->outline);
@@ -148,6 +148,7 @@ public:
const int total = (x2-x1) / helper.gridSize();
int cur = 0;
int numNodes = 0;
// build grid-points for floor-outline
for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
@@ -169,10 +170,17 @@ public:
if (grid.hasNodeFor(t)) {continue;}
grid.add(t);
// debug
++numNodes;
}
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
Log::add(name, "added " + std::to_string(numNodes) + " nodes");
// connect the g
connectAdjacent(floor, z_cm);
@@ -225,9 +233,9 @@ public:
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const Floorplan::Floor* floor, const float z_cm) {
void connectAdjacent(const Floorplan::Floor* floor, const int z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::add(name, "connecting all adjacent nodes within floor '" + floor->name + "' (atHeight: " + std::to_string(z_cm) + "cm)", false);
Log::tick();
// connect adjacent grid-points

6
grid/factory/v2/GridNodeImportance.h
View File

@@ -10,6 +10,12 @@ struct GridNodeImportance {
/** get the node's nav importance */
float getNavImportance() const {return navImportance;}
/** importance-weight for random walks */
float walkImportance;
/** get the node's random-walk importance */
float getWalkImportance() const {return walkImportance;}
/** ctor */
GridNodeImportance() : navImportance(1.0f) {;}

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

@@ -86,8 +86,8 @@ public:
KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Triangle<float> avoidWalls(0.0, 0.35f);
Distribution::Normal<float> favorDoors(0.0f, 0.4f);
Distribution::Normal<float> favorStairs(0.0f, 1.5f);
if (l) {
@@ -111,8 +111,8 @@ public:
// 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} ));
// get the distance to the nearest outline node [an outline node is directly adjacent to a wall]
const float distToOutline_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
// get the distance to the nearest door
const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
@@ -120,20 +120,41 @@ public:
// get the distance to the nearest stair
const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);
// use wall-avoidance?
//const bool useWallAvoidance = (distToWall_m*6.0 < distToDoor_m && distToWall_m*6.0 < distToStair_m);
//const bool useWallAvoidance = (distToDoor_m > 0.4f) && (distToStair_m > 0.4f);
const bool useWallAvoidance =
(distToOutline_m < 0.001f) && // node is an outline node [outline-nodes are adjacent to a wall]
(distToDoor_m > 0.3f) && // doors are 30cm away
(distToStair_m > 0.6f); // stairs are 60cm away;
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
n1.walkImportance = 1.0f;
//n1.walkImportance += favorDoors.getProbability(distToDoor_m) * 0.75f;
n1.walkImportance += favorStairs.getProbability(distToStair_m) * 1.0f;
// use wall avoidance
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
if (useWallAvoidance) {
//n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.3f;
n1.walkImportance = 0.20f; // only addresses direct outline nodes
}
// navigation importance is calculated using other formulae
n1.navImportance = 1.0f;
if (useWallAvoidance) {
n1.navImportance -= avoidWalls.getProbability(distToOutline_m) * 0.3f;
}
//n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0f;
// sanity check
Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");
}

4
grid/factory/v2/Stairs2.h
View File

@@ -112,7 +112,7 @@ public:
const Point3 p4 = quad.p4 * 100;
// get the z-value from one of the both triangles
int z_cm;
// int z_cm;
float u,v,w;
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
sn.z_cm = p1.z*u + p2.z*v + p3.z*w;
@@ -124,7 +124,7 @@ public:
}
// this might lead to stairs the start slightly above the starting-floor
// or end slightly below the ending floor. this would lead to DISCONNECTION!
// or ending slightly below the ending floor. this would lead to DISCONNECTION!
// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
float minZ = +9999999;
float maxZ = -9999999;

59
grid/walk/v2/GridWalker.h
View File

@@ -1,9 +1,12 @@
#ifndef GRIDWALKER_H
#define GRIDWALKER_H
#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "modules/WalkModule.h"
#include "../../../math/Distributions.h"
#include "../../../math/Stats.h"
/**
* modular grid-walker that takes various sub-components to determine
@@ -16,7 +19,7 @@ private:
/** all modules to evaluate */
std::vector<WalkModule<Node, WalkState>*> modules;
DrawList<const Node*> drawer;
RandomGenerator rnd;
public:
@@ -26,14 +29,26 @@ public:
}
/** perform the walk based on the configured setup */
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float _dist_m) {
double tmp; // ignored
return getDestination(grid, _startState, _dist_m, tmp);
}
/**
* perform the walk based on the configured setup
* returns the walks overall probability using the out-parameter
*/
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float _dist_m, double& probability) {
Assert::isTrue(_dist_m >= 0, "walk distance must not be negative!");
Assert::isTrue(_dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");
// keep the starting state for reference
//const WalkState startState = _startState;
// the current state that is modified for each step
WalkState currentState = _startState;
updateBefore(currentState);
// get the node that corresponds to start;
const Node* startNode = grid.getNodePtrFor(currentState.position);
@@ -42,19 +57,39 @@ public:
// currently examined node
const Node* curNode = startNode;
// perform initial update
updateBefore(currentState, *curNode);
// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
float dist_m = _dist_m + _startState.distance.error_m;
Stats::Maximum<double> maxEdgeProb;
// until distance is reached
while (dist_m > 0) {
drawer.reset();
// only needed for a global (no-thread-safe) drawer
//drawer.reset();
// the rnd-generator is shared among several threads which should be fine
// the draw-list, however, is per-thread, which is mandatory!
// alternative to the generator would be seeding the interal one which prooved very unstable!
DrawList<const Node*> drawer(rnd); drawer.reserve(10);
// evaluate each neighbor
for (const Node& neighbor : grid.neighbors(*curNode)) {
const double prob = getProbability(currentState, *startNode, *curNode, neighbor);
drawer.add(&neighbor, prob);
maxEdgeProb.add(prob);
}
// pick a neighbor
// pick a neighbor and get its probability
//double nodeProbability;
const Node* nextNode = drawer.get();
//if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one
//probability += nodeProbability;
//++cnt;
// inform
step(currentState, *curNode, *nextNode);
@@ -64,12 +99,20 @@ public:
curNode = nextNode;
currentState.position = *curNode;
}
//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
probability = 1.0;
//probability = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0); // dist_m might be zero -> no edges -> no maximum
probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones)
//probability = std::pow(probability, 5);
// update after
updateAfter(currentState, *startNode, *curNode);
// calculate the walked-distance-error (is usually negative, as we walked a little too far)
currentState.distance.error_m = dist_m;
// done
return currentState;
@@ -78,10 +121,10 @@ public:
private:
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
inline void updateBefore(WalkState& state) {
inline void updateBefore(WalkState& state, const Node& startNode) {
for (WalkModule<Node, WalkState>* mdl : modules) {
mdl->updateBefore(state);
mdl->updateBefore(state, startNode);
}
}

18
grid/walk/v2/modules/WalkModule.h
View File

@@ -9,6 +9,22 @@ struct WalkState {
/** current position within the grid (-> in cm!) */
GridPoint position;
/** nested struct to prevent name clashes */
struct Distance {
/**
* for every walk, the walker is given a desired distance
* however, the walking distance depends on the grid size and can
* therefore never be reached exactly. therefor we track the
* error between desired and walked distance to ensure "in average"
* the walked distance is correct
*/
float error_m;
Distance() : error_m(0) {;}
} distance;
/** ctor */
explicit WalkState(const GridPoint& position) : position(position) {;}
@@ -23,7 +39,7 @@ template <typename Node, typename WalkState> class WalkModule {
public:
/** update the given WalkState before starting the walk. e.g. based on sensor readings */
virtual void updateBefore(WalkState& state) = 0;
virtual void updateBefore(WalkState& state, const Node& startNode) = 0;
/** get the probability p(e) from curNode to potentialNode */
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;

111
grid/walk/v2/modules/WalkModuleActivityControl.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef WALKMODULEACTIVITYCONTROL_H
#define WALKMODULEACTIVITYCONTROL_H
#include "WalkModule.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
/**
* use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
* from the system's control data to favor edges that resemble this activity
*/
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {
private:
const Control* ctrl;
public:
/** ctor */
WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
}
// static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {
// switch (activity) {
// case ActivityButterPressure::Activity::DOWN:
// if (deltaZ_cm < 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::UP:
// if (deltaZ_cm > 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::STAY:
// if (deltaZ_cm == 0) {return 0.85;}
// {return 0.15;}
// default:
// throw Exception("not yet implemented");
// }
// return 1.0;
// }
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
// TODO: general activity enum and activity-detector based on barometer and accelerometer?
const ActivityButterPressure::Activity activity = ctrl->activity;
// const float kappa = 0.75;
switch (activity) {
case ActivityButterPressure::Activity::DOWN:
if (deltaZ_cm < 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::UP:
if (deltaZ_cm > 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::STAY:
if (deltaZ_cm == 0) {return 0.60;}
{return 0.40;}
// case ActivityButterPressure::Activity::DOWN:
// case ActivityButterPressure::Activity::UP:
// if (potentialNode.getType() == GridNode::TYPE_STAIR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
// {return 1-kappa;}
// case ActivityButterPressure::Activity::STAY:
// if (potentialNode.getType() == GridNode::TYPE_DOOR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_FLOOR) {return kappa;}
// {return 1-kappa;}
default:
throw Exception("not yet implemented");
}
}
};
#endif // WALKMODULEACTIVITYCONTROL_H

32
grid/walk/v2/modules/WalkModuleButterActivity.h
View File

@@ -2,12 +2,30 @@
#define WALKMODULEBUTTERACTIVITY_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
DEPREACTED
SEE WalkModuleActivityControl
struct WalkStateBarometerActivity {
/** innser-struct to prevent name-clashes */
struct Barometer {
/** activity currently detected from the baromter */
ActivityButterPressure::Activity activity;
Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
} barometer;
/** ctor */
WalkStateBarometerActivity() : barometer() {;}
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
@@ -16,11 +34,15 @@ public:
/** ctor */
WalkModuleButterActivity() {
;
// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -43,11 +65,11 @@ public:
const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
if(state.act == ActivityButterPressure::Activity::DOWN){
if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
if (deltaZ_cm < 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;
} else if (state.act == ActivityButterPressure::Activity::UP){
} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
if (deltaZ_cm > 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;

15
grid/walk/v2/modules/WalkModuleFavorZ.h
View File

@@ -36,7 +36,7 @@ template <typename Node, typename WalkState> class WalkModuleFavorZ : public Wal
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
const int keepForXEdges = 8;
public:
@@ -49,8 +49,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -93,15 +94,15 @@ public:
const int diff = potentialNode.z_cm - curNode.z_cm;
// tendence available + tendence match? -> high score!
if (tendence > 0 && diff > 0) {return 0.95;}
if (tendence < 0 && diff < 0) {return 0.95;}
if (tendence > 0 && diff >= 0) {return 0.90;}
if (tendence < 0 && diff <= 0) {return 0.90;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
if (tendence > 0 && diff < 0) {return 0.10;}
if (tendence < 0 && diff > 0) {return 0.10;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
return (diff != 0) ? (0.75) : (0.25);
}

83
grid/walk/v2/modules/WalkModuleFollowDestination.h
View File

@@ -2,39 +2,82 @@
#define WALKMODULEFOLLOWDESTINATION_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../nav/dijkstra/Dijkstra.h"
#include "../../../../nav/dijkstra/DijkstraPath.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/**
* favour edges p(e) that approach the destination
* favor nodes that approach a known destination
*/
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
private:
const Grid<Node>& grid;
Dijkstra<Node> dijkstra;
const DijkstraNode<Node>* dnDest = nullptr;
struct DijkstraMapper {
struct DijkstraAccess {
const Grid<Node>& grid;
DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
DijkstraAccess(const Grid<Node>& grid) : grid(grid) {;}
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
float getWeightBetween(const Node& n1, const Node& n2) const {
return n1.getDistanceInCM(n2) * n2.navImportance;
return n1.getDistanceInMeter(n2) / n2.getNavImportance();
}
};
public:
/** ctor */
WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
/** ctor WITHOUT known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
// shortest path calculation
dijkstra.build(&destination, DijkstraMapper(grid));
// ensure the template WalkState inherits from 'WalkStateFavorZ'
//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
/** ctor WITH known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
setDestination(destination);
}
/** set the desired destination node */
void setDestination(const Node& dest) {
DijkstraAccess acc(grid);
dijkstra.build(&dest, acc);
dnDest = dijkstra.getNode(dest);
}
/** get the shortest path from the given start to the configured destination */
DijkstraPath<Node> getShortestPath(const Node& start) {
// destination unknown? -> empty path
if (!dnDest) {return DijkstraPath<Node>();}
const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
const DijkstraPath<Node> path(dnStart, dnDest);
return path;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
@@ -43,26 +86,28 @@ public:
(void) nextNode;
}
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const float kappa = 0.8;
const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
// probability
return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
if (dnCur == nullptr) {return 1.0;}
if (dnPot == nullptr) {return 1.0;}
constexpr double kappa = 0.70;
const float curDistToTarget = dnCur->cumWeight;
const float potDistToTarget = dnPot->cumWeight;
return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
};
#endif // WALKMODULEFOLLOWDESTINATION_H

4
grid/walk/v2/modules/WalkModuleHeading.h
View File

@@ -36,7 +36,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) startNode;
// add noise
state.heading.direction += draw.get();

40
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -14,20 +14,18 @@ template <typename Node, typename WalkState, typename Control> class WalkModuleH
private:
/** CURRENTLY NOT USED van-Mises distribution */
Distribution::LUT<double> dist;
Distribution::LUT<float> dist;
/** random noise */
Distribution::Normal<float> distNoise;
Control* ctrl;
//std::unordered_map<WalkState*, float> errorTracker;
const Control* ctrl;
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<float>(0.0f, 2.0f).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
@@ -37,17 +35,20 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
// reset this one after every transition!
Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
// sensor noise
const float var = distNoise.draw();
float var = distNoise.draw();
// stair? -> increase variance
if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
// adjust the state's heading using the control-data
state.heading.direction += ctrl->turnAngle + var;
state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
}
@@ -61,8 +62,13 @@ public:
(void) state;
// ignore for stairs?
//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
// for elevator edges [same (x,y) but different z] do not adjust anything
if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
if (nextNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
//if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
// get the heading denoted by the way from curNode to nextNode
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -86,9 +92,13 @@ public:
(void) startNode;
// ignore for stairs?
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
// for elevator edges [same (x,y) but different z] just return 1
if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// get the heading between curNode and potentialNode
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
@@ -99,16 +109,18 @@ public:
// get the difference
const float angularDiff = head.getDiffHalfRAD(stateHead);
if (angularDiff > Angle::degToRad(135)) {return 0.01;}
if (angularDiff > Angle::degToRad(90)) {return 0.02;}
if (angularDiff > Angle::degToRad(45)) {return 0.07;}
{return 0.90;}
// add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"
// determine probability
const float prob = dist.getProbability(angularDiff);
return prob;
// const float prob = dist.getProbability(angularDiff);
// return prob;
}

10
grid/walk/v2/modules/WalkModuleNodeImportance.h
View File

@@ -22,8 +22,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -44,7 +45,12 @@ public:
(void) startNode;
(void) curNode;
const double prob = potentialNode.getNavImportance();
//const double prob = potentialNode.getWalkImportance();
const float i1 = curNode.getWalkImportance();
const float i2 = potentialNode.getWalkImportance();
const double prob = (i2 > i1) ? (0.9) : (0.1);
return prob;
}

3
grid/walk/v2/modules/WalkModulePreventVisited.h
View File

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

3
grid/walk/v2/modules/WalkModuleRelativePressureControl.h
View File

@@ -46,8 +46,9 @@ public:
;
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

3
grid/walk/v2/modules/WalkModuleSpread.h
View File

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

4
main.cpp
View File

@@ -19,13 +19,15 @@ int main(int argc, char** argv) {
// skip all tests starting with LIVE_
//::testing::GTEST_FLAG(filter) = "*Barometer*";
::testing::GTEST_FLAG(filter) = "*Stairs*";
//::testing::GTEST_FLAG(filter) = "*Distribution.T*";
//::testing::GTEST_FLAG(filter) = "*RingBuffer*";
//::testing::GTEST_FLAG(filter) = "*Grid.*";
//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*";
::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
//::testing::GTEST_FLAG(filter) = "*Barometer*";

1
math/Distributions.h
View File

@@ -7,5 +7,6 @@
#include "distribution/Uniform.h"
#include "distribution/VonMises.h"
#include "distribution/Region.h"
#include "distribution/Triangle.h"
#endif // DISTRIBUTIONS_H

47
math/DrawList.h
View File

@@ -21,8 +21,11 @@ template <typename T> class DrawList {
/** the cumulative probability up to this element */
double cumProbability;
/** the element's own probability */
double probability;
/** ctor */
Entry(T element, const double cumProbability) : element(element), cumProbability(cumProbability) {;}
Entry(T element, const double cumProbability, const double probability) : element(element), cumProbability(cumProbability), probability(probability) {;}
/** compare for searches */
bool operator < (const double val) const {return cumProbability < val;}
@@ -37,13 +40,30 @@ private:
/** all contained elements */
std::vector<Entry> elements;
/** random number generator */
RandomGenerator gen;
/** the used random number generator */
RandomGenerator& gen;
private:
/** default random generator. fallback */
RandomGenerator defRndGen;
public:
/** ctor */
DrawList() : cumProbability(0) {
/** ctor with random seed */
DrawList() : cumProbability(0), gen(defRndGen) {
;
}
/** ctor with custom seed */
DrawList(const uint32_t seed) : cumProbability(0), gen(defRndGen(seed)) {
;
}
/** ctor with custom RandomNumberGenerator */
DrawList(RandomGenerator& gen) : cumProbability(0), gen(gen) {
;
}
@@ -58,15 +78,29 @@ public:
elements.clear();
}
/** adjust the reserved list size */
void reserve(const size_t numElements) {
elements.reserve(numElements);
}
/** add a new user-element and its probability */
void add(T element, const double probability) {
Assert::isTrue(probability >= 0, "probability must not be negative!");
cumProbability += probability;
elements.push_back(Entry(element, cumProbability));
elements.push_back(Entry(element, cumProbability, probability));
}
/** get a random element based on its probability */
T get() {
double tmp; // ignored
return get(tmp);
}
/**
* get a random element based on its probability.
* the probability of the picked element is returned using the out parameter
*/
T get(double& elemProbability) {
// generate random number between [0:cumProbability]
std::uniform_real_distribution<> dist(0, cumProbability);
@@ -81,6 +115,7 @@ public:
Assert::isFalse(tmp == elements.end(), "draw() did not find a valid element");
// done
elemProbability = (*tmp).probability;
return (*tmp).element;
}

43
math/Median.h
View File

@@ -1,43 +0,0 @@
#ifndef MEDIAN_H
#define MEDIAN_H
#include <vector>
#include <algorithm>
template <typename Scalar> class Median {
private:
/** all scalar values in a sorted order (ascending) */
std::vector<Scalar> sorted;
public:
/** add the given scalar value to the median calculation */
void add(const Scalar value) {
const auto idx = std::upper_bound( sorted.begin(), sorted.end(), value );
sorted.insert( idx, value );
}
/** get the median of all added values */
float get() const {
if (sorted.size() % 2 == 1) { // odd
const int idx = sorted.size()/2;
return sorted[idx];
} else { // even
const int idx0 = sorted.size()/2 - 1;
return (sorted[idx0] + sorted[idx0+1]) / 2;
}
}
};
#endif // MEDIAN_H

2
math/Random.h
View File

@@ -21,7 +21,7 @@ public:
RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
/** ctor with custom seed */
RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;}
RandomGenerator(result_type seed) : std::minstd_rand(seed) {;}
};

9
math/Stats.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef MATH_STATS_H
#define MATH_STATS_H
#include "stats/Average.h"
#include "stats/Median.h"
#include "stats/Minimum.h"
#include "stats/Maximum.h"
#endif // MATH_STATS_H

50
math/distribution/Triangle.h Normal file
View File

@@ -0,0 +1,50 @@
#ifndef TRIANGLE_H
#define TRIANGLE_H
#include <cmath>
#include <random>
#include "../Random.h"
#include "../../Assertions.h"
#include "Normal.h"
namespace Distribution {
/**
* distribution that forms a triangle
* sigma defines the width (from mu to 0.0, half the width of the triangle's base)
* all values outside of the triangle are zero
*/
template <typename T> class Triangle {
private:
const T mu;
const T sigma;
const T area;
public:
/** ctor */
Triangle(const T mu, const T sigma) : mu(mu), sigma(sigma), area(sigma*sigma) {
}
/** get probability for the given value */
T getProbability(const T val) const {
const T diff = std::abs(val - mu);
if (diff > sigma) {return 0;} // outside of triangle
return (sigma - diff) / area; // inside the triangle
}
/** get the probability for the given value */
static T getProbability(const T mu, const T sigma, const T val) {
Triangle<T> dist(mu, sigma);
return dist.getProbability(val);
}
};
}
#endif // TRIANGLE_H

43
math/stats/Average.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef AVERAGE_H
#define AVERAGE_H
#include "../../Assertions.h"
namespace Stats {
template <typename Scalar> class Average {
private:
int cnt;
Scalar sum;
public:
/** ctor */
Average() : cnt(0), sum(0) {
;
}
/** contains a valid average? */
bool isValid() const {
return cnt > 0;
}
/** add a new value */
void add(const Scalar val) {
sum += val;
++cnt;
}
/** get the current value */
Scalar get() const {
Assert::isNot0(cnt, "add() values first!");
return sum / (Scalar)cnt;
}
};
}
#endif // AVERAGE_H

42
math/stats/Maximum.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef STATS_MAXIMUM_H
#define STATS_MAXIMUM_H
#include "../../Assertions.h"
namespace Stats {
template <typename Scalar> class Maximum {
private:
const Scalar START = -99999999;
Scalar curMax;
public:
/** ctor */
Maximum() : curMax(START) {
;
}
/** is a valid maximum available? */
inline bool isValid() const {
return curMax != START;
}
/** add a new value */
inline void add(const Scalar val) {
if (val > curMax) {curMax = val;}
}
/** get the current value */
inline Scalar get() const {
Assert::notEqual(curMax, START, "add() values first!");
return curMax;
}
};
}
#endif // STATS_MAXIMUM_H

52
math/stats/Median.h Normal file
View File

@@ -0,0 +1,52 @@
#ifndef STATS_MEDIAN_H
#define STATS_MEDIAN_H
#include <vector>
#include <algorithm>
#include "../../Assertions.h"
namespace Stats {
template <typename Scalar> class Median {
private:
/** all scalar values in a sorted order (ascending) */
std::vector<Scalar> sorted;
public:
/** add the given scalar value to the median calculation */
void add(const Scalar value) {
const auto idx = std::upper_bound( sorted.begin(), sorted.end(), value );
sorted.insert( idx, value );
}
/** get the median of all added values */
float get() const {
// sanity check
Assert::isNot0(sorted.size(), "add elements first!");
if (sorted.size() % 2 == 1) { // odd
const int idx = sorted.size()/2;
return sorted[idx];
} else { // even
const int idx0 = sorted.size()/2 - 1;
return (sorted[idx0] + sorted[idx0+1]) / 2;
}
}
};
}
#endif // STATS_MEDIAN_H

40
math/stats/Minimum.h Normal file
View File

@@ -0,0 +1,40 @@
#ifndef STATS_MINIMUM_H
#define STATS_MINIMUM_H
namespace Stats {
template <typename Scalar> class Minimum {
private:
const Scalar START = +999999999;
Scalar curMin;
public:
/** ctor */
Minimum() : curMin(START) {
;
}
/** is a valid minimum available? */
inline bool isValid() const {
return curMin != START;
}
/** add a new value */
void add(const Scalar val) {
if (val < curMin) {curMin = val;}
}
/** get the current value */
Scalar get() const {
Assert::notEqual(curMin, START, "add() values first!");
return curMin;
}
};
}
#endif // STATS_MINIMUM_H

40
misc/Debug.h
View File

@@ -6,6 +6,8 @@
#include <iomanip>
#include "Time.h"
#include "log/LoggerCOUT.h"
/** quick and dirty workaround */
static decltype(Time::tick()) LogLastTick;
@@ -13,18 +15,36 @@ static decltype(Time::tick()) LogLastTick;
class Log {
private:
static Logger** getLoggerPtr() {
static Logger* logger = new LoggerCOUT();
return &logger;
}
static Logger* getLogger() {
return *getLoggerPtr();
}
public:
/** set the to-be-used logger */
static void setLogger(Logger* logger) {
*getLoggerPtr() = logger;
}
static void add(const char* comp, const std::string what, const bool nl = true) {
addComp(comp);
std::cout << what;
if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
std::stringstream out;
addComp(out, comp);
out << what;
getLogger()->add(out.str(), nl);
}
static void add(const std::string& component, const std::string what, const bool nl = true) {
addComp(component.c_str());
std::cout << what;
if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
std::stringstream out;
addComp(out, component.c_str());
out << what;
getLogger()->add(out.str(), nl);
}
@@ -37,15 +57,17 @@ public:
const auto cur = Time::tick();
const int diff_ms = Time::diffMS(LogLastTick, cur);
LogLastTick = cur;
std::cout << " (took: " << diff_ms << "ms)" << std::endl;
std::stringstream out;
out << " (took: " << diff_ms << "ms)";
getLogger()->add(out.str(), true);
}
private:
static void addComp(const char* component) {
std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
static void addComp(std::ostream& out, const char* component) {
out << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
}
};

17
misc/log/Logger.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef LOGGER_H
#define LOGGER_H
#include <string>
/** base-class for all loggers */
class Logger {
public:
virtual ~Logger() {;}
virtual void add(const std::string& str, const bool nl) = 0;
};
#endif // LOGGER_H

19
misc/log/LoggerAndroid.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef LOGGERANDROID_H
#define LOGGERANDROID_H
#include "Logger.h"
class LoggerAndroid : public Logger {
public:
virtual void add(const std::string& str, const bool nl) override {
(void) nl;
#ifdef ANDROID
QMessageLogger().info(str.c_str());
#endif
}
};
#endif // LOGGERANDROID_H

18
misc/log/LoggerCOUT.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef LOGGERCOUT_H
#define LOGGERCOUT_H
#include "Logger.h"
#include <iostream>
class LoggerCOUT : public Logger {
public:
virtual void add(const std::string& str, const bool nl) override {
std::cout << str;
if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
}
};
#endif // LOGGERCOUT_H

30
misc/log/LoggerComposite.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef LOGGERCOMPOSITE_H
#define LOGGERCOMPOSITE_H
#include "Logger.h"
#include <vector>
class LoggerComposite : public Logger {
private:
/** all contained loggers */
std::vector<Logger*> loggers;
public:
/** add a new logger to this composite */
void addLogger(Logger* l) {
loggers.push_back(l);
}
virtual void add(const std::string& str, const bool nl) override {
for (Logger* l : loggers) {l->add(str, nl);}
}
};
#endif // LOGGERCOMPOSITE_H

2
nav/dijkstra/Dijkstra.h
View File

@@ -30,7 +30,7 @@ public:
}
/** get the dijkstra-pendant for the given user-node. null if none matches */
DijkstraNode<T>* getNode(const T& userNode) const {
const inline DijkstraNode<T>* getNode(const T& userNode) const {
auto it = nodes.find(&userNode);
return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
}

5
nav/dijkstra/DijkstraPath.h
View File

@@ -20,6 +20,11 @@ private:
public:
/** empty ctor */
DijkstraPath() {
;
}
/** ctor from end- to start-node */
DijkstraPath(const DijkstraNode<T>* end, const DijkstraNode<T>* start) {

6
sensors/gps/GPSData.h
View File

@@ -15,11 +15,11 @@ struct GPSData {
float accuracy; // m [might be NAN]
float speed; // m/s [might be NAN]
GPSData() : ts(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
GPSData(const Timestamp ts, const float lat, const float lon, const float alt) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;}
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;}
GPSData(const Timestamp ts, const float lat, const float lon, const float alt, const float accuracy) : ts(ts), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;}
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;}
};

8
sensors/imu/AccelerometerData.h
View File

@@ -2,6 +2,8 @@
#define ACCELEROMETERDATA_H
#include <cmath>
#include <sstream>
/** data received from an accelerometer sensor */
struct AccelerometerData {
@@ -40,6 +42,12 @@ struct AccelerometerData {
return AccelerometerData(x/val, y/val, z/val);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
};
#endif // ACCELEROMETERDATA_H

13
sensors/imu/GyroscopeData.h
View File

@@ -2,8 +2,12 @@
#define GYROSCOPEDATA_H
#include <cmath>
#include <sstream>
/** data received from a gyroscope sensor */
/**
* data received from a gyroscope sensor
* IN RADIANS!
*/
struct GyroscopeData {
float x;
@@ -12,12 +16,19 @@ struct GyroscopeData {
GyroscopeData() : x(0), y(0), z(0) {;}
/** ctor from RADIANS */
GyroscopeData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
float magnitude() const {
return std::sqrt( x*x + y*y + z*z );
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
};
#endif // GYROSCOPEDATA_H

6
sensors/imu/StepDetection.h
View File

@@ -34,9 +34,9 @@ private:
Timestamp blockUntil;
bool waitForUp = false;
const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good
const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -0.8*0.6f; // the 0.8 is for testing!
const Timestamp blockTime = Timestamp::fromMS(250); // 150-250 looks good
const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing!
public:

30
sensors/offline/OfflineAndroid.h
View File

@@ -12,6 +12,7 @@
#include "../radio/WiFiMeasurements.h"
#include "../imu/AccelerometerData.h"
#include "../imu/GyroscopeData.h"
#include "../pressure/BarometerData.h"
template <typename SensorData> struct OfflineEntry {
@@ -41,6 +42,7 @@ public:
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
};
/** read recorded android sensor data files */
@@ -56,6 +58,8 @@ private:
std::vector<OfflineEntry<AccelerometerData>> accel;
std::vector<OfflineEntry<AccelerometerData>> gravity;
std::vector<OfflineEntry<BarometerData>> barometer;
WalkedPath walkedPath;
const char* name = "OfflineData";
@@ -82,6 +86,9 @@ public:
/** get all gravity readings */
const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}
/** get all barometer readings */
const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
/** get the walked path */
const WalkedPath& getWalkedPath() const {return walkedPath;}
@@ -171,8 +178,15 @@ private:
break;
}
case 5: {
const BarometerData data = parseBarometer(sensorData);
barometer.push_back(OfflineEntry<BarometerData>(ts, data));
if (listener) {listener->onBarometer(ts, data);}
break;
}
case 8: {
const WiFiMeasurements data = parseWiFi(sensorData);
const WiFiMeasurements data = parseWiFi(ts, sensorData);
wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
if (listener) {listener->onWiFi(ts, data);}
break;
@@ -196,7 +210,7 @@ private:
/** parse the given WiFiObservation string "MAC;freq;RSSI;MAC;freq;RSSI;...." */
static inline WiFiMeasurements parseWiFi(std::string data) {
static inline WiFiMeasurements parseWiFi(const Timestamp ts, std::string data) {
WiFiMeasurements obs;
@@ -219,7 +233,7 @@ private:
Assert::isTrue(data[0] == ';', "unexpected character");
data = data.substr(1);
const WiFiMeasurement e(mac, std::stof(rssi));
const WiFiMeasurement e(mac, std::stof(rssi), ts);
obs.entries.push_back(e);
}
@@ -265,6 +279,16 @@ private:
}
/** parse the given Barometer entry */
static inline BarometerData parseBarometer(const std::string& data) {
BarometerData baro;
const int pos = data.find(';');
baro.hPa = std::stof(data.substr(0, pos));
return baro;
}
/** parse the given GroundTruth entry */
static inline GroundTruthID parseGroundTruthTick(const std::string& data) {

8
sensors/pressure/ActivityButterPressure.h
View File

@@ -40,9 +40,9 @@ public:
/** change this values for much success */
const bool additionalLowpassFilter = false;
const int diffSize = 20; //the number values used for finding the activity.
const int diffSize = 20; //the number values used for finding the activity.
const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.05f,2);
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.1f,2);
Filter::ButterworthLP<float> butter2 = Filter::ButterworthLP<float>(10,0.1f,2);
FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
@@ -52,7 +52,7 @@ public:
/** ctor */
ActivityButterPressure() : currentActivity(STAY){
;
}
}
/** add new sensor readings that were received at the given timestamp */
@@ -89,7 +89,7 @@ public:
if(newInterpolatedValues == true){
//getActivity
if(output.size() > diffSize){
if((int)output.size() > diffSize){
//diff
std::vector<float> diff;
for(int i = output.size() - diffSize; i < output.size() - 1; ++i){

2
sensors/pressure/PressureTendence.h
View File

@@ -4,7 +4,7 @@
#include "../../data/Timestamp.h"
#include "../../math/MovingAVG.h"
#include "../../math/MovingMedian.h"
#include "../../math/Median.h"
#include "../../math/Stats.h"
#include "BarometerData.h"

15
sensors/radio/VAPGrouper.h
View File

@@ -5,7 +5,7 @@
#include <unordered_map>
#include <cmath>
#include "../../math/Median.h"
#include "../../math/Stats.h"
#include "WiFiMeasurements.h"
@@ -142,7 +142,7 @@ private:
/** get the median signal strength */
inline float getMedian(const std::vector<WiFiMeasurement>& vaps) const {
Median<float> median;
Stats::Median<float> median;
for (const WiFiMeasurement& vap : vaps) {
median.add(vap.getRSSI());
}
@@ -153,11 +153,16 @@ private:
/** get the maximum signal strength */
inline float getMax(const std::vector<WiFiMeasurement>& vaps) const {
float max = -9999999;
Stats::Maximum<float> max;
for (const WiFiMeasurement& vap : vaps) {
if (vap.getRSSI() > max) {max = vap.getRSSI();}
max.add(vap.getRSSI());
}
return max;
return max.get();
// float max = -9999999;
// for (const WiFiMeasurement& vap : vaps) {
// if (vap.getRSSI() > max) {max = vap.getRSSI();}
// }
// return max;
}

55
sensors/radio/WiFiGridEstimator.h
View File

@@ -18,23 +18,23 @@ class WiFiGridEstimator {
public:
/**
* convenience method
*/
template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) {
// /**
// * convenience method
// */
// template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) {
// list of all APs
std::vector<LocatedAccessPoint> aps;
for (const Floorplan::Floor* f : im->floors) {
for (const Floorplan::AccessPoint* ap : f->accesspoints) {
aps.push_back(LocatedAccessPoint(*ap));
}
}
// // list of all APs
// std::vector<LocatedAccessPoint> aps;
// for (const Floorplan::Floor* f : im->floors) {
// for (const Floorplan::AccessPoint* ap : f->accesspoints) {
// aps.push_back(LocatedAccessPoint(*ap));
// }
// }
// perform estimation
estimate(grid, mdl, aps);
// // perform estimation
// estimate(grid, mdl, aps);
}
// }
/**
* perform a signal-strength estimation for all of the given access points
@@ -42,39 +42,50 @@ public:
* store the estimated strength onto each node.
* as nodes only provide a limited number of rssi-entries,
* store only the strongest ones.
*
* as the smartphone is held above the ground, we do NOT want to estimate
* the signal strength for the nodes (on the ground) but for the nodes
* + the height the smartphone is held at
*
*/
template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const std::vector<AccessPoint> aps) {
template <typename Node> static void estimate(Grid<Node>& grid, WiFiModel& mdl, const float smartphoneAtHeight) {
// sanity checks
Assert::isTrue(Node::getMapAPs().empty(), "there are already some processed APs available!");
Assert::isTrue(Node::getMapAPs().empty(), "there are already APs stored on the grid nodes!");
// attach the access-points to the shared node-vector
// all APs known to the model
std::vector<AccessPoint> aps = mdl.getAllAPs();
// attach each access-points to a vector shared for all grid-nodes
for (const AccessPoint& ap : aps) {
Node::getMapAPs().push_back(ap);
}
// smartphone offset (meter above ground)
const Point3 smartphoneOffset(0,0,smartphoneAtHeight);
// process each node
for (Node& n : grid) {
// keep the strongest APs to attach to this node
std::vector<WiFiGridNodeAP> nodeAPs;
// process each AP
// process each AP known to the model
for (int apIdx = 0; apIdx < (int) aps.size(); ++apIdx) {
// estimate the signal-strength
const float rssi = mdl.getRSSI(aps[apIdx].getMAC(), n.inMeter());
const float rssi = mdl.getRSSI(aps[apIdx].getMAC(), n.inMeter() + smartphoneOffset);
// keep it
// (temporarily) keep it
nodeAPs.push_back(WiFiGridNodeAP(apIdx, rssi));
}
// sort all APs by signal strength
// now sort all the visible APs by signal strength
auto comp = [] (const WiFiGridNodeAP& ap1, const WiFiGridNodeAP& ap2) {return ap1.getRSSI() > ap2.getRSSI();};
std::sort(nodeAPs.begin(), nodeAPs.end(), comp);
// attach the strongest X to the node
// and finally attach the strongest X to the node
const int cnt = std::min( n.getMaxAPs(), (int) nodeAPs.size() );
for (int i = 0; i < cnt; ++i) {
n.strongestAPs[i] = nodeAPs[i];

17
sensors/radio/WiFiGridNode.h
View File

@@ -3,6 +3,7 @@
#include <cstdint>
#include "AccessPoint.h"
#include "../../misc/Debug.h"
/**
* rssi model-estimation for one AP, denoted by its index [among all APs present within the map]
@@ -100,9 +101,11 @@ template <int maxAccessPoints> struct WiFiGridNode {
* returns 0 if unknown
*/
float getRSSI(const MACAddress mac) const {
for (const WiFiGridNodeAP ap : strongestAPs) {
if (!ap.isValid()) {break;} // reached the end
if (getMapAPs()[ap.getAPIdx()].getMAC() == mac) {return ap.getRSSI();}
for (const WiFiGridNodeAP& ap : strongestAPs) {
//std::cout << getMapAPs()[ap.getAPIdx()].getMAC().asString() << std::endl;
//std::cout << mac.asString() << std::endl;
if (!ap.isValid()) {break;} // reached the end of all APs visible on this node
if (getMapAPs()[ap.getAPIdx()].getMAC() == mac) {return ap.getRSSI();} // does this APs MAC match with the requested MAC? -> found!
}
return 0;
}
@@ -124,6 +127,8 @@ template <int maxAccessPoints> struct WiFiGridNode {
protected:
static constexpr const char* name = "WiFiGridNode";
/** serialize static members */
static void staticSerialize(std::ostream& out) {
@@ -134,6 +139,8 @@ protected:
out.write((const char*) &numAPs, sizeof(numAPs));
out.write((const char*) getMapAPs().data(), sizeof(getMapAPs()[0])*numAPs);
Log::add(name, "serialized " + std::to_string(numAPs) + " APs");
}
/** deserialize static members */
@@ -149,6 +156,10 @@ protected:
// deserialize APs within map
inp.read((char*) getMapAPs().data(), sizeof(getMapAPs()[0])*numAPs);
Log::add(name, "de-serialized " + std::to_string(numAPs) + " APs");
std::string aps; for (const AccessPoint& ap : getMapAPs()) {aps += ap.getMAC().asString() + " ";}
Log::add(name, aps);
}

2
sensors/radio/WiFiMeasurement.h
View File

@@ -9,7 +9,7 @@
*/
class WiFiMeasurement {
private:
public:
friend class VAPGrouper;

9
sensors/radio/WiFiMeasurements.h
View File

@@ -13,6 +13,15 @@ struct WiFiMeasurements {
/** all contained measurements */
std::vector<WiFiMeasurement> entries;
/** convert to string */
std::string asString() const {
std::string res;
for (const WiFiMeasurement& m : entries) {
res += m.getAP().getMAC().asString() + ": " + std::to_string(m.getRSSI()) + "\n";
}
return res;
}
};
#endif // WIFIMEASUREMENTS_H

23
sensors/radio/WiFiProbabilityFree.h
View File

@@ -4,6 +4,7 @@
#include "WiFiProbability.h"
#include "model/WiFiModel.h"
#include "../../math/Distributions.h"
#include "VAPGrouper.h"
#include <unordered_map>
@@ -17,7 +18,7 @@ private:
const float sigma = 8.0f;
const float sigmaPerSecond = 1.5f;
const float sigmaPerSecond = 3.0f;
/** the RSSI prediction model */
WiFiModel& model;
@@ -25,23 +26,25 @@ private:
/** the map's floorplan */
Floorplan::IndoorMap* map;
public:
WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) {
}
double getProbability(const Point3& pos, const Timestamp curTime, const WiFiMeasurements& obs) const {
double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
double prob = 1.0;
int numMatchingAPs = 0;
// process each measured AP
for (const WiFiMeasurement& entry : obs.entries) {
// sanity check
Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");
// get the model's RSSI (if possible!)
const float modelRSSI = model.getRSSI(entry.getAP().getMAC(), pos);
const float modelRSSI = model.getRSSI(entry.getAP().getMAC(), pos_m);
// NaN? -> AP not known to the model -> skip
if (modelRSSI != modelRSSI) {continue;}
@@ -53,21 +56,29 @@ public:
// the measurement's age
const Timestamp age = curTime - entry.getTimestamp();
Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
// sigma grows with measurement age
const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
// update probability
prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
++numMatchingAPs;
}
// sanity check
Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
return prob;
}
template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {
throw "todo??";
throw Exception("todo??");
}
};

67
sensors/radio/WiFiProbabilityGrid.h
View File

@@ -6,14 +6,17 @@
#include "../../math/Distributions.h"
#include "../../data/Timestamp.h"
#include "WiFiGridNode.h"
#include "WiFiProbability.h"
#include <unordered_set>
/**
* probability is calculated by comparing pre-calculated wifi-signal-strengths
* attached to each grid-node with a given WiFiMeasurements data structure
*/
class WiFiObserverGrid : public WiFiProbability {
template <typename Node> class WiFiObserverGrid : public WiFiProbability {
private:
@@ -21,14 +24,23 @@ private:
float sigma = 8.0f;
/** additional sigma-per-second (measurement age) to*/
float sigmaPerSecond = 1.5;
float sigmaPerSecond = 3;
std::unordered_set<MACAddress> knownAPs;
public:
/** ctor with uncertainty */
WiFiObserverGrid(const float sigma) : sigma(sigma) {
;
//StaticAssert::AinheritsB<Node, WiFiGridNode>();
for (const AccessPoint& ap : Node::getMapAPs()) {
knownAPs.insert(ap.getMAC());
}
Assert::isFalse(knownAPs.empty(), "no APs known to the grid nodes?!");
}
/**
@@ -36,43 +48,72 @@ public:
* compares the predicted signal-strengths stored on the given node
* with the provided WiFi measurements
*/
template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs) const {
double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs) const {
double prob = 0;
// compile-time sanity check. Node must be a subclass off WiFiGridNode
//StaticAssert::AinheritsB<Node, WiFiGridNode>();
double prob = 1;
int numMatchingAPs = 0;
// after some runtime, check whether the wifi timestamps make sense
// those must not be zero, otherwise something is wrong!
if (!obs.entries.empty()) {
Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
}
// process each observed measurement
for (const WiFiMeasurement& measurement : obs.entries) {
// if an AP is not known to any of the nodes, just skip it
if (knownAPs.find(measurement.getAP().getMAC()) == knownAPs.end()) {
continue;}
// determine the age for this measurement
const Timestamp age = curTime - measurement.getTimestamp();
// sigma grows with measurement age
const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
float sigma = this->sigma + this->sigmaPerSecond * age.sec();
// the RSSI from the scan
const float measuredRSSI = measurement.getRSSI();
// the RSSI from the model (if available!)
const float modelRSSI = n.getRSSI(measurement.getAP().getMAC());
float modelRSSI = n.getRSSI(measurement.getAP().getMAC());
// no model RSSI available?
if (modelRSSI == 0) {continue;}
// if no model RSSI is available, that means,
// the AP in question is not / only barely visible at this location
// assume a very low signal-strength and increase the sigma
if (modelRSSI == 0) {
modelRSSI = -100;
sigma *= 2;
}
// compare both
const double p = Distribution::Normal<double>::getProbability(measuredRSSI, sigma, modelRSSI);
//const double p = Distribution::Region<double>::getProbability(measuredRSSI, sigma, modelRSSI);
// adjust using log
prob += std::log(p);
//prob += std::log(p);
prob *= p;
++numMatchingAPs;
}
//return std::pow(std::exp(prob), 0.1);
return std::exp(prob);
// sanity check
// Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
// if (numMatchingAPs == 0) {return 0;}
// as not every node has the same number of visible/matching APs
// we MUST return something like the average probability
return prob;
//return std::pow(prob, 1.0/3.0);
}
/** gnuplot debug dump */
template <typename Node> void dump(Grid<Node>& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) {
void dump(Grid<Node>& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) {
std::ofstream out(fileName);
out << "splot '-' with points palette\n";

3
sensors/radio/model/WiFiModel.h
View File

@@ -16,6 +16,9 @@ public:
// /** get the given access-point's RSSI at the provided location */
// virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
/** get a list of all APs known to the model */
virtual std::vector<AccessPoint> getAllAPs() const = 0;
/**
* get the RSSI expected at the given location (in meter)
* for an AP identified by the given MAC.

7
sensors/radio/model/WiFiModelLogDist.h
View File

@@ -36,6 +36,13 @@ public:
;
}
/** get a list of all APs known to the model */
std::vector<AccessPoint> getAllAPs() const {
std::vector<AccessPoint> aps;
for (const auto it : accessPoints) {aps.push_back(AccessPoint(it.first));}
return aps;
}
/** make the given AP (and its parameters) known to the model */
void addAP(const MACAddress& accessPoint, const APEntry& params) {

87
sensors/radio/model/WiFiModelLogDistCeiling.h
View File

@@ -6,6 +6,8 @@
#include "../../../Assertions.h"
#include "WiFiModel.h"
#include "LogDistanceModel.h"
#include "../VAPGrouper.h"
#include "../../../misc/Debug.h"
/**
* signal-strength estimation using log-distance model
@@ -39,9 +41,12 @@ private:
public:
/** ctor */
/** ctor with floorplan (needed for ceiling position) */
WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) {
// sanity checks
Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
// position of all ceilings
for (Floorplan::Floor* f : map->floors) {
ceilingsAtHeight_m.push_back(f->atHeight);
@@ -49,6 +54,39 @@ public:
}
/** get a list of all APs known to the model */
std::vector<AccessPoint> getAllAPs() const {
std::vector<AccessPoint> aps;
for (const auto it : accessPoints) {aps.push_back(AccessPoint(it.first));}
return aps;
}
/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
const APEntry ape(ap->getPos(floor), txp, exp, waf);
addAP(MACAddress(ap->mac), ape);
}
}
}
/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
const APEntry ape(ap->getPos(floor), txp, exp, waf);
const MACAddress mac = vg.getBaseMAC(MACAddress(ap->mac));
Log::add("WiModLDC", "AP: " + ap->mac + " -> " + mac.asString());
addAP(MACAddress(mac), ape);
}
}
}
/** make the given AP (and its parameters) known to the model */
void addAP(const MACAddress& accessPoint, const APEntry& params) {
@@ -57,11 +95,18 @@ public:
Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?");
// add
accessPoints.insert( std::pair<MACAddress, APEntry>(accessPoint, params) );
}
/** remove all added APs */
void clear() {
accessPoints.clear();
}
float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
// try to get the corresponding parameters
@@ -90,6 +135,28 @@ public:
protected:
FRIEND_TEST(LogDistanceCeilingModel, numCeilings);
FRIEND_TEST(LogDistanceCeilingModel, numCeilingsFloat);
/** get the number of ceilings between z1 and z2 */
float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
float cnt = 0;
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {
const float dmax = zMax - z;
cnt += (dmax > 1) ? (1) : (dmax);
}
}
return cnt;
}
/** get the number of ceilings between z1 and z2 */
int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
@@ -98,6 +165,24 @@ protected:
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
#ifdef WITH_ASSERTIONS
static int numNear = 0;
static int numFar = 0;
for (const float z : ceilingsAtHeight_m) {
const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
if (diff < 0.1) {++numNear;} else {++numFar;}
}
if ((numNear + numFar) > 150000) {
Assert::isTrue(numNear < numFar*0.1,
"many requests to the WiFiModel address nodes (very) near to a ground! \
due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
expect very wrong outputs! \
consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
);
}
#endif
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {++cnt;}
}

94
sensors/radio/setup/WiFiFingerprint.h Normal file
View File

@@ -0,0 +1,94 @@
#ifndef WIFIFINGERPRINT_H
#define WIFIFINGERPRINT_H
#include "../../../geo/Point3.h"
#include "../WiFiMeasurements.h"
#include <unordered_map>
/**
* denotes a wifi fingerprint
* known position and several measurements conducted at this position
*
* as several measurements were conducted, each AP is usually contained more than once!
*/
struct WiFiFingerprint {
/** real-world-position that was measured */
Point3 pos_m;
/** measurements (APs) at the given location */
WiFiMeasurements measurements;
/** ctor */
WiFiFingerprint() {;}
/** ctor */
WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;}
/** as each AP is contained more than once (scanned more than once), group them by MAC and use the average RSSI */
WiFiMeasurements average() {
// group scans by MAC (all measurements for one AP)
std::unordered_map<MACAddress, WiFiMeasurements> group;
for (WiFiMeasurement& m : measurements.entries) {
group[m.getAP().getMAC()].entries.push_back(m);
}
// create the output that contains the AP's average
WiFiMeasurements res;
for (auto& it : group) {
const WiFiMeasurements& apMeasurements = it.second;
WiFiMeasurement avg = apMeasurements.entries.front(); // average starts with a copy of the first entry (to get all data-fields beside the rssi)
for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) { // sum up all other entries [1:end]
avg.rssi += apMeasurements.entries[i].rssi;
}
avg.rssi /= apMeasurements.entries.size();
res.entries.push_back(avg); // add to output
}
// done
return res;
}
/** serialize */
void write(std::ostream& out) const {
out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n";
out << "num: " << measurements.entries.size() << "\n";
for (const WiFiMeasurement& wm : measurements.entries) {
out << wm.getTimestamp().ms() << " " << wm.ap.getMAC().asString() << " " << wm.getRSSI() << "\n";
}
}
/** deserialize */
void read(std::istream& inp) {
std::string tmp;
// read the position
inp >> tmp; if ("pos:" != tmp) {throw "error";}
inp >> pos_m.x >> pos_m.y >> pos_m.z;
// number of entries
inp >> tmp; if ("num:" != tmp) {throw "error";}
int numEntries; inp >> numEntries;
// read the entries
for (int i = 0; i < numEntries; ++i) {
uint64_t ms; inp >> ms;
std::string mac; inp >> mac;
float rssi; inp >> rssi;
WiFiMeasurement wm(AccessPoint(MACAddress(mac)), rssi, Timestamp::fromMS(ms));
measurements.entries.push_back(wm);
}
}
};
#endif // WIFIFINGERPRINT_H

247
sensors/radio/setup/WiFiOptimizer.h Normal file
View File

@@ -0,0 +1,247 @@
#ifndef OPTIMIZER_H
#define OPTIMIZER_H
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
#include "../VAPGrouper.h"
#include "../../../geo/BBox3.h"
#include "../../../misc/Debug.h"
#include "WiFiFingerprint.h"
#include "../model/WiFiModel.h"
#include "../model/WiFiModelLogDistCeiling.h"
#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
#include <string>
#include <sstream>
struct WiFiOptimizer {
private:
/** combine one RSSI measurement with the position the signal was measured at */
struct RSSIatPosition {
/** real-world position (in meter) */
const Point3 pos_m;
/** measured signal strength (for one AP) */
const float rssi;
/** ctor */
RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
};
public:
struct APParams {
float x;
float y;
float z;
float txp;
float exp;
float waf;
Point3 getPos() const {return Point3(x,y,z);}
APParams() {;}
APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
std::string asString() {
std::stringstream ss;
ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
return ss.str();
}
};
/** add MAC-info to params */
struct APParamsMAC {
MACAddress mac;
APParams params;
APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
};
private:
Floorplan::IndoorMap* map;
const VAPGrouper vg;
/** each MAC-Adress has several position->rssi entries */
std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
const char* name = "WiFiOptimizer";
public:
/** ctor */
WiFiOptimizer(Floorplan::IndoorMap* map, const VAPGrouper& vg) : map(map), vg(vg) {
;
}
/** add a new fingerprint to the optimizers data-source */
void addFingerprint(const WiFiFingerprint& fp) {
// group the fingerprint's measurements by VAP (if configured)
const WiFiMeasurements measurements = vg.group(fp.measurements);
// add each available AP to its slot (lookup map)
for (const WiFiMeasurement& m : measurements.entries) {
const RSSIatPosition rap(fp.pos_m, m.rssi);
apMap[m.getAP().getMAC()].push_back(rap);
}
}
/** get a list of all to-be-optimized access-points (given by their mac-address) */
std::vector<MACAddress> getAllMACs() const {
std::vector<MACAddress> res;
for (const auto& it : apMap) {res.push_back(it.first);}
return res;
}
/** optimize all known APs */
std::vector<APParamsMAC> optimizeAll() const {
// sanity chekc
Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!");
float errSum = 0;
std::vector<APParamsMAC> res;
for (const MACAddress& mac : getAllMACs()) {
float err;
const APParams params = optimize(mac, err);
res.push_back(APParamsMAC(mac, params));
errSum += err;
}
const float avgErr = errSum / getAllMACs().size();
Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
return res;
}
/** optimize the given AP */
APParams optimize(const MACAddress& mac, float& errResult) const {
// starting parameters do not matter for the current optimizer!
APParams params(0,0,0, -40, 2.5, -4.0);
constexpr float hugeError = 1e10;
// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
// signal-strength-prediction-model...
WiFiModelLogDistCeiling model(map);
auto func = [&] (const float* data) {
const APParams* params = (APParams*) data;
// some sanity checks
if (params->waf > 0) {return hugeError;}
if (params->txp < -50) {return hugeError;}
if (params->txp > -30) {return hugeError;}
if (params->exp > 4) {return hugeError;}
if (params->exp < 1) {return hugeError;}
// current position guess for the AP;
const Point3 apPos_m = params->getPos();
// add the AP [described by the current guess] to the signal-strength-prediction model
model.clear();
model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
float err = 0;
int cnt = 0;
// process each measurement
for (const RSSIatPosition& reading : entries) {
// get the model-estimation for the fingerprint's position
const float rssiModel = model.getRSSI(mac, reading.pos_m);
// difference between estimation and measurement
const float diff = std::abs(rssiModel - reading.rssi);
// adjust the error
err += diff*diff;
++cnt;
// max distance penality
// [unlikely to get a reading for this AP here!]
if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
}
err /= cnt;
err = std::sqrt(err);
if (params->txp < -50) {err += 999999;}
if (params->txp > -35) {err += 999999;}
if (params->exp > 3.5) {err += 999999;}
if (params->exp < 1.0) {err += 999999;}
return err;
};
//
const BBox3 mapBBox = FloorplanHelper::getBBox(map);
using LeOpt = K::NumOptAlgoRangeRandom<float>;
const std::vector<LeOpt::MinMax> valRegion = {
LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x
LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y
LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z
LeOpt::MinMax(-50,-30), // txp
LeOpt::MinMax(1,3), // exp
LeOpt::MinMax(-10,-4), // waf
};
// log
Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
Log::tick();
LeOpt opt(valRegion);
opt.setPopulationSize(500); // USE MORE FOR PRODUCTION
opt.setNumIerations(150);
opt.calculateOptimum(func, (float*) &params);
// using LeOpt = K::NumOptAlgoGenetic<float>;
// LeOpt opt(6);
// opt.setPopulationSize(750);
// opt.setMaxIterations(50);
// opt.setElitism(0.05f);
// opt.setMutation(0.75f);
// //opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
// opt.setValRegion(valRegion);
// K::NumOptAlgoDownhillSimplex<float, 6> opt;
// opt.setMaxIterations(100);
// opt.setNumRestarts(10);
opt.calculateOptimum(func, (float*) &params);
errResult = func((float*)&params);
Log::tock();
Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err");
return params;
}
};
#endif // OPTIMIZER_H

2
tests/grid/TestAll.cpp
View File

@@ -60,7 +60,7 @@ TEST(TestAll, Nav) {
// plot path
K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
DijkstraNode<GP>* dn = d.getNode(end);
const 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+50));
dn = dn->previous;

5
tests/grid/TestGridWalk2HeadingControl.cpp
View File

@@ -30,6 +30,7 @@
struct MyNode12456012 : public GridPoint, public GridNode {
float getWalkImportance() const {return 1;}
MyNode12456012() {;}
MyNode12456012(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};
@@ -104,7 +105,7 @@ TEST(GridWalk2HeadingControl, LIVE_walkHeading) {
struct MyControl {
float turnAngle = 0;
float turnSinceLastTransition_rad = 0;
} ctrl;
@@ -139,7 +140,7 @@ TEST(GridWalk2HeadingControl, LIVE_walkHeading) {
// run
for (int i = 0; i < 30; ++i) {
ctrl.turnAngle = (i == 0) ? (rad) : (0);
ctrl.turnSinceLastTransition_rad = (i == 0) ? (rad) : (0);
p.clearParticles();

1
tests/grid/TestGridWalk2RelPressure.cpp
View File

@@ -28,6 +28,7 @@
struct MyNode124234 : public GridPoint, public GridNode {
float getWalkImportance() const {return 1;}
MyNode124234() {;}
MyNode124234(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};

2
tests/grid/TestGridWalkV2.cpp
View File

@@ -60,7 +60,7 @@ TEST(GridWalk2, LIVE_error) {
struct Control {
float turnAngle = 0; // keep the angle as-is!
float turnSinceLastTransition_rad = 0; // keep the angle as-is!
} ctrl;
GridWalker<MyNode1239, MyState23452> walker;

2
tests/grid/TestStairs.cpp
View File

@@ -25,6 +25,8 @@
// ENSURE UNIQUE CLASS NAME
struct MyNode345092134 : public GridPoint, public GridNode {
float walkImportance = 0;
float getWalkImportance() const {return walkImportance;}
float navImportance = 0;
float getNavImportance() const {return navImportance;}
MyNode345092134() {;}

29
tests/math/TestDistribution.cpp
View File

@@ -138,6 +138,35 @@ TEST(Distribution, Region1) {
}
TEST(Distribution, Triangle) {
std::ofstream out("/tmp/1.dat");
for (float x = -9; x <= +9; x += 0.025) {
//const float y = Distribution::Region<float>::getProbability(0, 3, 1.2, x);
const float y = Distribution::Triangle<float>::getProbability(3, 6, x);
out << x << " " << y << "\n";
}
out.close();
Distribution::Triangle<float> dist1(0, 3);
ASSERT_NEAR(1.0, distCheckArea(dist1, -20, +20), 0.01);
Distribution::Triangle<float> dist2(0, 1);
ASSERT_NEAR(1.0, distCheckArea(dist2, -20, +20), 0.01);
Distribution::Triangle<float> dist3(1, 3);
ASSERT_NEAR(1.0, distCheckArea(dist3, -20, +20), 0.01);
Distribution::Triangle<float> dist4(1, 2);
ASSERT_NEAR(1.0, distCheckArea(dist4, -20, +20), 0.01);
Distribution::Triangle<float> dist5(-1, 4);
ASSERT_NEAR(1.0, distCheckArea(dist5, -20, +20), 0.01);
}
#endif

92
tests/math/TestStats.cpp Normal file
View File

@@ -0,0 +1,92 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../math/Stats.h"
using namespace Stats;
TEST(Statistics, minimum) {
Minimum<float> min;
min.add(+999);
ASSERT_EQ(999, min.get());
min.add(1);
ASSERT_EQ(1, min.get());
min.add(2);
ASSERT_EQ(1, min.get());
min.add(0);
ASSERT_EQ(0, min.get());
min.add(-99);
ASSERT_EQ(-99, min.get());
}
TEST(Statistics, maximum) {
Maximum<float> max;
max.add(-999);
ASSERT_EQ(-999, max.get());
max.add(1);
ASSERT_EQ(1, max.get());
max.add(2);
ASSERT_EQ(2, max.get());
max.add(1);
ASSERT_EQ(2, max.get());
max.add(99);
ASSERT_EQ(99, max.get());
}
TEST(Statistics, median) {
Median<float> med;
ASSERT_THROW(med.get(), std::exception);
med.add(1);
ASSERT_EQ(1, med.get());
med.add(2);
ASSERT_EQ(1.5, med.get());
med.add(3);
ASSERT_EQ(2, med.get());
med.add(99);
ASSERT_EQ(2.5, med.get());
}
TEST(Statistics, average) {
Average<float> avg;
ASSERT_THROW(avg.get(), std::exception);
avg.add(1);
ASSERT_EQ(1, avg.get());
avg.add(2);
ASSERT_EQ(1.5, avg.get());
avg.add(3);
ASSERT_EQ(2, avg.get());
avg.add(99);
ASSERT_EQ(26.25, avg.get());
}
#endif

6
tests/nav/dijkstra/TestDijkstra.cpp
View File

@@ -34,15 +34,15 @@ TEST(Dijkstra, build) {
d.build(&grid[idx5], &grid[idx3], tmp, 99999);
// start node must be "idx5"
DijkstraNode<GP>* n = d.getNode(grid[idx5]);
const 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]);
const 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]);
const DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
ASSERT_EQ(&grid[idx3], n3->element); ASSERT_EQ(&grid[idx1], n3->previous->element);

40
tests/sensors/radio/TestLogDistanceCeilingModel.cpp
View File

@@ -77,5 +77,45 @@ TEST(LogDistanceCeilingModel, numCeilings) {
}
TEST(LogDistanceCeilingModel, numCeilingsFloat) {
// dummy floorplan
Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
Floorplan::IndoorMap map;
map.floors.push_back(f0);
map.floors.push_back(f1);
map.floors.push_back(f2);
WiFiModelLogDistCeiling model(&map);
const float d = 0.01;
ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,0)), d );
ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,-1)), d );
ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,1)), d );
ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,1), Point3(0,0,0)), d );
ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,-0.01), Point3(0,0,+0.50)), d );
ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,+0.50), Point3(0,0,-0.01)), d );
ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,2.99), Point3(0,0,3.20)), d );
ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,3.20), Point3(0,0,2.99)), d );
ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,6.99), Point3(0,0,8.33)), d );
ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,6.99)), d );
ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,0.00), Point3(0,0,8.33)), d );
ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,0.00)), d );
ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,7.00), Point3(0,0,99)), d );
ASSERT_NEAR(1, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,7)), d );
ASSERT_NEAR(3, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,8)), d );
}
#endif

10
tests/sensors/radio/TestWiFiGrid.cpp
View File

@@ -90,11 +90,11 @@ TEST(WiFiGridModelLogDist, create) {
model.addAP(ap3, WiFiModelLogDist::APEntry( Point3(0,20,0), -40, 1.5));
model.addAP(ap4, WiFiModelLogDist::APEntry( Point3(20,20,0), -40, 1.5));
std::vector<AccessPoint> aps = {
AccessPoint(ap1), AccessPoint(ap2), AccessPoint(ap3), AccessPoint(ap4)
};
// std::vector<AccessPoint> aps = {
// AccessPoint(ap1), AccessPoint(ap2), AccessPoint(ap3), AccessPoint(ap4)
// };
WiFiGridEstimator::estimate(grid, model, aps);
WiFiGridEstimator::estimate(grid, model, 0);
ASSERT_EQ(4, grid[0].getNumVisibleAPs()); // 4 APs visible at this node
@@ -117,7 +117,7 @@ TEST(WiFiGridModelLogDist, create) {
obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:03"), -55, ts));
obs.entries.push_back(WiFiMeasurement(MACAddress("00:00:00:00:00:04"), -55, ts));
WiFiObserverGrid observer(5.0f);
WiFiObserverGrid<TestNode190231> observer(5.0f);
const TestNode190231& gn = grid.getNodeFor(GridPoint(1000,1000,0));
const float p = observer.getProbability(gn, ts, obs);

83
tests/sensors/radio/TestWiFiOptimizer.cpp Normal file
View File

@@ -0,0 +1,83 @@
#ifdef WITH_TESTS
#include "../../Tests.h"
#include "../../../sensors/radio/setup/WiFiOptimizer.h"
#include "../../../sensors/radio/setup/WiFiFingerprint.h"
#include "../../../misc/Debug.h"
#include <random>
/**
* test the wifi-optimizer by generating synthetic fingerprints and optimizing parameters from them
*/
TEST(WiFiOptimizer, optimize) {
const VAPGrouper vg(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
const MACAddress mac1("00:00:00:00:00:01");
const MACAddress mac2("00:00:00:00:00:02");
const Point3 pos1(10, 12, 5);
const Point3 pos2(20, -10, 2);
// building with one floor at 3.0 meters
Floorplan::IndoorMap map;
Floorplan::Floor floor1; floor1.atHeight = 3.0f;
Floorplan::FloorOutlinePolygon poly1; poly1.poly.points.push_back(Point2(-30, -30)); poly1.poly.points.push_back(Point2(+30, +30));
floor1.outline.push_back(&poly1);
map.floors.push_back(&floor1);
// add the two APs to the model
WiFiModelLogDistCeiling mdl(&map);
mdl.addAP(mac1, WiFiModelLogDistCeiling::APEntry(pos1, -40, 2, -4));
mdl.addAP(mac2, WiFiModelLogDistCeiling::APEntry(pos2, -40, 2, -4));
// generate some (synthetic) fingerprints
std::minstd_rand gen;
std::vector<WiFiFingerprint> fingerprints;
for (int i = 0; i < 50; ++i) {
std::uniform_real_distribution<float> distX(-30, +30);
std::uniform_real_distribution<float> distY(-30, +30);
std::uniform_real_distribution<float> distZ( -9, +9);
// get a random position and calculate the model RSSIs
const Point3 randomPt(distX(gen), distY(gen), distZ(gen));
const float rssi1 = mdl.getRSSI(mac1, randomPt);
const float rssi2 = mdl.getRSSI(mac2, randomPt);
// construct a corresponding synthetic fingerprint
WiFiFingerprint fp(randomPt);
fp.measurements.entries.push_back(WiFiMeasurement(AccessPoint(mac1), rssi1));
fp.measurements.entries.push_back(WiFiMeasurement(AccessPoint(mac2), rssi2));
fingerprints.push_back(fp);
}
WiFiOptimizer opt(&map, vg);
for (const WiFiFingerprint& fp : fingerprints) {
opt.addFingerprint(fp);
}
ASSERT_EQ(2, opt.getAllMACs().size());
float errRes;
const WiFiOptimizer::APParams params1 = opt.optimize(mac1, errRes);
ASSERT_TRUE(errRes < 0.1);
ASSERT_NEAR(0, pos1.getDistance(params1.getPos()), 0.4); // apx position estimation
ASSERT_NEAR(-40, params1.txp, 1.0);
ASSERT_NEAR(2, params1.exp, 0.1);
ASSERT_NEAR(-4, params1.waf, 0.5);
const WiFiOptimizer::APParams params2 = opt.optimize(mac2, errRes);
ASSERT_TRUE(errRes < 0.1);
ASSERT_NEAR(0, pos2.getDistance(params2.getPos()), 0.4); // apx position estimation
ASSERT_NEAR(-40, params1.txp, 1.0);
ASSERT_NEAR(2, params2.exp, 0.1);
ASSERT_NEAR(-4, params2.waf, 0.5);
}
#endif