From a9cd8d5e681a04cc683c51f40fbdcf0553a6feae Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 4 Apr 2017 16:50:11 +0200 Subject: [PATCH 1/6] small bug change --- floorplan/v2/FloorplanCeilings.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h index 857bfe8..658e64a 100644 --- a/floorplan/v2/FloorplanCeilings.h +++ b/floorplan/v2/FloorplanCeilings.h @@ -80,8 +80,8 @@ namespace Floorplan { #ifdef WITH_ASSERTIONS - static int numNear = 0; - static int numFar = 0; + static uint64_t numNear = 0; + static uint64_t 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;} From 200aa94ca80d8dbc0895c2e77765b77272b07de0 Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 17 Apr 2017 16:50:56 +0200 Subject: [PATCH 2/6] fixed some bugs in jensen shannon and kullback leibler --- main.cpp | 2 +- math/Random.h | 4 +- math/divergence/JensenShannon.h | 7 ++ math/divergence/KullbackLeibler.h | 4 +- sensors/radio/VAPGrouper.h | 2 +- tests/math/divergence/TestJensenShannon.cpp | 94 +++++++++++++++++++++ tests/math/divergence/TestJensenShannon.h | 0 7 files changed, 107 insertions(+), 6 deletions(-) create mode 100644 tests/math/divergence/TestJensenShannon.cpp create mode 100644 tests/math/divergence/TestJensenShannon.h diff --git a/main.cpp b/main.cpp index 2808d16..46f1192 100755 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; - ::testing::GTEST_FLAG(filter) = "*Angle*"; + ::testing::GTEST_FLAG(filter) = "*Jensen*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; diff --git a/math/Random.h b/math/Random.h index 6dd8f8f..ae8d8c5 100644 --- a/math/Random.h +++ b/math/Random.h @@ -18,10 +18,10 @@ class RandomGenerator : public std::minstd_rand { public: /** ctor with default seed */ - RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} /** ctor with custom seed */ - RandomGenerator(result_type seed) : std::minstd_rand(seed) {;} + RandomGenerator(result_type seed) : std::minstd_rand(seed) {;} }; diff --git a/math/divergence/JensenShannon.h b/math/divergence/JensenShannon.h index 5c9bbb3..df64088 100644 --- a/math/divergence/JensenShannon.h +++ b/math/divergence/JensenShannon.h @@ -18,6 +18,13 @@ public: * @param Q is a vector containg the densities of the same samples set then P */ static inline Scalar getGeneralFromSamples(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){ + // normalize + P /= P.sum(); + Q /= Q.sum(); + + Assert::isNear((double)P.sum(), 1.0, 0.01,"Normalization failed.. this shouldn't happen"); + Assert::isNear((double)Q.sum(), 1.0, 0.01, "Normalization failed.. this shouldn't happen"); + Eigen::VectorXd M = 0.5 * (P + Q); return (0.5 * KullbackLeibler::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler::getGeneralFromSamples(Q, M, mode)); diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h index 630e5d2..27124e7 100644 --- a/math/divergence/KullbackLeibler.h +++ b/math/divergence/KullbackLeibler.h @@ -56,8 +56,8 @@ namespace Divergence { // calc PQratio if(Q[i] == 0.0){ - Assert::doThrow("Division by zero is not allowed ;)."); - //PQratio = P[i] / 0.00001; + //Assert::doThrow("Division by zero is not allowed ;)."); + PQratio = P[i] / 0.00001; } else { PQratio = P[i] / Q[i]; } diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index b8a7545..98211e2 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -89,7 +89,7 @@ public: } - Log::add(name, "grouped " + std::to_string(original.entries.size()) + " measurements into " + std::to_string(result.entries.size()), true); + //Log::add(name, "grouped " + std::to_string(original.entries.size()) + " measurements into " + std::to_string(result.entries.size()), true); // done diff --git a/tests/math/divergence/TestJensenShannon.cpp b/tests/math/divergence/TestJensenShannon.cpp new file mode 100644 index 0000000..4bf8cf3 --- /dev/null +++ b/tests/math/divergence/TestJensenShannon.cpp @@ -0,0 +1,94 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/JensenShannon.h" +#include "../../../math/Distributions.h" + +#include + + +TEST(JensenShannon, generalFromSamples) { + //ge cov + Eigen::VectorXd mu1(2); + mu1[0] = 1.0; + mu1[1] = 1.0; + + Eigen::VectorXd mu2(2); + mu2[0] = 1.0; + mu2[1] = 1.0; + + Eigen::VectorXd mu3(2); + mu3[0] = 1.0; + mu3[1] = 1.0; + + Eigen::VectorXd mu4(2); + mu4[0] = 9.0; + mu4[1] = 9.0; + + Eigen::MatrixXd cov1(2,2); + cov1(0,0) = 1.0; + cov1(0,1) = 0.0; + cov1(1,0) = 0.0; + cov1(1,1) = 1.0; + + Eigen::MatrixXd cov2(2,2); + cov2(0,0) = 1.0; + cov2(0,1) = 0.0; + cov2(1,0) = 0.0; + cov2(1,1) = 1.0; + + Eigen::MatrixXd cov3(2,2); + cov3(0,0) = 1.0; + cov3(0,1) = 0.0; + cov3(1,0) = 0.0; + cov3(1,1) = 1.0; + + Eigen::MatrixXd cov4(2,2); + cov4(0,0) = 3.0; + cov4(0,1) = 0.0; + cov4(1,0) = 0.0; + cov4(1,1) = 13.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + int size = 10000; + Eigen::VectorXd samples1(size); + Eigen::VectorXd samples2(size); + Eigen::VectorXd samples3(size); + Eigen::VectorXd samples4(size); + + //random numbers + std::mt19937_64 rng; + // initialize the random number generator with time-dependent seed + uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)}; + rng.seed(ss); + // initialize a uniform distribution between 0 and 1 + std::uniform_real_distribution unif(-9, 10); + + //generate samples + for(int i = 0; i < size; ++i){ + + double r1 = unif(rng); + double r2 = unif(rng); + Eigen::VectorXd v(2); + v << r1, r2; + + samples1[i] = norm1.getProbability(v); + samples2[i] = norm2.getProbability(v); + samples3[i] = norm3.getProbability(v); + samples4[i] = norm4.getProbability(v); + } + + double kld12 = Divergence::JensenShannon::getGeneralFromSamples(samples1, samples2, Divergence::LOGMODE::NATURALIS); + double kld34 = Divergence::JensenShannon::getGeneralFromSamples(samples3, samples4, Divergence::LOGMODE::NATURALIS); + std::cout << kld34 << " > " << kld12 << std::endl; + + ASSERT_GE(kld34, kld12); + +} + +#endif diff --git a/tests/math/divergence/TestJensenShannon.h b/tests/math/divergence/TestJensenShannon.h new file mode 100644 index 0000000..e69de29 From 99f282180efdb3bfba25eb48d90a9e7bb5f30b1b Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 18 Apr 2017 11:15:06 +0200 Subject: [PATCH 3/6] commiting a hackgit diff! attention --- math/distribution/NormalN.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index bb332c4..b30c4d9 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -91,7 +91,12 @@ namespace Distribution { Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!"); const Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); - const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); + Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); + + //this is a hack + if(cov(2,2) < 0.1){ + cov(2,2) = 0.1; + } return NormalDistributionN(mean, cov); } From 8e295e25af636c35d4b7397372dfa757655ffd2e Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 19 Apr 2017 14:59:04 +0200 Subject: [PATCH 4/6] removed some hacks --- math/distribution/NormalN.h | 5 ----- math/divergence/KullbackLeibler.h | 4 ++-- sensors/radio/WiFiQualityAnalyzer.h | 4 ++-- 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index b30c4d9..5075dea 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -93,11 +93,6 @@ namespace Distribution { const Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); - //this is a hack - if(cov(2,2) < 0.1){ - cov(2,2) = 0.1; - } - return NormalDistributionN(mean, cov); } diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h index 27124e7..630e5d2 100644 --- a/math/divergence/KullbackLeibler.h +++ b/math/divergence/KullbackLeibler.h @@ -56,8 +56,8 @@ namespace Divergence { // calc PQratio if(Q[i] == 0.0){ - //Assert::doThrow("Division by zero is not allowed ;)."); - PQratio = P[i] / 0.00001; + Assert::doThrow("Division by zero is not allowed ;)."); + //PQratio = P[i] / 0.00001; } else { PQratio = P[i] / Q[i]; } diff --git a/sensors/radio/WiFiQualityAnalyzer.h b/sensors/radio/WiFiQualityAnalyzer.h index 994b2ec..0d09430 100644 --- a/sensors/radio/WiFiQualityAnalyzer.h +++ b/sensors/radio/WiFiQualityAnalyzer.h @@ -68,8 +68,8 @@ private: const float stdDev = std::sqrt(avg2 - avg*avg); // avg rssi score - const float minRSSI = -90; - const float maxRSSI = -65; + const float minRSSI = -90; + const float maxRSSI = -75; float score1 = (avg-minRSSI) / (maxRSSI-minRSSI); // min = 0; max = 1 if (score1 > 1) {score1 = 1;} if (score1 < 0) {score1 = 0;} From d40032ca7402fdc404a84707ecfb158a73181617 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 24 May 2017 09:23:27 +0200 Subject: [PATCH 5/6] interface changes added new data-strcutures for new sensors new helper methods fixed some issues --- data/HistoryTS.h | 68 +++++++++++++++++ floorplan/v2/FloorplanCeilings.h | 62 +++++++++++++--- floorplan/v2/FloorplanLINT.h | 17 ++++- grid/factory/v2/GridFactory.h | 2 +- grid/walk/v2/GridWalker.h | 3 - .../WalkModuleAbsoluteHeadingControl.h | 5 +- .../v2/modules/WalkModuleActivityControl.h | 2 +- math/Distributions.h | 1 + math/Interpolator.h | 13 +++- math/MovingAVG.h | 4 +- sensors/gps/GPSData.h | 7 +- sensors/offline/FileReader.h | 74 ++++++++++++++++--- sensors/pressure/PressureTendence.h | 6 +- sensors/radio/AccessPoint.h | 5 ++ sensors/radio/VAPGrouper.h | 26 ++++++- sensors/radio/WiFiMeasurements.h | 44 ++++++++++- sensors/radio/WiFiProbabilityFree.h | 74 ++++++++++++++++++- sensors/radio/WiFiQualityAnalyzer.h | 4 +- sensors/radio/model/WiFiModel.h | 8 +- sensors/radio/model/WiFiModelLogDist.h | 8 ++ sensors/radio/model/WiFiModelLogDistCeiling.h | 74 ++++++++++++++++++- sensors/radio/setup/WiFiFingerprint.h | 2 + sensors/radio/setup/WiFiFingerprints.h | 2 +- tests/grid/Plot.h | 8 +- tests/grid/TestAll.cpp | 2 +- tests/grid/TestGridWalkV2.cpp | 4 +- tests/grid/TestStairs.cpp | 4 +- tests/grid/TestWalk.cpp | 2 +- tests/math/filter/TestButter.cpp | 8 +- 29 files changed, 471 insertions(+), 68 deletions(-) create mode 100755 data/HistoryTS.h diff --git a/data/HistoryTS.h b/data/HistoryTS.h new file mode 100755 index 0000000..87e4291 --- /dev/null +++ b/data/HistoryTS.h @@ -0,0 +1,68 @@ +#ifndef HISTORYTS_H +#define HISTORYTS_H + +#include +#include "Timestamp.h" +#include + +/** + * keep the history of values for a given amount of time + */ +template class HistoryTS { + +private: + + /** timestamp -> value combination */ + struct Entry { + Timestamp ts; + T value; + Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;} + }; + + /** the time-window to keep */ + Timestamp window; + + /** the value history for the window-size */ + std::vector history; + +public: + + + /** ctor with the time-window to keep */ + HistoryTS(const Timestamp window) : window(window) { + + } + + /** add a new entry */ + void add(const Timestamp ts, const T& data) { + + // append to history + history.push_back(Entry(ts, data)); + + // remove too-old history entries + const Timestamp oldest = ts - window; + while(history.front().ts < oldest) { + + // remove from history + history.erase(history.begin()); + + } + + } + + /** get the most recent entry */ + T getMostRecent() const { + return history.back().value; + } + + /** get the oldest entry available */ + T getOldest() const { + return history.front().value; + } + + + +}; + + +#endif // HISTORYTS_H diff --git a/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h index 857bfe8..b86e22e 100644 --- a/floorplan/v2/FloorplanCeilings.h +++ b/floorplan/v2/FloorplanCeilings.h @@ -46,28 +46,62 @@ namespace Floorplan { } - /** get the number of ceilings between z1 and z2 */ - float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const { + std::vector getCeilings() const { + return ceilingsAtHeight_m; + } + + void addCeiling(const float height_m) { + ceilingsAtHeight_m.push_back(height_m); + } + + void clear() { + ceilingsAtHeight_m.clear(); + } + + /** get a fading number of floors between ap and person using sigmod in the area where the ceiling is */ + float numCeilingsBetweenFloat(const Point3 ap, const Point3 person) const { // sanity checks Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?"); - const float zMin = std::min(pos1.z, pos2.z); - const float zMax = std::max(pos1.z, pos2.z); - + // fading between floors using sigmoid + const float near = 1.0; float cnt = 0; + for (const float z : ceilingsAtHeight_m) { - if (zMin < z && zMax > z) { - const float dmax = zMax - z; - cnt += (dmax > 1) ? (1) : (dmax); + + const float myDistToCeil = (ap.z < person.z) ? (person.z - z) : (z - person.z); + if ( std::abs(myDistToCeil) < near ) { + cnt += sigmoid(myDistToCeil * 6); + } else if (ap.z < z && person.z >= z+near) { // AP below celing, me above ceiling + cnt += 1; + } else if (ap.z > z && person.z <= z-near) { // AP above ceiling, me below ceiling + cnt += 1; } + } return cnt; } + float numCeilingsBetweenLinearInt(const Point3 ap, const Point3 person) const { + + // sanity checks + Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?"); + + int cnt = 0; + float sum = 0; + for (float z = -1.0; z <= +1.0; z+= 0.25) { + sum += numCeilingsBetween(ap, person + Point3(0,0,z)); + ++cnt; + } + + return sum/cnt; + + } + /** get the number of ceilings between z1 and z2 */ int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const { @@ -80,14 +114,14 @@ namespace Floorplan { #ifdef WITH_ASSERTIONS - static int numNear = 0; - static int numFar = 0; + static size_t numNear = 0; + static size_t 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.15, + Assert::isTrue(numNear < numFar*0.5, "many requests to Floorplan::Ceilings::numCeilingsBetween 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! \ @@ -105,6 +139,12 @@ namespace Floorplan { } + private: + + static inline float sigmoid(const float val) { + return 1.0f / (1.0f + std::exp(-val)); + } + }; } diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index 34ca7b8..719fb65 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -97,6 +97,11 @@ namespace Floorplan { checkStair(res, floor, s); } + // check elevators + for (const Elevator* e : floor->elevators) { + checkElevator(res, floor, e); + } + } // done @@ -190,7 +195,7 @@ namespace Floorplan { for (int i = 0; i < (int) parts.size(); ++i) { - const Floorplan::Quad3& quad = quads[i]; + //const Floorplan::Quad3& quad = quads[i]; // disconnected within? if (i > 0) { @@ -201,7 +206,17 @@ namespace Floorplan { } + } + static void checkElevator(Issues& res, const Floor* floor, const Elevator* e) { + + if (e->depth < 0.5) { + res.push_back(Issue(Type::ERROR, floor, "elevator's depth @" + e->center.asString() + " is too small: " + std::to_string(e->depth) + "m")); + } + + if (e->width < 0.5) { + res.push_back(Issue(Type::ERROR, floor, "elevator's width @" + e->center.asString() + " is too small: " + std::to_string(e->width) + "m")); + } } diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 0303c51..589dd5d 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -121,7 +121,7 @@ public: }; /** get the part of outline the given location belongs to. currently: none, indoor, outdoor */ - PartOfOutline isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) { + static PartOfOutline isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) { // assume the point is not part of the outline PartOfOutline res = PartOfOutline::NO; diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 59f51e9..eaf10de 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -111,9 +111,6 @@ public: Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?"); probability *= walkImportance;// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) - - //probability = std::pow(probability, 5); - // sanity check Assert::isNotNaN(probability, "detected NaN grid-walk probability"); diff --git a/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h index d3987f9..168cc94 100644 --- a/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h @@ -74,9 +74,8 @@ public: // get the difference const float angularDiff = head.getDiffHalfRAD(stateHead); - if (angularDiff > Angle::degToRad(180)) {return 0.05;} - if (angularDiff > Angle::degToRad(90)) {return 0.25;} - {return 0.70;} + if (angularDiff > Angle::degToRad(100)) {return 0.10;} + {return 0.90;} } diff --git a/grid/walk/v2/modules/WalkModuleActivityControl.h b/grid/walk/v2/modules/WalkModuleActivityControl.h index f15e428..95121ed 100644 --- a/grid/walk/v2/modules/WalkModuleActivityControl.h +++ b/grid/walk/v2/modules/WalkModuleActivityControl.h @@ -100,7 +100,7 @@ public: // if (potentialNode.getType() == GridNode::TYPE_FLOOR) {return kappa;} // {return 1-kappa;} default: - throw Exception("not yet implemented"); + throw Exception("not yet implemented activity within WalkModuleActivityControl::getProbability"); } } diff --git a/math/Distributions.h b/math/Distributions.h index 93d35c8..1f2bb44 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -9,5 +9,6 @@ #include "distribution/Region.h" #include "distribution/Triangle.h" #include "distribution/NormalN.h" +#include "distribution/Rectangular.h" #endif // DISTRIBUTIONS_H diff --git a/math/Interpolator.h b/math/Interpolator.h index 3ead1bf..d362949 100644 --- a/math/Interpolator.h +++ b/math/Interpolator.h @@ -3,6 +3,7 @@ #include "../Assertions.h" #include "../Exception.h" +#include "../data/Timestamp.h" #include #include @@ -49,7 +50,7 @@ public: // interpolate const int idx1 = (idx2 > 0) ? (idx2 - 1) : (idx2); - const float percent = (key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key); + const float percent = getPercent(key, entries[idx1].key, entries[idx2].key); //(key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key); const Value res = entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent; return res; @@ -57,6 +58,16 @@ public: protected: + /** special interpolation for the timestamp class */ + static inline float getPercent(const Timestamp key, const Timestamp t1, const Timestamp t2) { + return (key - t1).ms() / (float) (t2 - t1).ms(); + } + + /** interpolation for generic datatypes [int, float, double, ..] */ + template static inline float getPercent(const T key, const T t1, const T t2) { + return (key - t1) / (float) (t2 - t1); + } + /** get the nearest index for the given key */ int getIdxAfter(const Key key) const { diff --git a/math/MovingAVG.h b/math/MovingAVG.h index d3c0ee3..95d1488 100644 --- a/math/MovingAVG.h +++ b/math/MovingAVG.h @@ -11,7 +11,7 @@ private: std::vector values; /** track the current sum of the vector's values */ - T curSum = 0; + T curSum; /** the number of elements to average */ int size; @@ -19,7 +19,7 @@ private: public: /** ctor */ - MovingAVG(const int size) : size(size) {;} + MovingAVG(const int size) : curSum(), size(size) {;} /** add a new value */ void add(const T val) { diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index 7d63cac..80f4360 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -2,7 +2,7 @@ #define GPSDATA_H #include "../../data/Timestamp.h" - +#include "../../geo/EarthPos.h" struct GPSData { @@ -25,6 +25,11 @@ struct GPSData { /** ctor */ 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) {;} + /** get as EarthPos struct */ + EarthPos toEarthPos() const { + return EarthPos(lat, lon, alt); + } + /** data valid? */ bool isValid() const { return (lat == lat) && (lon == lon); diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 60b8e47..67ca235 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -21,6 +21,7 @@ #include "../../grid/factory/v2/GridFactory.h" #include "../../grid/factory/v2/Importance.h" #include "../../floorplan/v2/Floorplan.h" +#include "../../floorplan/v2/FloorplanHelper.h" #include "Splitter.h" #include "Sensors.h" @@ -30,10 +31,16 @@ namespace Offline { + /** + * read and parse previously recorded ["offline"] files + */ class FileReader { public: + using GroundTruth = Interpolator; + + /** all entries grouped by sensor */ std::vector> groundTruth; std::vector> wifi; std::vector> beacon; @@ -45,13 +52,11 @@ namespace Offline { std::vector> gps; std::vector> compass; - /** ALL entries */ + /** all entries in linear order as they appeared while recording */ std::vector entries; static constexpr char sep = ';'; - Listener* listener = nullptr; - public: /** empty ctor. call open() */ @@ -65,11 +70,26 @@ namespace Offline { } /** open the given file */ - void open(const std::string& file, Listener* listener = nullptr) { - this->listener = listener; + void open(const std::string& file) { + clear(); parse(file); } + /** remove all parsed entries */ + void clear() { + entries.clear(); + groundTruth.clear(); + wifi.clear(); + beacon.clear(); + acc.clear(); + gyro.clear(); + gps.clear(); + compass.clear(); + barometer.clear(); + lin_acc.clear(); + gravity.clear(); + } + const std::vector& getEntries() const {return entries;} @@ -93,6 +113,35 @@ namespace Offline { const std::vector>& getGravity() const {return gravity;} + /** get an interpolateable ground-truth based on the time-clicks during recording */ + GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector groundTruthPoints) const { + + // sanity check: given path [indices to ground-truth points within the map] + // must have the same size as the number of clicks during recording + Assert::equal(groundTruthPoints.size(), groundTruth.size(), "mismatch of ground-truth points between given path and recording"); + + // allows getting a position on the ground-truth given a timestamp + GroundTruth interpol; + + // all ground-truth points within the map + static std::unordered_map gt = FloorplanHelper::getGroundTruthPoints(map); + + // process each "tap smartphone when reaching ground-truth-point" + for (const TS& entry : groundTruth) { + const Timestamp ts = Timestamp::fromMS(entry.ts); + const int idx = entry.data; // starting at 0, incrementing over time [1st point, 2nd points, 3d points, ...] + const int id = groundTruthPoints[idx]; // convert point number to point-id within floorplan + const auto& it = gt.find(id); + if (it == gt.end()) {throw Exception("missing ground-truth point ID:" + std::to_string(id));} + const Point3 pos = it->second; + interpol.add(ts, pos); + } + + // done + return interpol; + + } + private: void parse(const std::string& file) { @@ -165,7 +214,7 @@ namespace Offline { entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); // inform listener - if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);} + //if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);} } @@ -184,7 +233,7 @@ namespace Offline { entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); // inform listener - if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);} + //if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);} } @@ -203,7 +252,7 @@ namespace Offline { entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); // inform listener - if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);} + //if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);} } @@ -230,7 +279,7 @@ namespace Offline { entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1)); // inform listener - if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);} + //if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);} } @@ -273,7 +322,7 @@ namespace Offline { entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); // inform listener - if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);} + //if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);} } @@ -290,7 +339,7 @@ namespace Offline { entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1)); // inform listener - if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);} + //if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);} } @@ -311,12 +360,13 @@ namespace Offline { entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1)); // inform listener - if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);} + //if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);} } public: + const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { // finde alle positionen der waypoints im gtPath aus map diff --git a/sensors/pressure/PressureTendence.h b/sensors/pressure/PressureTendence.h index 5a6f22a..7916158 100644 --- a/sensors/pressure/PressureTendence.h +++ b/sensors/pressure/PressureTendence.h @@ -51,9 +51,9 @@ public: K::GnuplotPlotElementLines tendence; Debug() { - plot.add(&raw); raw.setColorHex("#999999"); - plot.add(&avg); avg.setColorHex("#000000"); - plot.add(&tendence); tendence.setLineWidth(2); + plot.add(&raw); raw.getStroke().getColor().setHexStr("#999999"); + plot.add(&avg); avg.getStroke().getColor().setHexStr("#000000"); + plot.add(&tendence); tendence.getStroke().setWidth(2); tendence.setCustomAttr(" axes x1y2 "); gp << "set y2tics\n"; gp << "set y2range[-0.3:+0.3]\n"; diff --git a/sensors/radio/AccessPoint.h b/sensors/radio/AccessPoint.h index f46581e..839793c 100644 --- a/sensors/radio/AccessPoint.h +++ b/sensors/radio/AccessPoint.h @@ -45,6 +45,11 @@ public: ; } + /** equals? */ + bool operator == (const AccessPoint& o) { + return (o.mac == mac) && (o.ssid == ssid); + } + public: /** get the AP's MAC address */ diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index b8a7545..89906a2 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -50,18 +50,27 @@ private: /** the signal-strength aggregation algorithm to use */ const Aggregation agg; + /** respect only outputs with at-least X occurences of one physical hardware [can be used to prevent issues] */ + int minOccurences; + public: /** ctor */ - VAPGrouper(const Mode mode, const Aggregation agg) : mode(mode), agg(agg) { - + VAPGrouper(const Mode mode, const Aggregation agg, const int minOccurences = 2) : + mode(mode), agg(agg), minOccurences(minOccurences) { + ; } + /** set the number of needed occurences per VAP-group to be accepted */ + void setMinOccurences(const int min) { + this->minOccurences = min; + } /** get a vap-grouped version of the given input */ WiFiMeasurements group(const WiFiMeasurements& original) const { // first, group all VAPs into a vector [one vector per VAP-group] + // by using the modified MAC address that all VAPs have in common std::unordered_map> grouped; for (const WiFiMeasurement& m : original.entries) { @@ -72,8 +81,9 @@ public: } - // output + // to-be-constructed output WiFiMeasurements result; + int skipped = 0; // perform aggregation on each VAP-group for (auto it : grouped) { @@ -81,6 +91,9 @@ public: const MACAddress& base = it.first; const std::vector& vaps = it.second; + // remove entries that do not satify the min-occurences metric + if ((int)vaps.size() < minOccurences) {++skipped; continue;} + // group all VAPs into one measurement const WiFiMeasurement groupedMeasurement = groupVAPs(base, vaps); @@ -89,7 +102,12 @@ public: } - Log::add(name, "grouped " + std::to_string(original.entries.size()) + " measurements into " + std::to_string(result.entries.size()), true); + // debug + Log::add(name, + "grouped " + std::to_string(original.entries.size()) + " measurements " + + "into " + std::to_string(result.entries.size()) + " [omitted: " + std::to_string(skipped) + "]", + true + ); // done diff --git a/sensors/radio/WiFiMeasurements.h b/sensors/radio/WiFiMeasurements.h index 267075b..412883a 100644 --- a/sensors/radio/WiFiMeasurements.h +++ b/sensors/radio/WiFiMeasurements.h @@ -23,8 +23,8 @@ struct WiFiMeasurements { } /** get the measurements for the given MAC [if available] otherwise null */ - WiFiMeasurement* getForMac(const MACAddress& mac) { - for (WiFiMeasurement& m : entries) { + const WiFiMeasurement* getForMac(const MACAddress& mac) const { + for (const WiFiMeasurement& m : entries) { if (m.getAP().getMAC() == mac) { return &m; } @@ -32,6 +32,46 @@ struct WiFiMeasurements { return nullptr; } + /** remove the entry for the given MAC (if any) */ + void remove(const MACAddress& mac) { + for (size_t i = 0; i < entries.size(); ++i) { + if (entries[i].getAP().getMAC() == mac) { + entries.erase(entries.begin() + i); + break; + } + } + } + + + /** create a combination */ + static WiFiMeasurements mix(const WiFiMeasurements& a, const WiFiMeasurements& b, float sec = 3) { + + Timestamp max; + WiFiMeasurements res; + + for (const WiFiMeasurement& m : a.entries) { + res.entries.push_back(m); + if (m.getTimestamp() > max) {max = m.getTimestamp();} + } + + for (const WiFiMeasurement& m : b.entries) { + res.entries.push_back(m); + if (m.getTimestamp() > max) {max = m.getTimestamp();} + } + + std::vector tmp; + std::swap(res.entries, tmp); + + for (const WiFiMeasurement& m : tmp) { + if ((max - m.getTimestamp()).sec() < sec) { + res.entries.push_back(m); + } + } + + return res; + + } + }; #endif // WIFIMEASUREMENTS_H diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 5f3b5ac..360f9b0 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -27,15 +27,25 @@ private: /** the map's floorplan */ Floorplan::IndoorMap* map; + std::vector allAPs; + + bool useError = false; + public: WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) { + allAPs = model.getAllAPs(); + } + void setUseError(const bool useError) { + this->useError = useError; } double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const { double prob = 1.0; + //double prob = 0; + int numMatchingAPs = 0; // process each measured AP @@ -50,7 +60,6 @@ public: // NaN? -> AP not known to the model -> skip if (modelRSSI != modelRSSI) {continue;} - // the scan's RSSI const float scanRSSI = entry.getRSSI(); @@ -60,24 +69,81 @@ public: Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense"); Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?"); + Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range"); + //Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range"); + // sigma grows with measurement age const float sigma = this->sigma + this->sigmaPerSecond * age.sec(); + // probability for this AP + double local = Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); + //double local = Distribution::Exponential::getProbability(0.1, std::abs(modelRSSI-scanRSSI)); + + // also add the error value? [location is OK but model is wrong] + if (useError) { + local = 0.95 * local + 0.05 * (1.0-local); + //local = 0.95 * local + 0.05; +#warning "TODO" + } + // update probability - prob *= Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); - //prob *= Distribution::Region::getProbability(modelRSSI, sigma, scanRSSI); + prob *= local; ++numMatchingAPs; } // sanity check - //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); + //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); return prob; } + /** + * for some locations it might make sense to also look at APs + * that have NOT been discovered by the smartphone but SHOULD + * be very well receivable at a location. + * such locations can be opted-out by using this veto-probability + */ + double getVeto(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const { + + (void) curTime; + + struct APR { + AccessPoint ap; + float rssi; + APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;} + }; + + std::vector all; + for (const AccessPoint& ap : allAPs) { + const float rssi = model.getRSSI(ap.getMAC(), pos_m); + if (rssi != rssi) {continue;} // unknown to the model + all.push_back(APR(ap, rssi)); + } + + // stort by RSSI + auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;}; + std::sort(all.begin(), all.end(), comp); + + int numVetos = 0; + + for (int i = 0; i < 3; ++i) { + const APR& apr = all[i]; + const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC()); + const float rssiScan = (mes) ? (mes->getRSSI()) : (-100); + const float rssiModel = apr.rssi; + const float diff = std::abs(rssiScan - rssiModel); + if (diff > 20) {++numVetos;} + } + + if (numVetos == 0) {return 0.70;} + if (numVetos == 1) {return 0.29;} + else {return 0.01;} + + } + template double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const { return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs); diff --git a/sensors/radio/WiFiQualityAnalyzer.h b/sensors/radio/WiFiQualityAnalyzer.h index 994b2ec..522b8cf 100644 --- a/sensors/radio/WiFiQualityAnalyzer.h +++ b/sensors/radio/WiFiQualityAnalyzer.h @@ -68,8 +68,8 @@ private: const float stdDev = std::sqrt(avg2 - avg*avg); // avg rssi score - const float minRSSI = -90; - const float maxRSSI = -65; + const float minRSSI = -85; + const float maxRSSI = -70; float score1 = (avg-minRSSI) / (maxRSSI-minRSSI); // min = 0; max = 1 if (score1 > 1) {score1 = 1;} if (score1 < 0) {score1 = 0;} diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index f6603c4..cb2e975 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -3,6 +3,11 @@ #include "../AccessPoint.h" #include "../../../geo/Point3.h" +#include + + +#include "../../../data/XMLserialize.h" + /** * interface for signal-strength prediction models. @@ -10,7 +15,7 @@ * the model is passed a MAC-address of an AP in question, and a position. * hereafter the model returns the RSSI for this AP at the questioned location. */ -class WiFiModel { +class WiFiModel : public XMLserialize { public: @@ -31,6 +36,7 @@ public: */ virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0; + }; #endif // WIFIMODEL_H diff --git a/sensors/radio/model/WiFiModelLogDist.h b/sensors/radio/model/WiFiModelLogDist.h index e30158b..9affa1b 100644 --- a/sensors/radio/model/WiFiModelLogDist.h +++ b/sensors/radio/model/WiFiModelLogDist.h @@ -77,6 +77,14 @@ public: } + void readFromXML(XMLDoc* doc, XMLElem* src) override { + throw Exception("not yet implemented"); + } + + void writeToXML(XMLDoc* doc, XMLElem* dst) override { + throw Exception("not yet implemented"); + } + }; #endif // WIFIMODELLOGDIST_H diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index 37d80e4..f024251 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -10,6 +10,8 @@ #include "../VAPGrouper.h" #include "../../../misc/Debug.h" +#include "../../../data/XMLserialize.h" + /** * signal-strength estimation using log-distance model * including ceilings between AP and position @@ -52,6 +54,13 @@ public: } + /** get the entry for the given mac. exception if missing */ + const APEntry& getAP(const MACAddress& mac) const { + const auto& it = accessPoints.find(mac); + if (it == accessPoints.end()) {throw Exception("model does not contain an entry for " + mac.asString());} + return it->second; + } + /** get a list of all APs known to the model */ std::vector getAllAPs() const { std::vector aps; @@ -109,6 +118,14 @@ public: } + /** + * make the given AP (and its parameters) known to the model + * usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false! + */ + void addAP(const MACAddress& accessPoint, const Point3 pos_m, const float txp, const float exp, const float waf, const bool assertSafe = true) { + addAP(accessPoint, APEntry(pos_m, txp, exp, waf), assertSafe); + } + /** remove all added APs */ void clear() { accessPoints.clear(); @@ -130,7 +147,9 @@ public: const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m); // WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value - const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m); + //const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m); + const float wafLoss = params.waf * ceilings.numCeilingsBetweenFloat(position_m, params.position_m); + //const float wafLoss = params.waf * ceilings.numCeilingsBetweenLinearInt(position_m, params.position_m); // combine const float res = rssiLOS + wafLoss; @@ -143,6 +162,59 @@ public: } + void writeToXML(XMLDoc* doc, XMLElem* dst) override { + + // set my type + dst->SetAttribute("type", "WiFiModelLogDistCeiling"); + + for (const auto& it : accessPoints) { + const MACAddress& mac = it.first; + const APEntry& ape = it.second; + XMLElem* xap = doc->NewElement("ap"); + xap->SetAttribute("mac", mac.asString().c_str()); + xap->SetAttribute("px", ape.position_m.x); + xap->SetAttribute("py", ape.position_m.y); + xap->SetAttribute("pz", ape.position_m.z); + xap->SetAttribute("txp", ape.txp); + xap->SetAttribute("exp", ape.exp); + xap->SetAttribute("waf", ape.waf); + dst->InsertEndChild(xap); + } + + for (const float atHeight_m : ceilings.getCeilings()) { + XMLElem* xceil = doc->NewElement("ceiling"); + xceil->SetAttribute("atHeight", atHeight_m); + dst->InsertEndChild(xceil); + } + + } + + void readFromXML(XMLDoc* doc, XMLElem* src) override { + + // check type + if (std::string("WiFiModelLogDistCeiling") != src->Attribute("type")) {throw Exception("invalid model type");} + + accessPoints.clear(); + ceilings.clear(); + + XML_FOREACH_ELEM_NAMED("ap", xap, src) { + MACAddress mac = MACAddress(xap->Attribute("mac")); + APEntry ape( + Point3(xap->FloatAttribute("px"), xap->FloatAttribute("py"), xap->FloatAttribute("pz")), + xap->FloatAttribute("txp"), + xap->FloatAttribute("exp"), + xap->FloatAttribute("waf") + ); + accessPoints.insert( std::make_pair(mac, ape) ); + } + + XML_FOREACH_ELEM_NAMED("ceiling", xceil, src) { + const float atHeight_m = xceil->FloatAttribute("atHeight"); + ceilings.addCeiling(atHeight_m); + } + + } + }; diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index 2938c5d..b380faf 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -29,6 +29,8 @@ struct WiFiFingerprint { /** ctor */ WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;} + /** ctor */ + WiFiFingerprint(const Point3 pos_m, const WiFiMeasurements& measurements) : pos_m(pos_m), measurements(measurements) {;} /** as each AP might be contained more than once (scanned more than once), group them by MAC and use the average RSSI */ diff --git a/sensors/radio/setup/WiFiFingerprints.h b/sensors/radio/setup/WiFiFingerprints.h index 8745b2b..85da2cf 100644 --- a/sensors/radio/setup/WiFiFingerprints.h +++ b/sensors/radio/setup/WiFiFingerprints.h @@ -4,7 +4,7 @@ #include #include "WiFiFingerprint.h" - +#include /** * helper class to load and save fingerprints. diff --git a/tests/grid/Plot.h b/tests/grid/Plot.h index 02877ca..f02d9a3 100644 --- a/tests/grid/Plot.h +++ b/tests/grid/Plot.h @@ -49,12 +49,12 @@ public: //gp << "set hidden3d front\n"; splot.add(&nodes); nodes.setPointSize(0.5); - splot.add(&lines); lines.setColorHex("#999999"); + splot.add(&lines); lines.getStroke().getColor().setHexStr("#999999"); splot.add(&floors); - splot.add(&particles); particles.setColorHex("#0000ff"); particles.setPointSize(0.5); + splot.add(&particles); particles.getColor().setHexStr("#0000ff"); particles.setPointSize(0.5); - floors.setLineWidth(2); - floors.setColorHex("#008800"); + floors.getStroke().setWidth(2); + floors.getStroke().getColor().setHexStr("#008800"); } diff --git a/tests/grid/TestAll.cpp b/tests/grid/TestAll.cpp index b15abee..baed8e0 100644 --- a/tests/grid/TestAll.cpp +++ b/tests/grid/TestAll.cpp @@ -59,7 +59,7 @@ TEST(TestAll, Nav) { gi.addImportance(g, d.getNode(start), d.getNode(end)); // plot path - K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2); + K::GnuplotSplotElementLines path; path.getStroke().getColor().setHexStr("#0000ff"); path.getStroke().setWidth(2); 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)); diff --git a/tests/grid/TestGridWalkV2.cpp b/tests/grid/TestGridWalkV2.cpp index cbf7357..cecf1b2 100644 --- a/tests/grid/TestGridWalkV2.cpp +++ b/tests/grid/TestGridWalkV2.cpp @@ -131,8 +131,8 @@ TEST(GgridWalk2, LIVE_walkHeading) { K::Gnuplot gp; K::GnuplotSplot splot; - K::GnuplotSplotElementPoints nodes; nodes.setPointSize(0.1); nodes.setColorHex("#888888"); splot.add(&nodes); - K::GnuplotSplotElementPoints states; states.setPointSize(0.5); states.setColorHex("#0000ff"); splot.add(&states); + K::GnuplotSplotElementPoints nodes; nodes.setPointSize(0.1); nodes.getColor().setHexStr("#888888"); splot.add(&nodes); + K::GnuplotSplotElementPoints states; states.setPointSize(0.5); states.getColor().setHexStr("#0000ff"); splot.add(&states); for (const MyNode1239& n : grid) { static int cnt = 0; diff --git a/tests/grid/TestStairs.cpp b/tests/grid/TestStairs.cpp index ae505dc..30aebe3 100644 --- a/tests/grid/TestStairs.cpp +++ b/tests/grid/TestStairs.cpp @@ -50,8 +50,8 @@ TEST(Stairs, live_testWalk) { K::Gnuplot gp; K::GnuplotSplot splot; - K::GnuplotSplotElementPoints pnodes; splot.add(&pnodes); pnodes.setColorHex("#888888"); pnodes.setPointType(7); - K::GnuplotSplotElementPoints pstates; splot.add(&pstates); pstates.setColorHex("#0000ff"); pstates.setPointType(7); pstates.setPointSize(1); + K::GnuplotSplotElementPoints pnodes; splot.add(&pnodes); pnodes.getColor().setHexStr("#888888"); pnodes.setPointType(7); + K::GnuplotSplotElementPoints pstates; splot.add(&pstates); pstates.getColor().setHexStr("#0000ff"); pstates.setPointType(7); pstates.setPointSize(1); for (int i = 0; i < g.getNumNodes(); i+=2) { const MyNode345092134& gp = g[i]; diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp index 91eb480..53af77b 100644 --- a/tests/grid/TestWalk.cpp +++ b/tests/grid/TestWalk.cpp @@ -101,7 +101,7 @@ TEST(Walk, DISABLED_plot) { std::normal_distribution dWalk(0.3, 0.3); std::normal_distribution dTurn(0.3, 0.3); - K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000"); + K::GnuplotSplotElementPoints pStates; pStates.getColor().setHexStr("#880000"); // setup starting states std::vector> states; diff --git a/tests/math/filter/TestButter.cpp b/tests/math/filter/TestButter.cpp index 750909d..1946e1a 100644 --- a/tests/math/filter/TestButter.cpp +++ b/tests/math/filter/TestButter.cpp @@ -57,7 +57,7 @@ TEST(Butterworth, offlineSinus) { linesInput.addSegment(input_p1, input_p2); linesOutput.addSegment(output_p1, output_p2); } - linesOutput.setColorHex("#00FF00"); + linesOutput.getStroke().getColor().setHexStr("#00FF00"); plot.add(&linesInput); plot.add(&linesOutput); @@ -104,7 +104,7 @@ TEST(Butterworth, onlineSinus) { linesInput.addSegment(input_p1, input_p2); linesOutput.addSegment(output_p1, output_p2); } - linesOutput.setColorHex("#00FF00"); + linesOutput.getStroke().getColor().setHexStr("#00FF00"); plot.add(&linesInput); plot.add(&linesOutput); @@ -171,7 +171,7 @@ TEST(Butterworth, offlineOctaveBaro) { linesInput.addSegment(input_p1, input_p2); linesOutput.addSegment(output_p1, output_p2); } - linesOutput.setColorHex("#00FF00"); + linesOutput.getStroke().getColor().setHexStr("#00FF00"); plot.add(&linesInput); plot.add(&linesOutput); @@ -242,7 +242,7 @@ TEST(Butterworth, onlineOctaveBaro) { linesInput.addSegment(input_p1, input_p2); linesOutput.addSegment(output_p1, output_p2); } - linesOutput.setColorHex("#00FF00"); + linesOutput.getStroke().getColor().setHexStr("#00FF00"); plot.add(&linesInput); plot.add(&linesOutput); From 0864f55a54df624e7893b12574b127dde58100b6 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 24 May 2017 09:32:05 +0200 Subject: [PATCH 6/6] added support for XML reading/writing new serialization interfaces new helper methods new wifi models --- data/XMLload.h | 14 ++ data/XMLsave.h | 14 ++ data/XMLserialize.h | 44 ++++++ data/xml.h | 33 ++++ debug/PlotWifiMeasurements.h | 76 ++++++++++ geo/BBoxes3.h | 45 ++++++ math/MovingStdDevTS.h | 49 ++++++ math/distribution/Rectangular.h | 46 ++++++ sensors/activity/Activity.h | 20 +++ sensors/activity/ActivityDetector.h | 161 ++++++++++++++++++++ sensors/gps/GPSProbability.h | 65 ++++++++ sensors/offline/FilePlayer.h | 123 +++++++++++++++ sensors/radio/model/WiFiModelFactory.h | 32 ++++ sensors/radio/model/WiFiModelFactoryImpl.h | 42 ++++++ sensors/radio/model/WiFiModelPerBBox.h | 167 +++++++++++++++++++++ sensors/radio/model/WiFiModelPerFloor.h | 133 ++++++++++++++++ sensors/radio/model/WiFiModels.h | 8 + 17 files changed, 1072 insertions(+) create mode 100644 data/XMLload.h create mode 100644 data/XMLsave.h create mode 100644 data/XMLserialize.h create mode 100644 data/xml.h create mode 100644 debug/PlotWifiMeasurements.h create mode 100644 geo/BBoxes3.h create mode 100644 math/MovingStdDevTS.h create mode 100644 math/distribution/Rectangular.h create mode 100644 sensors/activity/Activity.h create mode 100644 sensors/activity/ActivityDetector.h create mode 100644 sensors/gps/GPSProbability.h create mode 100644 sensors/offline/FilePlayer.h create mode 100644 sensors/radio/model/WiFiModelFactory.h create mode 100644 sensors/radio/model/WiFiModelFactoryImpl.h create mode 100644 sensors/radio/model/WiFiModelPerBBox.h create mode 100644 sensors/radio/model/WiFiModelPerFloor.h create mode 100644 sensors/radio/model/WiFiModels.h diff --git a/data/XMLload.h b/data/XMLload.h new file mode 100644 index 0000000..f9dc5e4 --- /dev/null +++ b/data/XMLload.h @@ -0,0 +1,14 @@ +#ifndef XMLLOAD_H +#define XMLLOAD_H + +#include "xml.h" + +class XMLload { + +public: + + virtual void readFromXML(XMLDoc* doc, XMLElem* src) = 0; + +}; + +#endif // XMLLOAD_H diff --git a/data/XMLsave.h b/data/XMLsave.h new file mode 100644 index 0000000..f457c40 --- /dev/null +++ b/data/XMLsave.h @@ -0,0 +1,14 @@ +#ifndef XMLSAVE_H +#define XMLSAVE_H + +#include "xml.h" + +class XMLsave { + +public: + + virtual void writeToXML(XMLDoc* doc, XMLElem* dst) = 0; + +}; + +#endif // XMLSAVE_H diff --git a/data/XMLserialize.h b/data/XMLserialize.h new file mode 100644 index 0000000..7fc4817 --- /dev/null +++ b/data/XMLserialize.h @@ -0,0 +1,44 @@ +#ifndef XMLSERIALIZE_H +#define XMLSERIALIZE_H + +#include "XMLload.h" +#include "XMLsave.h" +#include "xml.h" + +class XMLserialize : public XMLload, public XMLsave { + +public: + + void loadXML(const std::string& file) { + + XMLDoc doc; + assertOK(doc.LoadFile(file.c_str()), doc, "error while loading file"); + XMLElem* root = doc.FirstChildElement("root"); + readFromXML(&doc, root); + + } + + void saveXML(const std::string& file) { + + XMLDoc doc; + XMLElem* root = doc.NewElement("root"); + doc.InsertFirstChild(root); + writeToXML(&doc, root); + assertOK(doc.SaveFile(file.c_str()), doc, "error while saving file"); + + } + +public: + + + static void assertOK(XMLErr res, XMLDoc& doc, const std::string& txt) { + if (res != tinyxml2::XMLError::XML_SUCCESS) { + const std::string err = doc.ErrorName(); + const std::string add = doc.GetErrorStr1(); + throw Exception(txt + ": " + err + " - " + add); + } + } + +}; + +#endif // XMLSERIALIZE_H diff --git a/data/xml.h b/data/xml.h new file mode 100644 index 0000000..b222148 --- /dev/null +++ b/data/xml.h @@ -0,0 +1,33 @@ +#ifndef DATA_XML_H +#define DATA_XML_H + +#include "../lib/tinyxml/tinyxml2.h" + +using XMLDoc = tinyxml2::XMLDocument; +using XMLErr = tinyxml2::XMLError; +using XMLNode = tinyxml2::XMLNode; +using XMLElem = tinyxml2::XMLElement; +using XMLAttr = tinyxml2::XMLAttribute; +using XMLText = tinyxml2::XMLText; + +#define XML_FOREACH_ATTR(attr, root) \ +for (const XMLAttr* attr = root->FirstAttribute(); attr != nullptr; attr = attr->Next()) + +#define XML_FOREACH_NODE(sub, root) \ +for (const XMLNode* sub = root->FirstChild(); sub != nullptr; sub = sub->NextSibling()) + +#define XML_FOREACH_ELEM(sub, root) \ +for (XMLElem* sub = (XMLElem*)root->FirstChild(); sub != nullptr; sub = (XMLElem*)sub->NextSibling()) + +#define XML_FOREACH_ELEM_NAMED(name, sub, root) \ +for (XMLElem* sub = root->FirstChildElement(name); sub != nullptr; sub = sub->NextSiblingElement(name)) + +#define XML_ID(node) node->Attribute("xml:id") +#define XML_WITH_ID(node) node->Attribute("with_id") + +#define XML_ASSERT_NODE_NAME(name, node) if (std::string(name) != std::string(node->Name())) {throw Exception("expected " + std::string(name) + " got " + node->Name());} + +#define XML_MANDATORY_ATTR(node, attr) (node->Attribute(attr) ? node->Attribute(attr) : throw Exception(std::string("missing XML attribute: ") + attr)); + + +#endif // DATA_XML_H diff --git a/debug/PlotWifiMeasurements.h b/debug/PlotWifiMeasurements.h new file mode 100644 index 0000000..541312f --- /dev/null +++ b/debug/PlotWifiMeasurements.h @@ -0,0 +1,76 @@ +#ifndef PLOTWIFIMEASUREMENTS_H +#define PLOTWIFIMEASUREMENTS_H + +#include +#include +#include + +#include "../sensors/radio/WiFiMeasurements.h" +#include "../sensors/radio/WiFiQualityAnalyzer.h" + +#include + +/** + * helper-class that plots incoming wifi measurements + * one line per AP + */ +class PlotWifiMeasurements { + + K::Gnuplot gp; + K::GnuplotPlot gplot; + K::GnuplotPlotElementLines lineQuality; + std::unordered_map lineForMac; + WiFiQualityAnalyzer quality; + +public: + + PlotWifiMeasurements(const std::string& labelX = "time (sec)", const std::string& labelY = "dBm") { + + gplot.getAxisX().setLabel(labelX); + gplot.getAxisY().setLabel(labelY); + + // quality indicator using the 2nd y axis + gplot.add(&lineQuality); lineQuality.getStroke().setWidth(2); lineQuality.setUseAxis(1, 2); + gplot.getAxisY2().setTicsVisible(true); + gplot.getAxisY2().setRange(K::GnuplotAxis::Range(0, 1)); + + } + + K::Gnuplot& getGP() { + return gp; + } + + K::GnuplotPlot& getPlot() { + return gplot; + } + + void add(const WiFiMeasurements& mes) { + + const Timestamp ts = mes.entries.front().getTimestamp(); + + for (const WiFiMeasurement& m : mes.entries) { + const auto& it = lineForMac.find(m.getAP().getMAC()); + if (it == lineForMac.end()) { + K::GnuplotPlotElementLines* line = new K::GnuplotPlotElementLines(); + line->setTitle(m.getAP().getMAC().asString()); + line->getStroke().getColor().setAuto(); + lineForMac[m.getAP().getMAC()] = line; + gplot.add(line); + } + const K::GnuplotPoint2 gp2(m.getTimestamp().sec(), m.getRSSI()); + lineForMac[m.getAP().getMAC()]->add(gp2); + } + + quality.add(mes); + lineQuality.add(K::GnuplotPoint2(ts.sec(), quality.getQuality())); + + } + + void plot() { + gp.draw(gplot); + gp.flush(); + } + +}; + +#endif // PLOTWIFIMEASUREMENTS_H diff --git a/geo/BBoxes3.h b/geo/BBoxes3.h new file mode 100644 index 0000000..b870861 --- /dev/null +++ b/geo/BBoxes3.h @@ -0,0 +1,45 @@ +#ifndef BBOXES3_H +#define BBOXES3_H + +#include "BBox3.h" +#include + +class BBoxes3 { + +private: + + /** all contained bboxes */ + std::vector bboxes; + +public: + + /** empty ctor */ + BBoxes3() {;} + + /** ctor with entries */ + BBoxes3(const std::vector& bboxes) : bboxes(bboxes) {;} + + + /** add the given bbox */ + void add(const BBox3& bbox) { + bboxes.push_back(bbox); + } + + /** get all contained bboxes */ + const std::vector& get() const { + return bboxes; + } + + /** does the compound contain the given point? */ + bool contains(const Point3& p) const { + for (const BBox3& bb : bboxes) { + if (bb.contains(p)) {return true;} + } + return false; + } + + + +}; + +#endif // BBOXES3_H diff --git a/math/MovingStdDevTS.h b/math/MovingStdDevTS.h new file mode 100644 index 0000000..4cfd439 --- /dev/null +++ b/math/MovingStdDevTS.h @@ -0,0 +1,49 @@ +#ifndef MOVINGSTDDEVTS_H +#define MOVINGSTDDEVTS_H + +#include "MovingAverageTS.h" + +/** + * moving stadnard-deviation using a given time-region + */ +template class MovingStdDevTS { + +private: + + MovingAverageTS avg; + MovingAverageTS avg2; + + +public: + + + /** ctor with the window-size to use */ + MovingStdDevTS(const Timestamp window, const T zeroElement) : avg(window, zeroElement), avg2(window, zeroElement) { + ; + } + + /** add a new entry */ + void add(const Timestamp ts, const T& data) { + + // E(x) + avg.add(ts, data); + + // E(x^2) + avg2.add(ts, data*data); + + } + + /** get the current std-dev */ + T get() const { + + const T e = avg.get(); + const T e2 = avg2.get(); + const T var = e2 - e*e; + return std::sqrt(var); + + } + + +}; + +#endif // MOVINGSTDDEVTS_H diff --git a/math/distribution/Rectangular.h b/math/distribution/Rectangular.h new file mode 100644 index 0000000..3f3a5fb --- /dev/null +++ b/math/distribution/Rectangular.h @@ -0,0 +1,46 @@ +#ifndef RECTANGULAR_H +#define RECTANGULAR_H + + +#include +#include +#include "../Random.h" +#include "../../Assertions.h" +#include "Normal.h" + +namespace Distribution { + + /** normal distribution */ + template class Rectangular { + + private: + + const T mu; + const T h; + const T width; + + public: + + /** ctor */ + Rectangular(const T mu, const T width) : mu(mu), h(1.0/width), width(width) { + + } + + /** get probability for the given value */ + T getProbability(const T val) const { + const T diff = std::abs(val - mu); + return (diff < width/2) ? (h) : (0.0); + } + + /** get the probability for the given value */ + static T getProbability(const T mu, const T width, const T val) { + Rectangular dist(mu, width); + return dist.getProbability(val); + } + + }; + +} + + +#endif // RECTANGULAR_H diff --git a/sensors/activity/Activity.h b/sensors/activity/Activity.h new file mode 100644 index 0000000..bcacc55 --- /dev/null +++ b/sensors/activity/Activity.h @@ -0,0 +1,20 @@ +#ifndef ACTIVITY_H +#define ACTIVITY_H + +enum class Activity { + + STANDING, + + WALKING, + + WALKING_UP, + + WALKING_DOWN, + + ELEVATOR_UP, + + ELEVATOR_DOWN, + +}; + +#endif // ACTIVITY_H diff --git a/sensors/activity/ActivityDetector.h b/sensors/activity/ActivityDetector.h new file mode 100644 index 0000000..e816064 --- /dev/null +++ b/sensors/activity/ActivityDetector.h @@ -0,0 +1,161 @@ +#ifndef ACTIVITYDETECTOR_H +#define ACTIVITYDETECTOR_H + + +#include "../imu/AccelerometerData.h" +#include "../pressure/BarometerData.h" + +#include "../../data/Timestamp.h" + +#include "../../Assertions.h" +#include "../../math/MovingAverageTS.h" +#include "../../math/MovingStdDevTS.h" +#include "../../data/HistoryTS.h" + +#include "../activity/Activity.h" + +//#define ACT_DET_DEBUG_PLOT + +#ifdef ACT_DET_DEBUG_PLOT +#include +#include +#include +#include +#include +#endif + + + +/** + * simple step detection based on accelerometer magnitude. + * magnitude > threshold? -> step! + * block for several msec until detecting the next one + */ +class ActivityDetector { + +private: + + MovingAverageTS avgLong; + MovingAverageTS avgShort; + + MovingStdDevTS stdDev; + MovingStdDevTS stdDev2; + + MovingAverageTS baroAvg; + HistoryTS baroHistory; + + Activity current; + + + +public: + +#ifdef ACT_DET_DEBUG_PLOT + K::Gnuplot gp; + K::GnuplotPlot gplot; + K::GnuplotPlotElementLines l1; + K::GnuplotPlotElementLines l2; +#endif + + /** ctor */ + ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0), + stdDev(Timestamp::fromMS(150), 0), stdDev2(Timestamp::fromMS(2000), 0), + baroAvg(Timestamp::fromMS(500), 0), baroHistory(Timestamp::fromMS(4000)) { + ; + + #ifdef ACT_DET_DEBUG_PLOT + gplot.add(&l1); + gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000"); + #endif + + } + + //int xx = 0; + + /** add barometer data */ + void add(const Timestamp ts, const BarometerData& baro) { + if (baro.isValid()) { + baroAvg.add(ts, baro.hPa); + const float avg = baroAvg.get(); + baroHistory.add(ts, avg); + //l1.add(K::GnuplotPoint2(xx, avg)); + update(); + } + } + + /** get the currently detected activity */ + Activity get() const { + return current; + } + + /** does the given data indicate a step? */ + void add(const Timestamp ts, const AccelerometerData& acc) { + + // update averages + avgLong.add(ts, acc.magnitude()); + avgShort.add(ts, acc.magnitude()); + stdDev.add(ts, acc.magnitude()); + stdDev2.add(ts, acc.magnitude()); + +// const float delta = std::abs(avgLong.get() - avgShort.get()); + +// static int x = 0; ++x; + + +// if (delta < 0.3) { +// return Activity::STANDING; +// } + +// if (avgLong.get() > 9.81+0.5) { +// return Activity::WALKING_UP; +// } else if (avgLong.get() < 9.81-0.5) { +// return Activity::WALKING_DOWN; +// } + +// return Activity::WALKING; + + } + + +private: + + /** estimate the current activity based on the sensor data */ + void update() { + + // delta in acceleration + const float delta_acc = std::abs(avgLong.get() - avgShort.get()); + + if (delta_acc < 0.015) { + current = Activity::STANDING; + return; + } + + // delta in pressure + const float delta_hPa = baroHistory.getMostRecent() - baroHistory.getOldest(); + + #ifdef ACT_DET_DEBUG_PLOT + l2.add(K::GnuplotPoint2(xx, delta_hPa)); + gp.draw(gplot); + gp.flush(); + ++xx; + #endif + + if (std::abs(delta_hPa) < 0.042) { + current = Activity::WALKING; + return; + } else if (delta_hPa > 0) { + current = Activity::WALKING_DOWN; + return; + } else { + current = Activity::WALKING_UP; + return; + } + + } + + + + +}; + +#endif // ACTIVITYDETECTOR_H diff --git a/sensors/gps/GPSProbability.h b/sensors/gps/GPSProbability.h new file mode 100644 index 0000000..194cbd7 --- /dev/null +++ b/sensors/gps/GPSProbability.h @@ -0,0 +1,65 @@ +#ifndef GPSPROBABILITY_H +#define GPSPROBABILITY_H + +#include "GPSData.h" +#include "../../geo/Point3.h" +#include "../../math/distribution/Region.h" +#include "../../geo/EarthMapping.h" + +class GPSProbability { + +private: + + /** convert between map and earth */ + const EarthMapping& em; + +public: + + /** ctor with the map<->earth translator */ + GPSProbability(const EarthMapping& em) : em(em) { + ; + } + + /** get the probability for residing at pos_m [in meter, map-coordinates] given some GPS measurement */ + double getProbability(const Point3 pos_m, const GPSData& d) const { + + // pad GPS? -> no gps eval + if (isBad(d)) {return 1.0;} + + // adjust accuracy [sometimes strange values are provided here!] + float accuracy = d.accuracy; + if (accuracy < 3.0) { + std::cout << "note: adjusting gps accuracy as '" << accuracy << "'' seems invalid" << std::endl; + accuracy = 3.0; + } + + // convert GPS to map coordinats + const Point3 gpsToMap_m = em.worldToMap(d.toEarthPos()); + + // distance between given point and GPS's estimation + const float dist = pos_m.getDistance(gpsToMap_m); + + // calculate probability + //const double prob = Distribution::Region::getProbability(0, d.accuracy, dist); + const double prob = Distribution::Normal::getProbability(0, accuracy, dist); + + // sanity checks + Assert::isNot0(prob, "gps probability is 0.0"); + Assert::isNotNaN(prob, "gps probability is NaN"); + + // done + return prob; + + } + +private: + + /** returns true if the given GPS reading is bad [inaccurate, invalid, ...] */ + static inline bool isBad(const GPSData& d) { + return (!d.isValid()) || (d.accuracy == 0) || (d.accuracy > 25); + } + + +}; + +#endif // GPSPROBABILITY_H diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h new file mode 100644 index 0000000..7d5c6e2 --- /dev/null +++ b/sensors/offline/FilePlayer.h @@ -0,0 +1,123 @@ +#ifndef FILEPLAYER_H +#define FILEPLAYER_H + +#include "FileReader.h" +#include + +namespace Offline { + + /** + * this class can be used to "play" previously recorded [so-called "offline"] files. + * one can attach itself as listener and is informed whenever new sensor data is available. + * one may choose whether to use full-speed playback [as many events as possible] or + * live-speed playback with the same timing the values were recorded with + */ + class FilePlayer { + + private: + + FileReader* reader; + + bool realtime = false; + bool enabled; + std::thread thread; + + /** the listener to inform */ + Listener* listener = nullptr; + + public: + + /** empty ctor */ + FilePlayer() : reader(nullptr), listener(nullptr) { + ; + } + + /** ctor */ + FilePlayer(FileReader* reader, Listener* l) : reader(reader), listener(l) { + ; + } + + /** whether to use realtime playback or "as fast as possible" */ + void setRealtime(const bool rt) { + this->realtime = rt; + } + + /** set the offline-file-reader to use as data source */ + void setReader(FileReader* r) { + this->reader = r; + } + + /** set the event listener to inform */ + void setListener(Listener* l) { + this->listener = l; + } + + /** start playback */ + void start() { + + // sanity check + Assert::isNotNull(reader, "call FilePlayer::setReader() first"); + Assert::isNotNull(listener, "call FilePlayer::setListener() first"); + Assert::isFalse(reader->getEntries().empty(), "FileReader has no loaded entries for playback within the FilePlayer!"); + + enabled = true; + thread = std::thread(&FilePlayer::loop, this); + + } + + /** stop playback */ + void stop() { + enabled = false; + } + + /** wait for termination */ + void join() { + thread.join(); + } + + private: + + /** background loop */ + void loop() { + + // get all sensor events from the offline file + const std::vector events = reader->getEntries(); + + // process every event + for (const Entry& e : events) { + + // aborted? + if (!enabled) {break;} + + // timestamp + const Timestamp ts = Timestamp::fromMS(e.ts); + + // event index + const size_t idx = e.idx; + +#warning "some sensors todo:" + switch(e.type) { + case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; + case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break; + case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break; + case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break; + case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break; + case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; + case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; + case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; + case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; + default: throw Exception("code error. found not-yet-implemented sensor"); + } + + } + + // done + enabled = false; + + } + + }; + +} + +#endif // FILEPLAYER_H diff --git a/sensors/radio/model/WiFiModelFactory.h b/sensors/radio/model/WiFiModelFactory.h new file mode 100644 index 0000000..6bcf94a --- /dev/null +++ b/sensors/radio/model/WiFiModelFactory.h @@ -0,0 +1,32 @@ +#ifndef WIFIMODELFACTORY_H +#define WIFIMODELFACTORY_H + + +#include "WiFiModel.h" +#include "../../../floorplan/v2/Floorplan.h" + +/** + * this class can instantiate WiFiModels based on serialized XML files + */ +class WiFiModelFactory { + +private: + + Floorplan::IndoorMap* map; + +public: + + /** ctor. needs the floorplan for some models */ + WiFiModelFactory(Floorplan::IndoorMap* map) : map(map) { + ; + } + + /** parse model from XML file */ + WiFiModel* loadXML(const std::string& file); + + /** parse model from XML node */ + WiFiModel* readFromXML(XMLDoc* doc, XMLElem* src); + +}; + +#endif // WIFIMODELFACTORY_H diff --git a/sensors/radio/model/WiFiModelFactoryImpl.h b/sensors/radio/model/WiFiModelFactoryImpl.h new file mode 100644 index 0000000..d318146 --- /dev/null +++ b/sensors/radio/model/WiFiModelFactoryImpl.h @@ -0,0 +1,42 @@ +#ifndef WIFIMODELFACTORYIMPL_H +#define WIFIMODELFACTORYIMPL_H + +#include "WiFiModelFactory.h" +#include "WiFiModelLogDist.h" +#include "WiFiModelLogDistCeiling.h" +#include "WiFiModelPerFloor.h" +#include "WiFiModelPerBBox.h" + +WiFiModel* WiFiModelFactory::loadXML(const std::string& file) { + + XMLDoc doc; + XMLserialize::assertOK(doc.LoadFile(file.c_str()), doc, "error while loading file"); + XMLElem* root = doc.FirstChildElement("root"); + + return readFromXML(&doc, root); + +} + +WiFiModel* WiFiModelFactory::readFromXML(XMLDoc* doc, XMLElem* src) { + + // each model attaches its "type" during serialization + const std::string type = src->Attribute("type"); + + WiFiModel* mdl = nullptr; + + // create an instance for the model + if (type == "WiFiModelLogDist") {mdl = new WiFiModelLogDist();} + else if (type == "WiFiModelLogDistCeiling") {mdl = new WiFiModelLogDistCeiling(map);} + else if (type == "WiFiModelPerFloor") {mdl = new WiFiModelPerFloor(map);} + else if (type == "WiFiModelPerBBox") {mdl = new WiFiModelPerBBox(map);} + else {throw Exception("invalid model type given: " + type);} + + // load the model from XML + mdl->readFromXML(doc, src); + + // done + return mdl; + +} + +#endif // WIFIMODELFACTORYIMPL_H diff --git a/sensors/radio/model/WiFiModelPerBBox.h b/sensors/radio/model/WiFiModelPerBBox.h new file mode 100644 index 0000000..1ae5ffc --- /dev/null +++ b/sensors/radio/model/WiFiModelPerBBox.h @@ -0,0 +1,167 @@ +#ifndef WIFIMODELPERBBOX_H +#define WIFIMODELPERBBOX_H + + +#include "../AccessPoint.h" +#include "../../../geo/Point3.h" +#include "../../../geo/BBoxes3.h" +#include + +#include "WiFiModelFactory.h" + +/** + * FOR TESTING + * + * this model allows using a different sub-models + * identified by a bound-box to reduce the error + */ +class WiFiModelPerBBox : public WiFiModel { + + struct ModelForBBoxes { + + WiFiModel* mdl; + BBoxes3 bboxes; + + /** ctor */ + ModelForBBoxes(WiFiModel* mdl, BBoxes3 bboxes) : mdl(mdl), bboxes(bboxes) {;} + + /** does this entry apply to the given position? */ + bool matches(const Point3 pt) const { + if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");} + return bboxes.contains(pt); + } + + }; + + Floorplan::IndoorMap* map; + + /** all contained models [one per bbox] */ + std::vector models; + +public: + + WiFiModelPerBBox(Floorplan::IndoorMap* map) : map(map) { + ; + } + + /** dtor */ + virtual ~WiFiModelPerBBox() { + + } + + + /** get a list of all APs known to the model */ + std::vector getAllAPs() const override { + + // combine all submodels + std::vector res; + for (const ModelForBBoxes& sub : models) { + for (const AccessPoint& ap : sub.mdl->getAllAPs()) { + if (std::find(res.begin(), res.end(), ap) == res.end()) { // TODO use map instead? + res.push_back(ap); + } + } + } + return res; + + } + + void add(WiFiModel* mdl, const BBoxes3 bboxes) { + if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");} + ModelForBBoxes mfb(mdl, bboxes); + models.push_back(mfb); + } + + float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override { + + for (const ModelForBBoxes& mfb : models) { + if (mfb.matches(position_m)) {return mfb.mdl->getRSSI(accessPoint, position_m);} + } + + return -120; + + } + + void readFromXML(XMLDoc* doc, XMLElem* src) override { + + // check type + if (std::string("WiFiModelPerBBox") != src->Attribute("type")) {throw Exception("invalid model type");} + + models.clear(); + + // model factory [create models based on XMl content] + WiFiModelFactory fac(map); + + // parse all contained models [one per floor] + XML_FOREACH_ELEM_NAMED("entry", xentry, src) { + + // all bboxes + BBoxes3 bboxes; + XML_FOREACH_ELEM_NAMED("bbox", xbbox, xentry) { + + const float x1 = xbbox->FloatAttribute("x1"); + const float y1 = xbbox->FloatAttribute("y1"); + const float z1 = xbbox->FloatAttribute("z1"); + + const float x2 = xbbox->FloatAttribute("x2"); + const float y2 = xbbox->FloatAttribute("y2"); + const float z2 = xbbox->FloatAttribute("z2"); + + const BBox3 bbox(Point3(x1,y1,z1), Point3(x2,y2,z2)); + bboxes.add(bbox); + + } + + // node for the model + XMLElem* xmodel = xentry->FirstChildElement("model"); + + // let the factory instantiate the model + WiFiModel* mdl = fac.readFromXML(doc, xmodel); + + // add + models.push_back(ModelForBBoxes(mdl, bboxes)); + + } + + } + + void writeToXML(XMLDoc* doc, XMLElem* dst) override { + + // set my type + dst->SetAttribute("type", "WiFiModelPerBBox"); + + for (const ModelForBBoxes& mfb : models) { + + // all models + XMLElem* xentry = doc->NewElement("entry"); { + + // each bbox + for (const BBox3& bbox : mfb.bboxes.get()) { + XMLElem* xbbox = doc->NewElement("bbox"); { + + xbbox->SetAttribute("x1", bbox.getMin().x); + xbbox->SetAttribute("y1", bbox.getMin().y); + xbbox->SetAttribute("z1", bbox.getMin().z); + + xbbox->SetAttribute("x2", bbox.getMax().x); + xbbox->SetAttribute("y2", bbox.getMax().y); + xbbox->SetAttribute("z2", bbox.getMax().z); + + }; xentry->InsertFirstChild(xbbox); + } + + // the corresponding model + XMLElem* xmodel = doc->NewElement("model"); { + mfb.mdl->writeToXML(doc, xmodel); + }; xentry->InsertEndChild(xmodel); + + }; dst->InsertEndChild(xentry); + + } + + } + +}; + + +#endif // WIFIMODELPERBBOX_H diff --git a/sensors/radio/model/WiFiModelPerFloor.h b/sensors/radio/model/WiFiModelPerFloor.h new file mode 100644 index 0000000..f14dd7e --- /dev/null +++ b/sensors/radio/model/WiFiModelPerFloor.h @@ -0,0 +1,133 @@ +#ifndef WIFIMODELPERFLOOR_H +#define WIFIMODELPERFLOOR_H + +#include "../AccessPoint.h" +#include "../../../geo/Point3.h" +#include + +#include "WiFiModelFactory.h" + +/** + * FOR TESTING + * + * this model allows using a different sub-models for each floor to reduce the error + */ +class WiFiModelPerFloor : public WiFiModel { + + struct ModelForFloor { + + float fromZ; + float toZ; + WiFiModel* mdl; + + /** ctor */ + ModelForFloor(const float fromZ, const float toZ, WiFiModel* mdl) : fromZ(fromZ), toZ(toZ), mdl(mdl) {;} + + /** does this entry apply to the given z-position? */ + bool matches(const float z) const { + return (fromZ <= z && z < toZ); + } + + }; + + Floorplan::IndoorMap* map; + + /** all contained models [one per floor] */ + std::vector models; + +public: + + WiFiModelPerFloor(Floorplan::IndoorMap* map) : map(map) { + ; + } + + /** dtor */ + virtual ~WiFiModelPerFloor() { + + } + + + /** get a list of all APs known to the model */ + std::vector getAllAPs() const override { + + // combine all submodels + std::vector res; + for (const ModelForFloor& sub : models) { + for (const AccessPoint& ap : sub.mdl->getAllAPs()) { + if (std::find(res.begin(), res.end(), ap) == res.end()) { // TODO use map instead? + res.push_back(ap); + } + } + } + return res; + + } + + void add(WiFiModel* mdl, const Floorplan::Floor* floor) { + ModelForFloor mff(floor->atHeight, floor->atHeight+floor->height, mdl); + models.push_back(mff); + } + + float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override { + + for (const ModelForFloor& mff : models) { + if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);} + } + + return -120; + + } + + void readFromXML(XMLDoc* doc, XMLElem* src) override { + + // check type + if (std::string("WiFiModelPerFloor") != src->Attribute("type")) {throw Exception("invalid model type");} + + models.clear(); + + // model factory [create models based on XMl content] + WiFiModelFactory fac(map); + + // parse all contained models [one per floor] + XML_FOREACH_ELEM_NAMED("floor", xfloor, src) { + + // floor params + const float z1 = xfloor->FloatAttribute("z1"); + const float z2 = xfloor->FloatAttribute("z2"); + + // node for the model + XMLElem* xmodel = xfloor->FirstChildElement("model"); + + // let the factory instantiate the model + WiFiModel* mdl = fac.readFromXML(doc, xmodel); + + // add + models.push_back(ModelForFloor(z1, z2, mdl)); + + } + + } + + void writeToXML(XMLDoc* doc, XMLElem* dst) override { + + // set my type + dst->SetAttribute("type", "WiFiModelPerFloor"); + + for (const ModelForFloor& mff : models) { + + XMLElem* xfloor = doc->NewElement("floor"); { + xfloor->SetAttribute("z1", mff.fromZ); + xfloor->SetAttribute("z2", mff.toZ); + XMLElem* xmodel = doc->NewElement("model"); { + mff.mdl->writeToXML(doc, xmodel); + }; xfloor->InsertEndChild(xmodel); + }; dst->InsertEndChild(xfloor); + + } + + } + +}; + + +#endif // WIFIMODELPERFLOOR_H diff --git a/sensors/radio/model/WiFiModels.h b/sensors/radio/model/WiFiModels.h new file mode 100644 index 0000000..6dd00c9 --- /dev/null +++ b/sensors/radio/model/WiFiModels.h @@ -0,0 +1,8 @@ +#ifndef WIFIMODELS_H +#define WIFIMODELS_H + +#include "WiFiModel.h" +#include "WiFiModelFactory.h" +#include "WiFiModelFactoryImpl.h" + +#endif // WIFIMODELS_H