diff --git a/Assertions.h b/Assertions.h index 1608526..98ca4aa 100644 --- a/Assertions.h +++ b/Assertions.h @@ -64,7 +64,10 @@ namespace Assert { } template 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 static inline void isBetween(const T v, const T min, const T max, const STR err) { diff --git a/data/Timestamp.h b/data/Timestamp.h index abbf309..877bfff 100644 --- a/data/Timestamp.h +++ b/data/Timestamp.h @@ -1,6 +1,8 @@ #ifndef TIMESTAMP_H #define TIMESTAMP_H +#include + /** * 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(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();} }; diff --git a/floorplan/v2/FloorplanHelper.h b/floorplan/v2/FloorplanHelper.h index a6074dc..a62daa9 100644 --- a/floorplan/v2/FloorplanHelper.h +++ b/floorplan/v2/FloorplanHelper.h @@ -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) { diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index d2b626e..396f338 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -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 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 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"); diff --git a/geo/Angle.h b/geo/Angle.h index e278bcf..eec89ba 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -56,12 +56,12 @@ public: } /** convert degrees to radians */ - static constexpr float degToRad(const float deg) { + static constexpr inline float degToRad(const float deg) { return deg / 180.0f * M_PI; } /** convert radians to degrees */ - static float radToDeg(const float rad) { + static constexpr inline float radToDeg(const float rad) { return rad * 180.0f / M_PI; } diff --git a/geo/Point3.h b/geo/Point3.h index 80e6aea..ccce6e5 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -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;} diff --git a/grid/Grid.h b/grid/Grid.h index 873635d..0443635 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -6,6 +6,8 @@ #include #include +#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 must inherit from GridNode!" + StaticAssert::AinheritsB(); // "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(); diff --git a/grid/GridPoint.h b/grid/GridPoint.h index 5fb804a..50d8ef6 100755 --- a/grid/GridPoint.h +++ b/grid/GridPoint.h @@ -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 { + 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 diff --git a/grid/factory/GridImportance.h b/grid/factory/GridImportance.h index c098dca..d374b6e 100644 --- a/grid/factory/GridImportance.h +++ b/grid/factory/GridImportance.h @@ -130,7 +130,7 @@ public: } - template void addImportance(Grid& g, DijkstraNode* start, DijkstraNode* end) { + template void addImportance(Grid& g, const DijkstraNode* start, const DijkstraNode* end) { // routing path DijkstraPath path(end, start); diff --git a/grid/factory/v2/Elevators.h b/grid/factory/v2/Elevators.h index 4c035c8..ae20b3a 100644 --- a/grid/factory/v2/Elevators.h +++ b/grid/factory/v2/Elevators.h @@ -40,7 +40,6 @@ public: const int gs_cm = grid.getGridSize_cm(); - struct IntPos { int x_cm; int y_cm; diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index c8c79d5..d6beece 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -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 diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 8b5f92b..6ee6f4c 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -86,8 +86,8 @@ public: KNN>, 3> knnStairs(knnArrStairs); // probability adjustments - Distribution::Normal avoidWalls(0.0, 0.35); - Distribution::Normal favorDoors(0.0f, 0.5f); + Distribution::Triangle avoidWalls(0.0, 0.45f); + Distribution::Normal favorDoors(0.0f, 0.4f); Distribution::Normal favorStairs(0.0f, 1.5f); if (l) { @@ -120,20 +120,33 @@ 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); + const bool useNormal = (distToWall_m*3.5 < distToDoor_m && distToWall_m*3.5 < distToStair_m); // 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; + n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.4f; } + + // navigation importance is calculated using other formulae + n1.navImportance = 1.0f; + if (useNormal) { + n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.4; + } + //n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5; + n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0; + + + + // 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!"); } diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index 5165432..1e4e6cc 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -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; diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 322f1ef..a737daf 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -1,6 +1,7 @@ #ifndef GRIDWALKER_H #define GRIDWALKER_H +#include "../../../data/Timestamp.h" #include "../../Grid.h" #include "../../../math/DrawList.h" #include "modules/WalkModule.h" @@ -16,7 +17,7 @@ private: /** all modules to evaluate */ std::vector*> modules; - DrawList drawer; + RandomGenerator rnd; public: @@ -26,14 +27,16 @@ public: } /** perform the walk based on the configured setup */ - WalkState getDestination(Grid& grid, const WalkState& _startState, float dist_m) { + WalkState getDestination(Grid& grid, const WalkState& _startState, 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,10 +45,26 @@ 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) + dist_m += _startState.distance.error_m; + + probability = 1.0; + int cnt = 0; + // 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 drawer(rnd); drawer.reserve(10); // evaluate each neighbor for (const Node& neighbor : grid.neighbors(*curNode)) { @@ -53,8 +72,12 @@ public: drawer.add(&neighbor, prob); } - // pick a neighbor - const Node* nextNode = drawer.get(); + // pick a neighbor and get its probability + double nodeProbability; + const Node* nextNode = drawer.get(nodeProbability); + if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one + //probability += nodeProbability; + //++cnt; // inform step(currentState, *curNode, *nextNode); @@ -64,12 +87,17 @@ public: curNode = nextNode; currentState.position = *curNode; - } + //if (cnt != 0) {probability /= cnt;} else {probability = 1.0;} + probability = curNode->getWalkImportance(); + // 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 +106,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* mdl : modules) { - mdl->updateBefore(state); + mdl->updateBefore(state, startNode); } } diff --git a/grid/walk/v2/modules/WalkModule.h b/grid/walk/v2/modules/WalkModule.h index 09403db..2d82dc9 100644 --- a/grid/walk/v2/modules/WalkModule.h +++ b/grid/walk/v2/modules/WalkModule.h @@ -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 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; diff --git a/grid/walk/v2/modules/WalkModuleActivityControl.h b/grid/walk/v2/modules/WalkModuleActivityControl.h new file mode 100644 index 0000000..f15e428 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleActivityControl.h @@ -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 class WalkModuleActivityControl : public WalkModule { + +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 diff --git a/grid/walk/v2/modules/WalkModuleButterActivity.h b/grid/walk/v2/modules/WalkModuleButterActivity.h index b110d1c..bc3f37c 100644 --- a/grid/walk/v2/modules/WalkModuleButterActivity.h +++ b/grid/walk/v2/modules/WalkModuleButterActivity.h @@ -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 class WalkModuleButterActivity : public WalkModule { @@ -16,11 +34,15 @@ public: /** ctor */ WalkModuleButterActivity() { - ; + + // ensure templates WalkState inherits from 'WalkStateBarometerActivity' + StaticAssert::AinheritsB(); + } - 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; diff --git a/grid/walk/v2/modules/WalkModuleFavorZ.h b/grid/walk/v2/modules/WalkModuleFavorZ.h index 59e01a6..eacc910 100644 --- a/grid/walk/v2/modules/WalkModuleFavorZ.h +++ b/grid/walk/v2/modules/WalkModuleFavorZ.h @@ -36,7 +36,7 @@ template 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); } diff --git a/grid/walk/v2/modules/WalkModuleFollowDestination.h b/grid/walk/v2/modules/WalkModuleFollowDestination.h index 7a065a6..556f9e4 100644 --- a/grid/walk/v2/modules/WalkModuleFollowDestination.h +++ b/grid/walk/v2/modules/WalkModuleFollowDestination.h @@ -2,39 +2,77 @@ #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 class WalkModuleFollowDestination : public WalkModule { private: + const Grid& grid; Dijkstra dijkstra; + const DijkstraNode* dnDest; - struct DijkstraMapper { + struct DijkstraAccess { const Grid& grid; - DijkstraMapper(const Grid& grid) : grid(grid) {;} + DijkstraAccess(const Grid& 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& grid, const Node& destination) { + /** ctor WITHOUT known destination*/ + WalkModuleFollowDestination(const Grid& grid) : grid(grid) { - // shortest path calculation - dijkstra.build(&destination, DijkstraMapper(grid)); + // ensure the template WalkState inherits from 'WalkStateFavorZ' + //StaticAssert::AinheritsB(); } - virtual void updateBefore(WalkState& state) override { + /** ctor WITH known destination*/ + WalkModuleFollowDestination(const Grid& 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 getShortestPath(const Node& start) { + const DijkstraNode* dnStart = dijkstra.getNode(start); + const DijkstraPath 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 +81,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* dnCur = dijkstra.getNode(curNode); - const DijkstraNode* dnNext = dijkstra.getNode(potentialNode); + const DijkstraNode* 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 diff --git a/grid/walk/v2/modules/WalkModuleHeading.h b/grid/walk/v2/modules/WalkModuleHeading.h index 02e6453..beb3687 100644 --- a/grid/walk/v2/modules/WalkModuleHeading.h +++ b/grid/walk/v2/modules/WalkModuleHeading.h @@ -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(); diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index a5b804c..2b31312 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -19,14 +19,12 @@ private: /** random noise */ Distribution::Normal distNoise; - Control* ctrl; - - //std::unordered_map errorTracker; + const Control* ctrl; public: /** ctor 3.0 should be OK! */ - WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) : + WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) : dist(Distribution::VonMises(0.0f, 2.0).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; } diff --git a/grid/walk/v2/modules/WalkModuleNodeImportance.h b/grid/walk/v2/modules/WalkModuleNodeImportance.h index 6801d98..7e62632 100644 --- a/grid/walk/v2/modules/WalkModuleNodeImportance.h +++ b/grid/walk/v2/modules/WalkModuleNodeImportance.h @@ -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; } diff --git a/grid/walk/v2/modules/WalkModulePreventVisited.h b/grid/walk/v2/modules/WalkModulePreventVisited.h index afbaaa0..3b3d674 100644 --- a/grid/walk/v2/modules/WalkModulePreventVisited.h +++ b/grid/walk/v2/modules/WalkModulePreventVisited.h @@ -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 { diff --git a/grid/walk/v2/modules/WalkModuleRelativePressureControl.h b/grid/walk/v2/modules/WalkModuleRelativePressureControl.h index 877a1ff..6e7853d 100644 --- a/grid/walk/v2/modules/WalkModuleRelativePressureControl.h +++ b/grid/walk/v2/modules/WalkModuleRelativePressureControl.h @@ -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 { diff --git a/grid/walk/v2/modules/WalkModuleSpread.h b/grid/walk/v2/modules/WalkModuleSpread.h index 0e933bb..b4fd4fc 100644 --- a/grid/walk/v2/modules/WalkModuleSpread.h +++ b/grid/walk/v2/modules/WalkModuleSpread.h @@ -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 { diff --git a/main.cpp b/main.cpp index c3a7960..255a441 100755 --- a/main.cpp +++ b/main.cpp @@ -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*"; diff --git a/math/Distributions.h b/math/Distributions.h index 138b72b..1879357 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -7,5 +7,6 @@ #include "distribution/Uniform.h" #include "distribution/VonMises.h" #include "distribution/Region.h" +#include "distribution/Triangle.h" #endif // DISTRIBUTIONS_H diff --git a/math/DrawList.h b/math/DrawList.h index 67119c5..2c2b994 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -21,8 +21,11 @@ template 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 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,20 @@ 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() { + T get(double& elemProbability) { // generate random number between [0:cumProbability] std::uniform_real_distribution<> dist(0, cumProbability); @@ -81,6 +106,7 @@ public: Assert::isFalse(tmp == elements.end(), "draw() did not find a valid element"); // done + elemProbability = (*tmp).probability; return (*tmp).element; } diff --git a/math/Random.h b/math/Random.h index 6c1393d..6dd8f8f 100644 --- a/math/Random.h +++ b/math/Random.h @@ -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) {;} }; diff --git a/math/distribution/Triangle.h b/math/distribution/Triangle.h new file mode 100644 index 0000000..dbffbff --- /dev/null +++ b/math/distribution/Triangle.h @@ -0,0 +1,50 @@ +#ifndef TRIANGLE_H +#define TRIANGLE_H + +#include +#include +#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 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 dist(mu, sigma); + return dist.getProbability(val); + } + + }; + +} + +#endif // TRIANGLE_H diff --git a/misc/Debug.h b/misc/Debug.h index 5897a64..4f1f1a3 100644 --- a/misc/Debug.h +++ b/misc/Debug.h @@ -6,6 +6,8 @@ #include #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 << "] "; } }; diff --git a/misc/log/Logger.h b/misc/log/Logger.h new file mode 100644 index 0000000..2513585 --- /dev/null +++ b/misc/log/Logger.h @@ -0,0 +1,17 @@ +#ifndef LOGGER_H +#define LOGGER_H + +#include + +/** base-class for all loggers */ +class Logger { + +public: + + virtual ~Logger() {;} + + virtual void add(const std::string& str, const bool nl) = 0; + +}; + +#endif // LOGGER_H diff --git a/misc/log/LoggerAndroid.h b/misc/log/LoggerAndroid.h new file mode 100644 index 0000000..bcb3f30 --- /dev/null +++ b/misc/log/LoggerAndroid.h @@ -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 diff --git a/misc/log/LoggerCOUT.h b/misc/log/LoggerCOUT.h new file mode 100644 index 0000000..c954986 --- /dev/null +++ b/misc/log/LoggerCOUT.h @@ -0,0 +1,18 @@ +#ifndef LOGGERCOUT_H +#define LOGGERCOUT_H + +#include "Logger.h" +#include + +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 diff --git a/misc/log/LoggerComposite.h b/misc/log/LoggerComposite.h new file mode 100644 index 0000000..b36bdd8 --- /dev/null +++ b/misc/log/LoggerComposite.h @@ -0,0 +1,30 @@ +#ifndef LOGGERCOMPOSITE_H +#define LOGGERCOMPOSITE_H + + +#include "Logger.h" +#include + +class LoggerComposite : public Logger { + +private: + + /** all contained loggers */ + std::vector 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 diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index 86d49f5..8ca5ccb 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -30,7 +30,7 @@ public: } /** get the dijkstra-pendant for the given user-node. null if none matches */ - DijkstraNode* getNode(const T& userNode) const { + const inline DijkstraNode* getNode(const T& userNode) const { auto it = nodes.find(&userNode); return (unlikely(it == nodes.end())) ? (nullptr) : (it->second); } diff --git a/nav/dijkstra/DijkstraPath.h b/nav/dijkstra/DijkstraPath.h index 0df19dc..9c0368e 100644 --- a/nav/dijkstra/DijkstraPath.h +++ b/nav/dijkstra/DijkstraPath.h @@ -20,6 +20,11 @@ private: public: + /** empty ctor */ + DijkstraPath() { + ; + } + /** ctor from end- to start-node */ DijkstraPath(const DijkstraNode* end, const DijkstraNode* start) { diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index cf98ca6..0f52963 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -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) {;} }; diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index 5fedce6..be9f262 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -2,6 +2,8 @@ #define ACCELEROMETERDATA_H #include +#include + /** 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 diff --git a/sensors/imu/GyroscopeData.h b/sensors/imu/GyroscopeData.h index f67e92a..de7e725 100644 --- a/sensors/imu/GyroscopeData.h +++ b/sensors/imu/GyroscopeData.h @@ -2,8 +2,12 @@ #define GYROSCOPEDATA_H #include +#include -/** 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 diff --git a/sensors/offline/OfflineAndroid.h b/sensors/offline/OfflineAndroid.h index 5aa0b30..6d8801d 100644 --- a/sensors/offline/OfflineAndroid.h +++ b/sensors/offline/OfflineAndroid.h @@ -12,6 +12,7 @@ #include "../radio/WiFiMeasurements.h" #include "../imu/AccelerometerData.h" #include "../imu/GyroscopeData.h" +#include "../pressure/BarometerData.h" template 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> accel; std::vector> gravity; + std::vector> barometer; + WalkedPath walkedPath; const char* name = "OfflineData"; @@ -82,6 +86,9 @@ public: /** get all gravity readings */ const std::vector>& getGravity() const {return gravity;} + /** get all barometer readings */ + const std::vector>& 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(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(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) { diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 34ad055..46a5323 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -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 butter = Filter::ButterworthLP(10,0.05f,2); + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.1f,2); Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.1f,2); FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); @@ -89,7 +89,7 @@ public: if(newInterpolatedValues == true){ //getActivity - if(output.size() > diffSize){ + if((int)output.size() > diffSize){ //diff std::vector diff; for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ diff --git a/sensors/radio/WiFiGridEstimator.h b/sensors/radio/WiFiGridEstimator.h index 3b117f8..0bb33a2 100644 --- a/sensors/radio/WiFiGridEstimator.h +++ b/sensors/radio/WiFiGridEstimator.h @@ -18,23 +18,23 @@ class WiFiGridEstimator { public: - /** - * convenience method - */ - template static void estimate(Grid& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) { +// /** +// * convenience method +// */ +// template static void estimate(Grid& grid, WiFiModel& mdl, const Floorplan::IndoorMap* im) { - // list of all APs - std::vector 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 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 static void estimate(Grid& grid, WiFiModel& mdl, const std::vector aps) { + template static void estimate(Grid& 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 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 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]; diff --git a/sensors/radio/WiFiGridNode.h b/sensors/radio/WiFiGridNode.h index 60472ac..2797b6f 100644 --- a/sensors/radio/WiFiGridNode.h +++ b/sensors/radio/WiFiGridNode.h @@ -3,6 +3,7 @@ #include #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 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 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); + } diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 72f158d..3a07386 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -9,7 +9,7 @@ */ class WiFiMeasurement { -private: +public: friend class VAPGrouper; diff --git a/sensors/radio/WiFiMeasurements.h b/sensors/radio/WiFiMeasurements.h index 8a6e619..a846251 100644 --- a/sensors/radio/WiFiMeasurements.h +++ b/sensors/radio/WiFiMeasurements.h @@ -13,6 +13,15 @@ struct WiFiMeasurements { /** all contained measurements */ std::vector 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 diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 1b9357b..a0783f5 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -4,6 +4,7 @@ #include "WiFiProbability.h" #include "model/WiFiModel.h" #include "../../math/Distributions.h" +#include "VAPGrouper.h" #include @@ -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::getProbability(modelRSSI, sigma, scanRSSI); + //prob *= Distribution::Region::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 double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const { - throw "todo??"; + throw Exception("todo??"); } }; diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index db071cf..35d8c7c 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -6,14 +6,17 @@ #include "../../math/Distributions.h" #include "../../data/Timestamp.h" +#include "WiFiGridNode.h" #include "WiFiProbability.h" +#include /** * 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 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 knownAPs; public: /** ctor with uncertainty */ WiFiObserverGrid(const float sigma) : sigma(sigma) { - ; + + //StaticAssert::AinheritsB(); + + for (const AccessPoint& ap : Node::getMapAPs()) { + knownAPs.insert(ap.getMAC()); + } + + Assert::isFalse(knownAPs.empty(), "no APs known to the grid nodes?!"); + } /** @@ -36,43 +48,66 @@ public: * compares the predicted signal-strengths stored on the given node * with the provided WiFi measurements */ - template 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(); + + double prob = 1; + int numMatchingAPs = 0; // 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::getProbability(measuredRSSI, sigma, modelRSSI); + //const double p = Distribution::Region::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 void dump(Grid& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) { + void dump(Grid& grid, const Timestamp curTime, const WiFiMeasurements& obs, const std::string& fileName) { std::ofstream out(fileName); out << "splot '-' with points palette\n"; diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index 90df451..05f336c 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -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 getAllAPs() const = 0; + /** * get the RSSI expected at the given location (in meter) * for an AP identified by the given MAC. diff --git a/sensors/radio/model/WiFiModelLogDist.h b/sensors/radio/model/WiFiModelLogDist.h index a86807f..417f19c 100644 --- a/sensors/radio/model/WiFiModelLogDist.h +++ b/sensors/radio/model/WiFiModelLogDist.h @@ -36,6 +36,13 @@ public: ; } + /** get a list of all APs known to the model */ + std::vector getAllAPs() const { + std::vector 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) { diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index 7f0ba3c..0ee8913 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -39,9 +39,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 +52,25 @@ public: } + /** get a list of all APs known to the model */ + std::vector getAllAPs() const { + std::vector 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); + } + } + + } + /** make the given AP (and its parameters) known to the model */ void addAP(const MACAddress& accessPoint, const APEntry& params) { @@ -57,11 +79,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!"); + // add accessPoints.insert( std::pair(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 +119,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 +149,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;} } diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h new file mode 100644 index 0000000..d0a2d6b --- /dev/null +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -0,0 +1,94 @@ +#ifndef WIFIFINGERPRINT_H +#define WIFIFINGERPRINT_H + + +#include "../../../geo/Point3.h" +#include "../WiFiMeasurements.h" + +#include + +/** + * 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 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 diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h new file mode 100644 index 0000000..bcd84d6 --- /dev/null +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -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 +#include +#include + +#include +#include + +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> 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 getAllMACs() const { + std::vector res; + for (const auto& it : apMap) {res.push_back(it.first);} + return res; + } + + /** optimize all known APs */ + std::vector optimizeAll() const { + + // sanity chekc + Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!"); + + float errSum = 0; + std::vector 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& 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; + const std::vector 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*) ¶ms); + +// using LeOpt = K::NumOptAlgoGenetic; +// 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 opt; +// opt.setMaxIterations(100); +// opt.setNumRestarts(10); + + opt.calculateOptimum(func, (float*) ¶ms); + errResult = func((float*)¶ms); + + Log::tock(); + Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err"); + + return params; + + } + + +}; + + +#endif // OPTIMIZER_H diff --git a/tests/grid/TestAll.cpp b/tests/grid/TestAll.cpp index c7e1b3a..b15abee 100644 --- a/tests/grid/TestAll.cpp +++ b/tests/grid/TestAll.cpp @@ -60,7 +60,7 @@ TEST(TestAll, Nav) { // plot path K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2); - DijkstraNode* dn = d.getNode(end); + const DijkstraNode* 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; diff --git a/tests/grid/TestGridWalk2HeadingControl.cpp b/tests/grid/TestGridWalk2HeadingControl.cpp index 4f58ffe..57242d5 100644 --- a/tests/grid/TestGridWalk2HeadingControl.cpp +++ b/tests/grid/TestGridWalk2HeadingControl.cpp @@ -104,7 +104,7 @@ TEST(GridWalk2HeadingControl, LIVE_walkHeading) { struct MyControl { - float turnAngle = 0; + float turnSinceLastTransition_rad = 0; } ctrl; @@ -139,7 +139,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(); diff --git a/tests/grid/TestGridWalkV2.cpp b/tests/grid/TestGridWalkV2.cpp index 822e368..cbf7357 100644 --- a/tests/grid/TestGridWalkV2.cpp +++ b/tests/grid/TestGridWalkV2.cpp @@ -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 walker; diff --git a/tests/grid/TestStairs.cpp b/tests/grid/TestStairs.cpp index b65282c..ae505dc 100644 --- a/tests/grid/TestStairs.cpp +++ b/tests/grid/TestStairs.cpp @@ -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() {;} diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp index 34854f3..8886d45 100644 --- a/tests/math/TestDistribution.cpp +++ b/tests/math/TestDistribution.cpp @@ -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::getProbability(0, 3, 1.2, x); + const float y = Distribution::Triangle::getProbability(3, 6, x); + out << x << " " << y << "\n"; + } + out.close(); + + Distribution::Triangle dist1(0, 3); + ASSERT_NEAR(1.0, distCheckArea(dist1, -20, +20), 0.01); + + Distribution::Triangle dist2(0, 1); + ASSERT_NEAR(1.0, distCheckArea(dist2, -20, +20), 0.01); + + Distribution::Triangle dist3(1, 3); + ASSERT_NEAR(1.0, distCheckArea(dist3, -20, +20), 0.01); + + Distribution::Triangle dist4(1, 2); + ASSERT_NEAR(1.0, distCheckArea(dist4, -20, +20), 0.01); + + Distribution::Triangle dist5(-1, 4); + ASSERT_NEAR(1.0, distCheckArea(dist5, -20, +20), 0.01); + + + } #endif diff --git a/tests/nav/dijkstra/TestDijkstra.cpp b/tests/nav/dijkstra/TestDijkstra.cpp index b68056a..6c002b0 100644 --- a/tests/nav/dijkstra/TestDijkstra.cpp +++ b/tests/nav/dijkstra/TestDijkstra.cpp @@ -34,15 +34,15 @@ TEST(Dijkstra, build) { d.build(&grid[idx5], &grid[idx3], tmp, 99999); // start node must be "idx5" - DijkstraNode* n = d.getNode(grid[idx5]); + const DijkstraNode* 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* n2 = d.getNode(grid[idx1]); + const DijkstraNode* 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* n3 = d.getNode(grid[idx3]); + const DijkstraNode* n3 = d.getNode(grid[idx3]); ASSERT_EQ(&grid[idx3], n3->element); ASSERT_EQ(&grid[idx1], n3->previous->element); diff --git a/tests/sensors/radio/TestLogDistanceCeilingModel.cpp b/tests/sensors/radio/TestLogDistanceCeilingModel.cpp index ca56ec1..f734d44 100644 --- a/tests/sensors/radio/TestLogDistanceCeilingModel.cpp +++ b/tests/sensors/radio/TestLogDistanceCeilingModel.cpp @@ -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 diff --git a/tests/sensors/radio/TestWiFiGrid.cpp b/tests/sensors/radio/TestWiFiGrid.cpp index 60ec5c6..ffec908 100644 --- a/tests/sensors/radio/TestWiFiGrid.cpp +++ b/tests/sensors/radio/TestWiFiGrid.cpp @@ -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 aps = { - AccessPoint(ap1), AccessPoint(ap2), AccessPoint(ap3), AccessPoint(ap4) - }; +// std::vector 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 observer(5.0f); const TestNode190231& gn = grid.getNodeFor(GridPoint(1000,1000,0)); const float p = observer.getProbability(gn, ts, obs); diff --git a/tests/sensors/radio/TestWiFiOptimizer.cpp b/tests/sensors/radio/TestWiFiOptimizer.cpp new file mode 100644 index 0000000..c640650 --- /dev/null +++ b/tests/sensors/radio/TestWiFiOptimizer.cpp @@ -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 + +/** + * 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 fingerprints; + for (int i = 0; i < 50; ++i) { + + std::uniform_real_distribution distX(-30, +30); + std::uniform_real_distribution distY(-30, +30); + std::uniform_real_distribution 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