From c083aae476f9ee1ce02488c2f0910a62cfc05aaa Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 4 Nov 2016 17:25:49 +0100 Subject: [PATCH 01/43] added beacon stuff similiar architecture then wifi \n added activity percentage stuff \n added testcases --- floorplan/v2/Floorplan.h | 1 + .../WalkModuleActivityControlPercent.h | 91 ++++++ .../v2/modules/WalkModuleButterActivity.h | 86 ------ main.cpp | 6 +- sensors/beacon/Beacon.h | 59 ++++ sensors/beacon/BeaconMeasurement.h | 44 +++ sensors/beacon/BeaconMeasurements.h | 26 ++ sensors/beacon/BeaconProbability.h | 15 + sensors/beacon/BeaconProbabilityFree.h | 93 ++++++ sensors/beacon/model/BeaconModel.h | 46 +++ sensors/beacon/model/BeaconModelLogDist.h | 92 ++++++ .../beacon/model/BeaconModelLogDistCeiling.h | 208 ++++++++++++++ sensors/pressure/ActivityButterPressure.h | 24 +- .../pressure/ActivityButterPressurePercent.h | 270 ++++++++++++++++++ sensors/radio/LocatedAccessPoint.h | 27 -- sensors/radio/WiFiMeasurement.h | 22 +- sensors/radio/WiFiProbabilityFree.h | 1 + sensors/radio/WiFiProbabilityGrid.h | 4 +- sensors/radio/model/WiFiModel.h | 3 +- sensors/radio/model/WiFiModelLogDist.h | 2 + sensors/radio/setup/WiFiFingerprint.h | 6 +- sensors/radio/setup/WiFiOptimizer.h | 2 +- tests/Tests.h | 4 +- .../beacon/TestLogDistanceCeilingModel.cpp | 121 ++++++++ tests/sensors/beacon/TestProbabilityFree.cpp | 7 + tests/sensors/pressure/TestBarometer.cpp | 91 +++++- 26 files changed, 1207 insertions(+), 144 deletions(-) create mode 100644 grid/walk/v2/modules/WalkModuleActivityControlPercent.h delete mode 100644 grid/walk/v2/modules/WalkModuleButterActivity.h create mode 100644 sensors/beacon/Beacon.h create mode 100644 sensors/beacon/BeaconMeasurement.h create mode 100644 sensors/beacon/BeaconMeasurements.h create mode 100644 sensors/beacon/BeaconProbability.h create mode 100644 sensors/beacon/BeaconProbabilityFree.h create mode 100644 sensors/beacon/model/BeaconModel.h create mode 100644 sensors/beacon/model/BeaconModelLogDist.h create mode 100644 sensors/beacon/model/BeaconModelLogDistCeiling.h create mode 100644 sensors/pressure/ActivityButterPressurePercent.h delete mode 100644 sensors/radio/LocatedAccessPoint.h create mode 100644 tests/sensors/beacon/TestLogDistanceCeilingModel.cpp create mode 100644 tests/sensors/beacon/TestProbabilityFree.cpp diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 95648d0..0668f03 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -186,6 +186,7 @@ namespace Floorplan { Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} + Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground }; diff --git a/grid/walk/v2/modules/WalkModuleActivityControlPercent.h b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h new file mode 100644 index 0000000..0af1136 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h @@ -0,0 +1,91 @@ +#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H +#define WALKMODULEACTIVITYCONTROLPERCENT_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" +#include "../../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../../grid/GridNode.h" + + +/** favor z-transitions */ +template class WalkModuleActivityControlPercent : public WalkModule { + +private: + + Control* ctrl; + +public: + + /** ctor */ + WalkModuleActivityControlPercent(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; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) state; + (void) startNode; + + + const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; + + //floor and doors + if(potentialNode.getType() == 0 || potentialNode.getType() == 3){ + return ctrl->activityPercent.stay; + } + + //stairs + if(potentialNode.getType() == 1){ + +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown + ctrl->barometer.actProbs.stairsUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;} + return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp); + } + + //elevators + if(potentialNode.getType() == 2){ + +// //we need to do this, that particles are able to walk into an elevator even if the prob for that is low, +// //that happens often since the activity has some delay. +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown + ctrl->barometer.actProbs.elevatorUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;} + + //for walking out of the elevator + return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp); + } + + std::cout << "Node has unknown Type" << std::endl; + return 1.0; + } +}; + +#endif // WALKMODULEACTIVITYCONTROLPERCENT_H diff --git a/grid/walk/v2/modules/WalkModuleButterActivity.h b/grid/walk/v2/modules/WalkModuleButterActivity.h deleted file mode 100644 index bc3f37c..0000000 --- a/grid/walk/v2/modules/WalkModuleButterActivity.h +++ /dev/null @@ -1,86 +0,0 @@ -#ifndef WALKMODULEBUTTERACTIVITY_H -#define WALKMODULEBUTTERACTIVITY_H - -#include "WalkModule.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 { - -public: - - /** ctor */ - WalkModuleButterActivity() { - - // ensure templates WalkState inherits from 'WalkStateBarometerActivity' - StaticAssert::AinheritsB(); - - } - - 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; - } - - double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { - - (void) state; - (void) startNode; - - const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; - - 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.barometer.activity == ActivityButterPressure::Activity::UP){ - if (deltaZ_cm > 0) {return 0;} - if (deltaZ_cm == 0) {return 0.1;} - return 0.9; - } else { - if (deltaZ_cm == 0) {return 0.9;} - return 0.1; - } - - } - - -}; - -#endif // WALKMODULEBUTTERACTIVITY_H diff --git a/main.cpp b/main.cpp index 255a441..03d1e84 100755 --- a/main.cpp +++ b/main.cpp @@ -25,12 +25,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*"; - ::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; + ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + ::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/beacon/Beacon.h b/sensors/beacon/Beacon.h new file mode 100644 index 0000000..1604932 --- /dev/null +++ b/sensors/beacon/Beacon.h @@ -0,0 +1,59 @@ +#ifndef BEACON_H +#define BEACON_H + +#include "../MACAddress.h" + +/** + * represents a single beacon + * a beacon is represented by its MAC-Address and + * may provide a sending power TXP + */ +class Beacon { + +private: + + /** the AP's MAC-Address */ + MACAddress mac; + + /** OPTIONAL the beacons sending power */ + float txp; + +public: + + /** empty ctor */ + Beacon() { + ; + } + + /** ctor with MAC and TXP */ + Beacon(const MACAddress& mac, const float& txp) : mac(mac), txp(txp) { + ; + } + + /** ctor with MAC and TXP */ + Beacon(const std::string& mac, const float& txp) : mac(mac), txp(txp) { + ; + } + + /** ctor with MAC and without TXP */ + Beacon(const MACAddress& mac) : mac(mac), txp() { + ; + } + + /** ctor with MAC and without TXP */ + Beacon(const std::string& mac) : mac(mac), txp() { + ; + } + +public: + + /** get the AP's MAC address */ + inline const MACAddress& getMAC() const {return mac;} + + /** OPTIONAL: get the AP's ssid (if any) */ + inline const float& getTXP() const {return txp;} + + +}; + +#endif // BEACON_H diff --git a/sensors/beacon/BeaconMeasurement.h b/sensors/beacon/BeaconMeasurement.h new file mode 100644 index 0000000..0bc3ea9 --- /dev/null +++ b/sensors/beacon/BeaconMeasurement.h @@ -0,0 +1,44 @@ +#ifndef BEACONMEASUREMENT_H +#define BEACONMEASUREMENT_H + +#include "../MACAddress.h" +#include "../../data/Timestamp.h" +#include "Beacon.h" + +#include + + +/** one observed AP and its signal strength */ +class BeaconMeasurement { + +private: + + /** the timestamp this beacon was discovered at */ + Timestamp ts; + + /** the beacon's mac address */ + Beacon beacon; + + /** signal strength */ + float rssi; + +public: + + /** ctor */ + BeaconMeasurement(const Timestamp ts, const Beacon& beacon, const float rssi) : ts(ts), beacon(beacon), rssi(rssi) {;} + + +public: + + /** get the beacon */ + const Beacon& getBeacon() const {return beacon;} + + /** get the measurements timestamp */ + const Timestamp& getTimestamp() const {return ts;} + + /** get the rssi */ + float getRSSI() const {return rssi;} +}; + + +#endif // BEACONMEASUREMENT_H diff --git a/sensors/beacon/BeaconMeasurements.h b/sensors/beacon/BeaconMeasurements.h new file mode 100644 index 0000000..dbdf489 --- /dev/null +++ b/sensors/beacon/BeaconMeasurements.h @@ -0,0 +1,26 @@ +#ifndef BEACONMEASUREMENTS_H +#define BEACONMEASUREMENTS_H + +#include + +#include "BeaconMeasurement.h" + +/** + * group of several beacon measurements + */ +struct BeaconMeasurements { + + std::vector entries; + + /** remove entries older then 3000 ms*/ + void removeOld(const Timestamp latestTS) { + auto lambda = [latestTS] (const BeaconMeasurement& e) { + Timestamp age = latestTS - e.getTimestamp(); + return age > Timestamp::fromMS(1000*3); + }; + entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end()); + } + +}; + +#endif // BEACONMEASUREMENTS_H diff --git a/sensors/beacon/BeaconProbability.h b/sensors/beacon/BeaconProbability.h new file mode 100644 index 0000000..66c0707 --- /dev/null +++ b/sensors/beacon/BeaconProbability.h @@ -0,0 +1,15 @@ +#ifndef BEACONPROBABILITY_H +#define BEACONPROBABILITY_H + +#include "BeaconMeasurements.h" + +/** + * base class for all Beacon probability calculators. + * such a calculator determines the probabilty for a location (e.g. x,y,z) + * given BeaconMeasurements + */ +class BeaconProbability { + +}; + +#endif // BEACONPROBABILITY_H diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h new file mode 100644 index 0000000..35e902e --- /dev/null +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -0,0 +1,93 @@ +#ifndef BEACONPROBABILITYFREE_H +#define BEACONPROBABILITYFREE_H + +#include "BeaconProbability.h" +#include "BeaconMeasurements.h" +#include "model/BeaconModel.h" +#include "../../math/Distributions.h" +#include "../../data/Timestamp.h" + +#include "../../floorplan/v2/Floorplan.h" + +#include + +/** + * compare BeaconMeasurements within predictions of a given model. + * predictions are just based on the distance to the observed beacon. + */ +class BeaconObserverFree : public BeaconProbability { + +private: + + const float sigma = 8.0f; + + const float sigmaPerSecond = 3.0f; + + /** the RSSI prediction model */ + BeaconModel& model; + + /** the map's floorplan */ + Floorplan::IndoorMap* map; + +public: + + /** ctor */ + BeaconObserverFree(const float sigma, BeaconModel& model) : sigma(sigma), model(model) { + + } + + /** provides the probability for a specific point in space */ + double getProbability(const Point3& pos_m, const Timestamp curTime, const BeaconMeasurements& obs) const { + + double prob = 1.0; + int numMatchingBeacons = 0; + + // process each measured AP + for (const BeaconMeasurement& entry : obs.entries) { + + // sanity check + Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?"); + + // updating the beacons sended txp if available + if(entry.getBeacon().getTXP() != 0.0f){ + //TODO: check if this works + model.updateBeacon(entry); + } + + // get the model's RSSI (if possible!) + const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m); + + // NaN? -> AP not known to the model -> skip + if (modelRSSI != modelRSSI) {continue;} + + + // the scan's RSSI + const float scanRSSI = entry.getRSSI(); + + // 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); + + ++numMatchingBeacons; + + } + + // sanity check + Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); + + return prob; + + } + +}; + +#endif // WIFIPROBABILITYFREE_H diff --git a/sensors/beacon/model/BeaconModel.h b/sensors/beacon/model/BeaconModel.h new file mode 100644 index 0000000..c9ba73f --- /dev/null +++ b/sensors/beacon/model/BeaconModel.h @@ -0,0 +1,46 @@ +#ifndef BEACONMODEL_H +#define BEACONMODEL_H + +#include "../Beacon.h" +#include "../BeaconMeasurement.h" +#include "../../../geo/Point3.h" + +#include + +/** + * interface for signal-strength prediction models. + * + * 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 BeaconModel { + +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 getAllBeacons() const = 0; + + /** + * update the beacons signal strength using the current measurement + * this could happen if the txp is not updated within the floorplan + * + * be careful and don't use fantasy values, this could ruin your localitions + * completely + */ + virtual void updateBeacon(const BeaconMeasurement beacon) = 0; + + + /** + * get the RSSI expected at the given location (in meter) + * for an beacon identified by the given MAC. + * + * if the model can not predict the RSSI for an beacon, it returns NaN! + */ + virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0; + +}; + +#endif // BEACONMODEL_H diff --git a/sensors/beacon/model/BeaconModelLogDist.h b/sensors/beacon/model/BeaconModelLogDist.h new file mode 100644 index 0000000..460cac8 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDist.h @@ -0,0 +1,92 @@ +#ifndef BEACONMODELLOGDIST_H +#define BEACONMODELLOGDIST_H + +#include "BeaconModel.h" +#include "../../radio/model/LogDistanceModel.h" + +#include + +/** + * signal-strength estimation using log-distance model + */ +class BeaconModelLogDist : public BeaconModel { + +public: + + /** parameters describing one beacons to the model */ + struct APEntry { + + Point3 position_m; // the AP's position (in meter) + float txp; // sending power (-40) + float exp; // path-loss-exponent (~2.0 - 4.0) + + /** ctor */ + APEntry(const Point3 position_m, const float txp, const float exp) : + position_m(position_m), txp(txp), exp(exp) {;} + + }; + +private: + + /** map of all beacons (and their parameters) known to the model */ + std::unordered_map beacons; + +public: + + /** ctor */ + BeaconModelLogDist() { + ; + } + + /** get a list of all beacons known to the model */ + std::vector getAllBeacons() const { + std::vector aps; + for (const auto it : beacons) {aps.push_back(Beacon(it.first));} + return aps; + } + + /** make the given beacon (and its parameters) known to the model */ + void addAP(const MACAddress& beacon, const APEntry& params) { + + // sanity check + Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-90:-30]"); + Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]"); + + // add + beacons.insert( std::pair(beacon, params) ); + + } + + void updateBeacon(const BeaconMeasurement beacon) override{ + // try to get the corresponding parameters + const auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC())); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return;} + + it->second.txp = beacon.getBeacon().getTXP(); + } + + virtual float getRSSI(const MACAddress& beacon, const Point3 position_m) const override { + + // try to get the corresponding parameters + const auto it = beacons.find(beacon); + + // AP unknown? -> NAN + if (it == beacons.end()) {return NAN;} + + // the beacons' parameters + const APEntry& params = it->second; + + // free-space (line-of-sight) RSSI + const float distance_m = position_m.getDistance(params.position_m); + const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m); + + // done + return rssiLOS; + + } + +}; + +#endif // BEACONMODELLOGDIST_H diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h new file mode 100644 index 0000000..a10a128 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -0,0 +1,208 @@ +#ifndef BEACONMODELLOGDISTCEILING_H +#define BEACONMODELLOGDISTCEILING_H + +#include "../../../floorplan/v2/Floorplan.h" + +#include "../../../Assertions.h" +#include "BeaconModel.h" +#include "../../radio/model/LogDistanceModel.h" +#include "../BeaconMeasurement.h" + +#include + +/** + * signal-strength estimation using log-distance model + * including ceilings between beacon and position + */ +class BeaconModelLogDistCeiling : public BeaconModel { + +public: + + /** parameters describing one beacon to the model */ + struct APEntry { + + Point3 position_m; // the beacon's position (in meter) + float txp; // sending power (-40) + float exp; // path-loss-exponent (~2.0 - 4.0) + float waf; // attenuation per ceiling/floor (~-8.0) + + /** ctor */ + APEntry(const Point3 position_m, const float txp, const float exp, const float waf) : + position_m(position_m), txp(txp), exp(exp), waf(waf) {;} + + }; + +private: + + /** map of all beacons (and their parameters) known to the model */ + std::unordered_map beacons; + + /** position (height) of all ceilings (in meter) */ + std::vector ceilingsAtHeight_m; + +public: + + /** ctor with floorplan (needed for ceiling position) */ + BeaconModelLogDistCeiling(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); + } + + } + + /** get a list of all beacons known to the model */ + std::vector getAllBeacons() const { + std::vector aps; + for (const auto it : beacons) {aps.push_back(Beacon(it.first));} + return aps; + } + + /** load beacon information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */ + void loadBeaconsFromMap(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::Beacon* beacon : floor->beacons) { + APEntry ape(beacon->getPos(floor), txp, exp, waf); + addBeacon(MACAddress(beacon->mac), ape); + } + } + + } + + /** load beacon information from a vector. use the given fixed TXP/EXP/WAF for all APs */ + void loadBeaconsFromVector(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::Beacon* beacon : floor->beacons) { + APEntry ape(beacon->getPos(floor), txp, exp, waf); + addBeacon(MACAddress(beacon->mac), ape); + } + } + + } + + /** make the given beacon (and its parameters) known to the model */ + void addBeacon(const MACAddress& beacon, const APEntry& params) { + + // sanity check + Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); + 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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); + + // add + beacons.insert( std::pair(beacon, params) ); + + } + + void updateBeacon(const BeaconMeasurement beacon) override { + // try to get the corresponding parameters + auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC())); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return;} + + + // TODO: Check if this works as expected + it->second.txp = beacon.getBeacon().getTXP(); + } + + /** remove all added APs */ + void clear() { + beacons.clear(); + } + + float getRSSI(const MACAddress& beacon, const Point3 position_m) const override { + + // try to get the corresponding parameters + const auto it = beacons.find(beacon); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return NAN;} + + // the access-points' parameters + const APEntry& params = it->second; + + // free-space (line-of-sight) RSSI + const float distance_m = position_m.getDistance(params.position_m); + 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 * numCeilingsBetween(position_m, params.position_m); + + // combine + return rssiLOS + wafLoss; + + } + + + +protected: + + FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilings); + FRIEND_TEST(LogDistanceCeilingModelBeacon, 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 { + + int cnt = 0; + 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;} + } + + return cnt; + + } + +}; + + +#endif // WIFIMODELLOGDISTCEILING_H diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 5940ae6..2d1617c 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -38,12 +38,20 @@ public: Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); - /** change this values for much success */ + /** change this values for much success + * + * Nexus 6: + * butter = Filter::ButterworthLP(10,0.1f,2); + * threshold = 0.025; + * diffSize = 20; + * FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + */ 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.1f,2); - Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.1f,2); + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); public: @@ -68,14 +76,14 @@ public: return STAY; } - input.push_back(History(ts, baro)); + //input.push_back(History(ts, baro)); bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - inputInterp.push_back(History(ts, BarometerData(interpValue))); + //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -113,7 +121,7 @@ public: }else{ actValue = sum; } - sumHist.push_back(actValue); + //sumHist.push_back(actValue); if(actValue > threshold){ currentActivity = DOWN; @@ -127,7 +135,7 @@ public: } } - actHist.push_back(History(ts, BarometerData(currentActivity))); + //actHist.push_back(History(ts, BarometerData(currentActivity))); return currentActivity; diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/pressure/ActivityButterPressurePercent.h new file mode 100644 index 0000000..85290e2 --- /dev/null +++ b/sensors/pressure/ActivityButterPressurePercent.h @@ -0,0 +1,270 @@ +#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H +#define ACTIVITYBUTTERPRESSUREPERCENT_H + +#include "../../data/Timestamp.h" +#include "../../math/filter/Butterworth.h" +#include "../../math/FixedFrequencyInterpolator.h" +#include "../../math/distribution/Normal.h" +#include +#include + + +#include "BarometerData.h" + +/** + * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering + * activity recognition based on a small window given by matlabs diff(window) + */ +class ActivityButterPressurePercent { + +public: + + struct ActivityProbabilities{ + float elevatorDown; + float stairsDown; + float stay; + float stairsUp; + float elevatorUp; + + ActivityProbabilities(float elevatorDown, float stairsDown, + float stay, float stairsUp, float elevatorUp) : + elevatorDown(elevatorDown), stairsDown(stairsDown), + stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;} + + ActivityProbabilities() : + elevatorDown(0.01f), stairsDown(0.01f), + stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;} + }; + + + struct History { + Timestamp ts; + BarometerData data; + History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;} + }; + +private: + //just for debugging and plotting + std::vector input; + std::vector inputInterp; + std::vector output; + std::vector sumHist; + std::vector mvAvgHist; + std::vector actHist; + + bool initialize; + + ActivityProbabilities currentActivity; + + /** change this values for much success */ + const int diffSize = 20; //the number values used for finding the activity. + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + + const float variance = 0.02f; + + const float muStairs = 0.04f; + const float muStay = 0.00f; + const float muEleveator = 0.08; + + std::vector densities = std::vector(5, 1); + std::vector densitiesOld = std::vector(5, 1);; + +public: + + + /** ctor */ + ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){ + initialize = true; + } + + + /** add new sensor readings that were received at the given timestamp */ + ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) { + + //init + if(initialize){ + butter.stepInitialization(baro.hPa); + initialize = false; + + return currentActivity; + } + + //input.push_back(History(ts, baro)); + + bool newInterpolatedValues = false; + + //interpolate & butter + auto callback = [&] (const Timestamp ts, const float val) { + float interpValue = val; + //inputInterp.push_back(History(ts, BarometerData(interpValue))); + + //butter + float butterValue = butter.process(interpValue); + output.push_back(History(ts, BarometerData(butterValue))); + + newInterpolatedValues = true; + + }; + ffi.add(ts, baro.hPa, callback); + + if(newInterpolatedValues == true){ + + //getActivity + if(output.size() > diffSize){ + //diff + std::vector diff; + for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + + float diffVal = output[i+1].data.hPa - output[i].data.hPa; + + diff.push_back(diffVal); + } + + float sum = 0; + for(float val : diff){ + sum += val; + } + + float actValue = sum; + //sumHist.push_back(actValue); + + //calculate the probabilites of walking down/up etc... + densitiesOld = densities; + + //in one building there is an ultra fast elevator, therefore we need to clip the activity value... + if(actValue > muEleveator){ + actValue = muEleveator; + } + if(actValue < -muEleveator){ + actValue = -muEleveator; + } + + float densityElevatorDown = Distribution::Normal::getProbability(muEleveator, variance, actValue); + float densityStairsDown = Distribution::Normal::getProbability(muStairs, variance, actValue); + float densityStay = Distribution::Normal::getProbability(muStay, variance, actValue); + float densityStairsUp = Distribution::Normal::getProbability(-muStairs, variance, actValue); + float densityElevatorUp = Distribution::Normal::getProbability(-muEleveator, variance, actValue); + + _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!"); + _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!"); + _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!"); + _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); + _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); + + //_assertTrue( (densityElevatorDown != 0), "the probability of densityElevatorDown is null!"); + //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!"); + //_assertTrue( (densityStay != 0), "the probability of densityStay is null!"); + //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!"); + //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!"); + + + //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt, + //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV? + + //const Timestamp age = ts - ap.getTimestamp(); + + //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert. + densities[0] = densityElevatorDown; + densities[1] = densityStairsDown; + densities[2] = densityStay; + densities[3] = densityStairsUp; + densities[4] = densityElevatorUp; + + //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end()))); + // if an activity other then staying is detected with a high probability, we are using the previous probability + // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation + // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange. + // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the + // given activity! Since i had no time, this was the fastest solution for now. +// if(highestValueIdx != 2){ +// for(int i = 0; i < densities.size(); ++i){ +// densities[i] *= densitiesOld[i]; +// } +// } + + + //normalize + float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4]; + + for(int i = 0; i < densities.size(); ++i){ + densities[i] /= densitySum; + + //values cant be zero! + densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f); + } + +// densityElevatorDown /= densitySum; +// densityStairsDown /= densitySum; +// densityStay /= densitySum; +// densityStairsUp /= densitySum; +// densityElevatorUp /= densitySum; + + // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility +// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f); +// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f); +// densityStay = (densityStay > 0.0f ? densityStay : 0.01f); +// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f); +// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f); + + currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]); + + } + + //actHist.push_back(currentActivity); + } + + //retruns for every call, indepedent of callback. + return currentActivity; + } + + /** get the current Activity */ + ActivityProbabilities getCurrentActivity() { + return currentActivity; + } + + std::vector getSensorHistory(){ + std::vector tmp; + + for(History val : input){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getInterpolatedHistory(){ + std::vector tmp; + + for(History val : inputInterp){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getOutputHistory(){ + + std::vector tmp; + + for(History val : output){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getSumHistory(){ + return sumHist; + } + + + std::vector getActHistory(){ + + return actHist; + } + + +}; + +#endif // ACTIVITYBUTTERPRESSUREPERCENT_H diff --git a/sensors/radio/LocatedAccessPoint.h b/sensors/radio/LocatedAccessPoint.h deleted file mode 100644 index ddd4693..0000000 --- a/sensors/radio/LocatedAccessPoint.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef LOCATEDACCESSPOINT_H -#define LOCATEDACCESSPOINT_H - -#include "AccessPoint.h" -#include "../../geo/Point3.h" -#include "../../floorplan/v2/Floorplan.h" - -/** - * describes an access-point including its position (in meter) - */ -class LocatedAccessPoint : public AccessPoint, public Point3 { - -public: - - /** ctor */ - LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) { - ; - } - - /** ctor */ - LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) { - ; - } - -}; - -#endif // LOCATEDACCESSPOINT_H diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 3a07386..0b63d75 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -9,7 +9,7 @@ */ class WiFiMeasurement { -public: +private: friend class VAPGrouper; @@ -19,6 +19,9 @@ public: /** the measured signal strength */ float rssi; + /** OPTIONAL. frequence the signal was received */ + float freq; + /** OPTIONAL. timestamp the measurement was recorded at */ Timestamp ts; @@ -29,11 +32,21 @@ public: ; } + /** ctor with freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) { + ; + } + /** ctor with timestamp */ WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { ; } + /** ctor with timestamp and freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { + ; + } + public: /** get the AP we got the measurement for */ @@ -45,6 +58,13 @@ public: /** OPTIONAL: get the measurement's timestamp (if known!) */ const Timestamp& getTimestamp() const {return ts;} + /** OPTIONAL: get the measurement's frequence (if known!) */ + float getFrequency() const {return freq;} + + /** set another signal strength */ + void setRssi(float value){rssi = value;} }; + + #endif // WIFIMEASUREMENT_H diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index a0783f5..0ffc045 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -5,6 +5,7 @@ #include "model/WiFiModel.h" #include "../../math/Distributions.h" #include "VAPGrouper.h" +#include "../../floorplan/v2/Floorplan.h" #include diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index e2cfce2..1ac2189 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -59,7 +59,7 @@ public: // after some runtime, check whether the wifi timestamps make sense // those must not be zero, otherwise something is wrong! if (!obs.entries.empty()) { - Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense..."); + Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().getTimestamp().isZero(), "WiFiMeasurement timestamp is 0. this does not make sense..."); } // process each observed measurement @@ -73,7 +73,7 @@ public: const Timestamp age = curTime - measurement.getTimestamp(); // sigma grows with measurement age - 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(); diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index 05f336c..bebfce4 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -1,7 +1,8 @@ #ifndef WIFIMODEL_H #define WIFIMODEL_H -#include "../LocatedAccessPoint.h" +#include "../AccessPoint.h" +#include "../../../geo/Point3.h" /** * interface for signal-strength prediction models. diff --git a/sensors/radio/model/WiFiModelLogDist.h b/sensors/radio/model/WiFiModelLogDist.h index 417f19c..e30158b 100644 --- a/sensors/radio/model/WiFiModelLogDist.h +++ b/sensors/radio/model/WiFiModelLogDist.h @@ -4,6 +4,8 @@ #include "WiFiModel.h" #include "LogDistanceModel.h" +#include + /** * signal-strength estimation using log-distance model */ diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index d0a2d6b..a92652b 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -46,9 +46,9 @@ struct WiFiFingerprint { 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.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI()); } - avg.rssi /= apMeasurements.entries.size(); + avg.setRssi(avg.getRSSI() / apMeasurements.entries.size()); res.entries.push_back(avg); // add to output } @@ -62,7 +62,7 @@ struct WiFiFingerprint { 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"; + out << wm.getTimestamp().ms() << " " << wm.getAP().getMAC().asString() << " " << wm.getRSSI() << "\n"; } } diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index bcd84d6..5ac22b6 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -89,7 +89,7 @@ public: // add each available AP to its slot (lookup map) for (const WiFiMeasurement& m : measurements.entries) { - const RSSIatPosition rap(fp.pos_m, m.rssi); + const RSSIatPosition rap(fp.pos_m, m.getRSSI()); apMap[m.getAP().getMAC()].push_back(rap); } diff --git a/tests/Tests.h b/tests/Tests.h index fbe9932..3639618 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -6,8 +6,8 @@ #include static inline std::string getDataFile(const std::string& name) { - return "/mnt/data/workspaces/Indoor/tests/data/" + name; - //return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; + //return "/mnt/data/workspaces/Indoor/tests/data/" + name; + return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; } diff --git a/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp new file mode 100644 index 0000000..856394c --- /dev/null +++ b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp @@ -0,0 +1,121 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/beacon/model/BeaconModelLogDistCeiling.h" + +TEST(LogDistanceCeilingModelBeacon, calc) { + + // 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); + + //LocatedAccessPoint ap0("00:00:00:00:00:00", Point3(0,0,0)); + //LocatedAccessPoint ap25("00:00:00:00:00:00", Point3(0,0,2.5)); + + BeaconModelLogDistCeiling model(&map); + + const MACAddress ap0 = MACAddress("00:00:00:00:00:00"); + const MACAddress ap25 = MACAddress("00:00:00:00:00:01"); + + model.addBeacon(ap0, BeaconModelLogDistCeiling::APEntry( Point3(0,0,0), -40, 1.0, -8.0 )); + model.addBeacon(ap25, BeaconModelLogDistCeiling::APEntry( Point3(0,0,2.5), -40, 1.0, -8.0 )); + + + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(1,0,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,1,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,0,1))); + + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(1,0,2.5))); + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(0,1,2.5))); + ASSERT_EQ(-40-8, model.getRSSI(ap25, Point3(0,0,3.5))); // one floor within + + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,8,0))); + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,0,8))+8+8); // two ceilings within + +} + +TEST(LogDistanceCeilingModelBeacon, numCeilings) { + + // 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); + + BeaconModelLogDistCeiling model(&map); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) ); + ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) ); + +} + +TEST(LogDistanceCeilingModelBeacon, 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); + + BeaconModelLogDistCeiling 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/beacon/TestProbabilityFree.cpp b/tests/sensors/beacon/TestProbabilityFree.cpp new file mode 100644 index 0000000..8466c2b --- /dev/null +++ b/tests/sensors/beacon/TestProbabilityFree.cpp @@ -0,0 +1,7 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +//todo + +#endif diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 5dcdfe1..1fef297 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -5,6 +5,7 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" #include "../../../sensors/pressure/ActivityButterPressure.h" +#include "../../../sensors/pressure/ActivityButterPressurePercent.h" #include @@ -78,7 +79,7 @@ TEST(Barometer, LIVE_tendence) { } - sleep(1000); + sleep(1); } @@ -114,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) { } - sleep(1000); + sleep(1); // tendence must be clear and smaller than the sigma @@ -130,6 +131,9 @@ TEST(Barometer, Activity) { std::string filename = getDataFile("barometer/baro1.dat"); std::ifstream infile(filename); + std::vector actHist; + std::vector rawHist; + while (std::getline(infile, line)) { std::istringstream iss(line); @@ -138,35 +142,102 @@ TEST(Barometer, Activity) { while (iss >> ts >> value) { ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); } } - std::vector sum = act.getSumHistory(); - std::vector interpolated = act.getInterpolatedHistory(); - std::vector raw = act.getSensorHistory(); - std::vector butter = act.getOutputHistory(); - std::vector actHist = act.getActHistory(); - K::Gnuplot gp; + K::Gnuplot gpRaw; K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; for(int i=0; i < actHist.size()-1; ++i){ + //raw + K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa); + K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa); + + rawLines.addSegment(raw_p1, raw_p2); + + //results K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); - rawLines.addSegment(input_p1, input_p2); + resultLines.addSegment(input_p1, input_p2); } - plot.add(&rawLines); + plotRaw.add(&rawLines); + plot.add(&resultLines); gp.draw(plot); gp.flush(); + gpRaw.draw(plotRaw); + gpRaw.flush(); + sleep(1); } +TEST(Barometer, ActivityPercent) { + + ActivityButterPressurePercent act; + + //read file + std::string line; + std::string filename = getDataFile("barometer/baro1.dat"); + std::ifstream infile(filename); + + std::vector actHist; + std::vector rawHist; + + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(value); + actHist.push_back(activity); + } + } + + K::Gnuplot gp; + K::Gnuplot gpRaw; + K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; + K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; + + for(int i=0; i < actHist.size()-1; ++i){ + + K::GnuplotPoint2 raw_p1(i, rawHist[i]); + K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]); + + rawLines.addSegment(raw_p1, raw_p2); + + K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown); + K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown); + + resultLines.addSegment(input_p1, input_p2); + } + + plotRaw.add(&rawLines); + plot.add(&resultLines); + + gp.draw(plot); + gp.flush(); + + gpRaw.draw(plotRaw); + gpRaw.flush(); + + sleep(1000); +} + #endif From 41c1d90c54fd2e288d0c4c1b77c5bc67710ff621 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 16 Nov 2016 12:40:16 +0100 Subject: [PATCH 02/43] small fix. added getter and setter! --- .../beacon/model/BeaconModelLogDistCeiling.h | 2 +- sensors/pressure/ActivityButterPressure.h | 67 +---------- .../pressure/ActivityButterPressurePercent.h | 109 +++--------------- sensors/radio/WiFiMeasurement.h | 23 ++-- tests/sensors/pressure/TestBarometer.cpp | 4 +- 5 files changed, 34 insertions(+), 171 deletions(-) diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h index a10a128..8e794a6 100644 --- a/sensors/beacon/model/BeaconModelLogDistCeiling.h +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -91,7 +91,7 @@ public: // sanity check Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); - Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]"); + Assert::isBetween(params.txp, -90.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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 2d1617c..be53031 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -18,8 +18,6 @@ public: enum Activity {DOWN, STAY, UP}; - - struct History { Timestamp ts; BarometerData data; @@ -27,14 +25,8 @@ public: }; private: - //just for debugging and plotting - std::vector input; - std::vector inputInterp; - std::vector output; - std::vector sumHist; - std::vector mvAvgHist; - std::vector actHist; + std::vector output; Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); @@ -47,8 +39,8 @@ public: * FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); */ const bool additionalLowpassFilter = false; - 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! + const unsigned long diffSize = 20; //the number values used for finding the activity. + const float threshold = 0.025f; // 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 butter2 = Filter::ButterworthLP(10,0.05f,2); @@ -76,14 +68,11 @@ public: return STAY; } - //input.push_back(History(ts, baro)); - bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -97,10 +86,10 @@ public: if(newInterpolatedValues == true){ //getActivity - if((int)output.size() > diffSize){ + if(output.size() > diffSize){ //diff std::vector diff; - for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){ float diffVal = output[i+1].data.hPa - output[i].data.hPa; @@ -121,7 +110,6 @@ public: }else{ actValue = sum; } - //sumHist.push_back(actValue); if(actValue > threshold){ currentActivity = DOWN; @@ -135,8 +123,6 @@ public: } } - //actHist.push_back(History(ts, BarometerData(currentActivity))); - return currentActivity; } @@ -145,49 +131,6 @@ public: Activity getCurrentActivity() { return currentActivity; } - - std::vector getSensorHistory(){ - std::vector tmp; - - for(History val : input){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getInterpolatedHistory(){ - std::vector tmp; - - for(History val : inputInterp){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getOutputHistory(){ - - std::vector tmp; - - for(History val : output){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getSumHistory(){ - return sumHist; - } - - - std::vector getActHistory(){ - - return actHist; - } - - }; #endif // ACTIVITYBUTTERPRESSURE_H diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/pressure/ActivityButterPressurePercent.h index 85290e2..c20583e 100644 --- a/sensors/pressure/ActivityButterPressurePercent.h +++ b/sensors/pressure/ActivityButterPressurePercent.h @@ -14,6 +14,8 @@ /** * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering * activity recognition based on a small window given by matlabs diff(window) + * + * todo: if an elevator is detected, first we have a short time the stairs are more prober. */ class ActivityButterPressurePercent { @@ -44,20 +46,14 @@ public: }; private: - //just for debugging and plotting - std::vector input; - std::vector inputInterp; - std::vector output; - std::vector sumHist; - std::vector mvAvgHist; - std::vector actHist; + std::vector output; bool initialize; ActivityProbabilities currentActivity; /** change this values for much success */ - const int diffSize = 20; //the number values used for finding the activity. + const unsigned long diffSize = 20; //the number values used for finding the activity. Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); @@ -65,10 +61,10 @@ private: const float muStairs = 0.04f; const float muStay = 0.00f; - const float muEleveator = 0.08; + const float muEleveator = 0.08f; std::vector densities = std::vector(5, 1); - std::vector densitiesOld = std::vector(5, 1);; + std::vector densitiesOld = std::vector(5, 1); public: @@ -90,14 +86,11 @@ public: return currentActivity; } - //input.push_back(History(ts, baro)); - bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -114,7 +107,7 @@ public: if(output.size() > diffSize){ //diff std::vector diff; - for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){ float diffVal = output[i+1].data.hPa - output[i].data.hPa; @@ -127,7 +120,6 @@ public: } float actValue = sum; - //sumHist.push_back(actValue); //calculate the probabilites of walking down/up etc... densitiesOld = densities; @@ -152,17 +144,11 @@ public: _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); - //_assertTrue( (densityElevatorDown != 0), "the probability of densityElevatorDown is null!"); - //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!"); - //_assertTrue( (densityStay != 0), "the probability of densityStay is null!"); - //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!"); - //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!"); - - - //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt, - //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV? - - //const Timestamp age = ts - ap.getTimestamp(); + _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!"); + _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!"); + _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!"); + _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!"); + _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!"); //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert. densities[0] = densityElevatorDown; @@ -171,47 +157,19 @@ public: densities[3] = densityStairsUp; densities[4] = densityElevatorUp; - //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end()))); - // if an activity other then staying is detected with a high probability, we are using the previous probability - // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation - // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange. - // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the - // given activity! Since i had no time, this was the fastest solution for now. -// if(highestValueIdx != 2){ -// for(int i = 0; i < densities.size(); ++i){ -// densities[i] *= densitiesOld[i]; -// } -// } - - //normalize float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4]; - for(int i = 0; i < densities.size(); ++i){ + for(unsigned long i = 0; i < densities.size(); ++i){ densities[i] /= densitySum; //values cant be zero! densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f); } -// densityElevatorDown /= densitySum; -// densityStairsDown /= densitySum; -// densityStay /= densitySum; -// densityStairsUp /= densitySum; -// densityElevatorUp /= densitySum; - - // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility -// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f); -// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f); -// densityStay = (densityStay > 0.0f ? densityStay : 0.01f); -// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f); -// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f); - currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]); } - - //actHist.push_back(currentActivity); } //retruns for every call, indepedent of callback. @@ -223,47 +181,6 @@ public: return currentActivity; } - std::vector getSensorHistory(){ - std::vector tmp; - - for(History val : input){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getInterpolatedHistory(){ - std::vector tmp; - - for(History val : inputInterp){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getOutputHistory(){ - - std::vector tmp; - - for(History val : output){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getSumHistory(){ - return sumHist; - } - - - std::vector getActHistory(){ - - return actHist; - } - }; diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 0b63d75..d84327d 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -37,10 +37,10 @@ public: ; } - /** ctor with timestamp */ - WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { - ; - } + /** ctor with timestamp */ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { + ; + } /** ctor with timestamp and freq*/ WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { @@ -49,20 +49,23 @@ public: public: - /** get the AP we got the measurement for */ - const AccessPoint& getAP() const {return ap;} + /** get the AP we got the measurement for */ + const AccessPoint& getAP() const {return ap;} - /** get the measurement's signal strength */ - float getRSSI() const {return rssi;} + /** get the measurement's signal strength */ + float getRSSI() const {return rssi;} - /** OPTIONAL: get the measurement's timestamp (if known!) */ - const Timestamp& getTimestamp() const {return ts;} + /** OPTIONAL: get the measurement's timestamp (if known!) */ + const Timestamp& getTimestamp() const {return ts;} /** OPTIONAL: get the measurement's frequence (if known!) */ float getFrequency() const {return freq;} /** set another signal strength */ void setRssi(float value){rssi = value;} + + /** set the timestamp */ + void setTimestamp(const Timestamp& val){ts = val;} }; diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 1fef297..7c3451f 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -178,7 +178,7 @@ TEST(Barometer, Activity) { gpRaw.draw(plotRaw); gpRaw.flush(); - sleep(1); + sleep(5); } @@ -236,7 +236,7 @@ TEST(Barometer, ActivityPercent) { gpRaw.draw(plotRaw); gpRaw.flush(); - sleep(1000); + sleep(5); } From e10ba2cfd9405da2c2452f66174f36bc4f7bb54b Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 1 Dec 2016 19:48:27 +0100 Subject: [PATCH 03/43] added support for ground truth points \n small fixed in beaconprob --- floorplan/v2/Floorplan.h | 12 +++++++++++ floorplan/v2/FloorplanReader.h | 19 ++++++++++++++++ floorplan/v2/FloorplanWriter.h | 10 +++++++++ math/distribution/VonMises.h | 8 +++++-- sensors/MACAddress.h | 30 ++++++++++++++++++-------- sensors/beacon/BeaconProbabilityFree.h | 3 +-- 6 files changed, 69 insertions(+), 13 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 0668f03..d9682cc 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -117,6 +117,7 @@ namespace Floorplan { struct POI; struct Stair; struct Elevator; + struct GroundTruthPoint; using FloorOutline = std::vector; using FloorObstacles = std::vector; @@ -127,6 +128,7 @@ namespace Floorplan { using FloorPOIs = std::vector; using FloorStairs = std::vector; using FloorElevators = std::vector; + using FloorGroundTruthPoints = std::vector; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -143,6 +145,7 @@ namespace Floorplan { FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor FloorElevators elevators; // all elevators within one floor + FloorGroundTruthPoints gtpoints; // all ground truth points within one floor //FloorKeyValue other; // other, free elements Floor() {;} @@ -167,6 +170,15 @@ namespace Floorplan { bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);} }; + /** a GroundTruthPoint located somewhere on a floor */ + struct GroundTruthPoint { + int id; //TODO: this value can be changed and isn't set incremental within the indoormap + Point2 pos; + GroundTruthPoint() : id(), pos() {;} + GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;} + bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} + }; + /** an AccessPoint located somewhere on a floor */ struct AccessPoint { std::string name; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 396f338..47af25c 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -97,6 +97,7 @@ namespace Floorplan { if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} + if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} } return floor; } @@ -185,6 +186,24 @@ namespace Floorplan { } + /** parse the tag */ + static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } + } + return vec; + } + + /** parse a tag */ + static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { + GroundTruthPoint* gtp = new GroundTruthPoint(); + gtp->id = el->IntAttribute("id"); + gtp->pos = parsePoint2(el); + return gtp; + } + + /** parse the tag */ static std::vector parseFloorAccessPoints(const XMLElem* el) { std::vector vec; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index bd0bc4e..878b26d 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -145,6 +145,16 @@ namespace Floorplan { } floor->InsertEndChild(pois); + XMLElem* gtpoints = doc.NewElement("gtpoints"); + for (const GroundTruthPoint* gtp : mf->gtpoints) { + XMLElem* elem = doc.NewElement("gtpoint"); + elem->SetAttribute("id", gtp->id); + elem->SetAttribute("x", gtp->pos.x); + elem->SetAttribute("y", gtp->pos.y); + gtpoints->InsertEndChild(elem); + } + floor->InsertEndChild(gtpoints); + XMLElem* accesspoints = doc.NewElement("accesspoints"); for (const AccessPoint* ap : mf->accesspoints) { XMLElem* accesspoint = doc.NewElement("accesspoint"); diff --git a/math/distribution/VonMises.h b/math/distribution/VonMises.h index 789e0c8..40b4189 100644 --- a/math/distribution/VonMises.h +++ b/math/distribution/VonMises.h @@ -19,7 +19,7 @@ namespace Distribution { const T mu; /** like 1.0/variance of the distribution */ - const T kappa; + T kappa; /** pre-calcuated look-up-table */ std::vector lut; @@ -30,7 +30,7 @@ namespace Distribution { public: /** ctor */ - VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) { + VonMises(const T mu, T kappa) : mu(mu), kappa(kappa) { } @@ -49,6 +49,10 @@ namespace Distribution { } + void setKappa(T _kappa){ + kappa = _kappa; + } + }; } diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h index 19db998..02aee2e 100644 --- a/sensors/MACAddress.h +++ b/sensors/MACAddress.h @@ -47,15 +47,27 @@ public: MACAddress(const std::string& str) { // sanity check - if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");} - - mac = 0; // all zeros - fields.h5 = hexWordToInt(str[ 0], str[ 1]); - fields.h4 = hexWordToInt(str[ 3], str[ 4]); - fields.h3 = hexWordToInt(str[ 6], str[ 7]); - fields.h2 = hexWordToInt(str[ 9], str[10]); - fields.h1 = hexWordToInt(str[12], str[13]); - fields.h0 = hexWordToInt(str[15], str[16]); + if (str.size() == 17 ){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 3], str[ 4]); + fields.h3 = hexWordToInt(str[ 6], str[ 7]); + fields.h2 = hexWordToInt(str[ 9], str[10]); + fields.h1 = hexWordToInt(str[12], str[13]); + fields.h0 = hexWordToInt(str[15], str[16]); + } + else if (str.size() == 12){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 2], str[ 3]); + fields.h3 = hexWordToInt(str[ 4], str[ 5]); + fields.h2 = hexWordToInt(str[ 6], str[7]); + fields.h1 = hexWordToInt(str[8], str[9]); + fields.h0 = hexWordToInt(str[10], str[11]); + } + else{ + throw Exception("invalid hex string length. must be 17 or 12 (without :)"); + } } diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h index 35e902e..46f6475 100644 --- a/sensors/beacon/BeaconProbabilityFree.h +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -20,7 +20,6 @@ class BeaconObserverFree : public BeaconProbability { private: const float sigma = 8.0f; - const float sigmaPerSecond = 3.0f; /** the RSSI prediction model */ @@ -82,7 +81,7 @@ public: } // sanity check - Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); + //Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); return prob; From b6be58eebcac46fc800c37b98e91151a1cb251da Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 2 Mar 2017 18:08:02 +0100 Subject: [PATCH 04/43] many small changes, added filereader with beacons, added motion detection stuff, testcases --- CMakeLists.txt | 2 +- floorplan/v2/FloorplanReader.h | 12 +- main.cpp | 6 +- sensors/imu/GravityData.h | 53 ++++ sensors/imu/LinearAccelerationData.h | 53 ++++ sensors/imu/MotionDetection.h | 163 ++++++++++++ sensors/offline/FileReader.h | 299 ++++++++++++++++++++++ tests/sensors/imu/TestMotionDetection.cpp | 200 +++++++++++++++ 8 files changed, 778 insertions(+), 10 deletions(-) create mode 100644 sensors/imu/GravityData.h create mode 100644 sensors/imu/LinearAccelerationData.h create mode 100644 sensors/imu/MotionDetection.h create mode 100644 sensors/offline/FileReader.h create mode 100644 tests/sensors/imu/TestMotionDetection.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d8d105a..d7b87de 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O0 + #-O0 -march=native -DWITH_TESTS diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 7f00748..66ffe3d 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -281,12 +281,12 @@ namespace Floorplan { Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); b->name = n->Attribute("name"); - b->major = n->Attribute("major"); - b->minor = n->Attribute("minor"); - b->uuid = n->Attribute("uuid"); - b->model.txp = n->FloatAttribute("mdl_txp"); - b->model.exp = n->FloatAttribute("mdl_exp"); - b->model.waf = n->FloatAttribute("mdl_waf"); +// b->major = n->Attribute("major"); +// b->minor = n->Attribute("minor"); +// b->uuid = n->Attribute("uuid"); +// b->model.txp = n->FloatAttribute("mdl_txp"); +// b->model.exp = n->FloatAttribute("mdl_exp"); +// b->model.waf = n->FloatAttribute("mdl_waf"); b->pos = parsePoint3(n); return b; } diff --git a/main.cpp b/main.cpp index 03d1e84..fbb4e08 100755 --- a/main.cpp +++ b/main.cpp @@ -25,12 +25,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - - ::testing::GTEST_FLAG(filter) = "*Barometer*"; + ::testing::GTEST_FLAG(filter) = "*MotionDetection*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/imu/GravityData.h b/sensors/imu/GravityData.h new file mode 100644 index 0000000..d522f20 --- /dev/null +++ b/sensors/imu/GravityData.h @@ -0,0 +1,53 @@ +#ifndef GRAVITYDATA_H +#define GRAVITYDATA_H + +#include +#include + + +/** data received from an accelerometer sensor */ +struct GravityData { + + float x; + float y; + float z; + + GravityData() : x(0), y(0), z(0) {;} + + GravityData(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 ); + } + + GravityData& operator += (const GravityData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + GravityData& operator -= (const GravityData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + GravityData operator - (const GravityData& o) const { + return GravityData(x-o.x, y-o.y, z-o.z); + } + + GravityData operator / (const float val) const { + return GravityData(x/val, y/val, z/val); + } + + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + +}; + +#endif // GRAVITYDATA_H diff --git a/sensors/imu/LinearAccelerationData.h b/sensors/imu/LinearAccelerationData.h new file mode 100644 index 0000000..9ae116c --- /dev/null +++ b/sensors/imu/LinearAccelerationData.h @@ -0,0 +1,53 @@ +#ifndef LINEARACCELERATIONDATA_H +#define LINEARACCELERATIONDATA_H + +#include +#include + + +/** data received from an accelerometer sensor */ +struct LinearAccelerationData { + + float x; + float y; + float z; + + LinearAccelerationData() : x(0), y(0), z(0) {;} + + LinearAccelerationData(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 ); + } + + LinearAccelerationData& operator += (const LinearAccelerationData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + LinearAccelerationData& operator -= (const LinearAccelerationData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + LinearAccelerationData operator - (const LinearAccelerationData& o) const { + return LinearAccelerationData(x-o.x, y-o.y, z-o.z); + } + + LinearAccelerationData operator / (const float val) const { + return LinearAccelerationData(x/val, y/val, z/val); + } + + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + +}; + +#endif // LINEARACCELERATIONDATA_H diff --git a/sensors/imu/MotionDetection.h b/sensors/imu/MotionDetection.h new file mode 100644 index 0000000..76284fe --- /dev/null +++ b/sensors/imu/MotionDetection.h @@ -0,0 +1,163 @@ +#ifndef MOTIONDETECTION_H +#define MOTIONDETECTION_H + +#include "GravityData.h" +#include "LinearAccelerationData.h" +#include "../../data/Timestamp.h" +#include "../../math/MovingAverageTS.h" +#include "../../misc/Debug.h" + +#include + +#include +#include + +#include +#include +#include +#include +#include + + +#include "../../Assertions.h" + +class MotionDetection { + +private: + + bool newAccelerationMeasurementArrived = false; + bool newGravityMeasurementArrived = false; + + Eigen::Vector3f curGravity; + Eigen::Vector3f curLinearAcceleration; + + //fast algo + Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix + + int numMeasurementsPerInterval, updateCnt; + int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms + + struct Motion{ + Eigen::Vector2f vec = Eigen::Vector2f::Identity(); + Timestamp lastEstimation; + }; + + Motion curMotion; + Motion prevMotion; + + const char* name = "MotionDetection"; + +public: + + /** ctor */ + MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) { + ; + } + + void addGravity(const Timestamp& ts, const GravityData& grav){ + + curGravity << grav.x, grav.y, grav.z; + newGravityMeasurementArrived = true; + + updateProjectionVectorFast(ts); + } + + void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) { + + curLinearAcceleration << acc.x, acc.y, acc.z; + newAccelerationMeasurementArrived = true; + + updateProjectionVectorFast(ts); + } + + /** return the current motion axis + * NOTE: if no data is available, this vector is the Identity + */ + Eigen::Vector2f getCurrentMotionAxis(){ + return curMotion.vec; + } + + /** returns the radians between [-pi, pi] between successive motion axis estimations */ + float getMotionChangeInRad(){ + //TODO: put this in an EigenHelper Class within geo + const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x(); + const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f)); + + //nan? + if(std::isnan(ang)){ + Log::add(name, "The motion change angle is nAn, this is not correct!"); + } + + if(updateCnt < 2){ + return 0; + } + + return ang; + } + +private: + + FRIEND_TEST(MotionDetection, motionAngle); + FRIEND_TEST(MotionDetection, motionAxis); + + Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){ + + Eigen::SelfAdjointEigenSolver solver(covarianceMatrix); + return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue + } + + void updateProjectionVectorFast(const Timestamp& ts){ + + //make sure we have both measurements for calculation + if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){ + + numMeasurementsPerInterval++; + + //project acc into perpendicular plane of grav (using standard vector projection) + Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity; + + //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this + if(proj.isZero()){ + proj = curLinearAcceleration; + Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?"); + } + + //we are only interested in x,y + Eigen::Vector2f vec; + vec << proj.x(), proj.y(); + + // sum this up for later averaging. + sumProjectedCov += vec*vec.transpose(); + + // start with the first available timestamp + if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;} + + //update the motion axis depending on the update interval + if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){ + + prevMotion = curMotion; + + //calculate the average of the coveriance matrix + Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval; + + curMotion.vec = updateMotionAxis(Q); + curMotion.lastEstimation = ts; + + reset(); + } + + newGravityMeasurementArrived = false; + newAccelerationMeasurementArrived = false; + } + //do nothing + } + + void reset(){ + numMeasurementsPerInterval = 0; + sumProjectedCov = Eigen::Matrix2f::Zero(); + ++updateCnt; + } + +}; + +#endif // MOTIONDETECTION_H diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h new file mode 100644 index 0000000..bc30410 --- /dev/null +++ b/sensors/offline/FileReader.h @@ -0,0 +1,299 @@ +#ifndef FILEREADER_H +#define FILEREADER_H + +#include +#include +#include +#include + +#include "../../math/Interpolator.h" +#include "../../sensors/radio/WiFiMeasurements.h" +#include "../../sensors/pressure/BarometerData.h" +#include "../../sensors/imu/AccelerometerData.h" +#include "../../sensors/imu/GyroscopeData.h" +#include "../../sensors/imu/GravityData.h" +#include "../../sensors/imu/LinearAccelerationData.h" +#include "../../sensors/beacon/BeaconMeasurements.h" + + +#include "../../geo/Point2.h" +#include "../../grid/factory/v2/GridFactory.h" +#include "../../grid/factory/v2/Importance.h" +#include "../../floorplan/v2/Floorplan.h" + +class FileReader { + +public: + + template struct TS { + const uint64_t ts; + T data; + TS(const uint64_t ts) : ts(ts) {;} + TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;} + }; + + enum class Sensor { + ACC, + GYRO, + WIFI, + POS, + BARO, + BEACON, + LIN_ACC, + GRAVITY, + }; + + /** entry for one sensor */ + struct Entry { + Sensor type; + uint64_t ts; + int idx; + Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;} + }; + + std::vector> groundTruth; + std::vector> wifi; + std::vector> beacon; + std::vector> acc; + std::vector> gyro; + std::vector> barometer; + std::vector> lin_acc; + std::vector> gravity; + + /** ALL entries */ + std::vector entries; + +public: + + FileReader(const std::string& file) { + parse(file); + } + + const std::vector& getEntries() const {return entries;} + + + const std::vector>& getGroundTruth() const {return groundTruth;} + + const std::vector>& getWiFiGroupedByTime() const {return wifi;} + + const std::vector>& getBeacons() const {return beacon;} + + const std::vector>& getAccelerometer() const {return acc;} + + const std::vector>& getGyroscope() const {return gyro;} + + const std::vector>& getBarometer() const {return barometer;} + + const std::vector>& getLinearAcceleration() const {return lin_acc;} + + const std::vector>& getGravity() const {return gravity;} + +private: + + void parse(const std::string& file) { + + std::ifstream inp(file); + if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} + + while(!inp.eof() && !inp.bad()) { + + uint64_t ts; + char delim; + int idx = -1; + std::string data; + + inp >> ts; + inp >> delim; + inp >> idx; + inp >> delim; + inp >> data; + + if (idx == 8) {parseWiFi(ts, data);} + else if (idx == 9) {parseBeacons(ts, data);} + else if (idx == 99) {parseGroundTruth(ts, data);} + else if (idx == 0) {parseAccelerometer(ts, data);} + else if (idx == 3) {parseGyroscope(ts, data);} + else if (idx == 5) {parseBarometer(ts, data);} + else if (idx == 2) {parseLinearAcceleration(ts,data);} + else if (idx == 1) {parseGravity(ts,data);} + + // TODO: this is a hack... + // the loop is called one additional time after the last entry + // and keeps the entries of entry + + } + + inp.close(); + + } + + void parseLinearAcceleration(const uint64_t ts, const std::string& data){ + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z))); + lin_acc.push_back(elem); + entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1)); + } + + void parseGravity(const uint64_t ts, const std::string& data){ + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); + gravity.push_back(elem); + entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); + } + + void parseAccelerometer(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); + acc.push_back(elem); + entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); + + } + + void parseGyroscope(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); + gyro.push_back(elem); + entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); + + } + + void parseWiFi(const uint64_t ts, const std::string& data) { + + std::string tmp = data; + + // add new wifi reading + wifi.push_back(TS(ts, WiFiMeasurements())); + entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1)); + + // process all APs + while(!tmp.empty()) { + + auto pos1 = tmp.find(';'); + auto pos2 = tmp.find(';', pos1+1); + auto pos3 = tmp.find(';', pos2+1); + + std::string mac = tmp.substr(0, pos1); + std::string freq = tmp.substr(pos1+1, pos2); + std::string rssi = tmp.substr(pos2+1, pos3); + + tmp = tmp.substr(pos3); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + // append AP to current scan-entry + WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts)); + wifi.back().data.entries.push_back(e); + } + + } + + void parseBeacons(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + const auto pos3 = data.find(';', pos2+1); + + const std::string mac = data.substr(0, pos1); + const std::string rssi = data.substr(pos1+1, pos2); + const std::string txp = data.substr(pos2+1, pos3); + + //yes the timestamp is redundant here, but in case of multiusage... + TS e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi))); + beacon.push_back(e); + entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1)); + + } + + void parseGroundTruth(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + std::string gtIndex = data.substr(0, pos1); + + TS elem(ts, std::stoi(gtIndex)); + groundTruth.push_back(elem); + + } + + void parseBarometer(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + + const std::string hPa = data.substr(0, pos1); + + TS elem(ts, BarometerData(std::stof(hPa))); + barometer.push_back(elem); + entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + + } + +public: + const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { + + // finde alle positionen der waypoints im gtPath aus map + std::unordered_map waypointsMap; + for(Floorplan::Floor* f : map->floors){ + float h = f->atHeight; + for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){ + + //wenn die gleiche id 2x vergeben wurde, knallt es + if(waypointsMap.find(gtp->id) == waypointsMap.end()){ + waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)}); + } + else{ + throw std::string("the floorplan's ground truth contains two points with identical id's!"); + } + + } + } + + // bringe diese in richtige reihenfolge und füge timestamp hinzu + Interpolator interpol; + + int it = 0; + for(int id : gtPath){ + auto itMap = waypointsMap.find(id); + if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");} + + //the time, when the gt button was clicked on the app + uint64_t tsGT = groundTruth[it++].ts; + interpol.add(tsGT, itMap->second); + + } + + if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){ + throw std::string("No Ground Truth points found within the map.xml file"); + } + + return interpol; + } + +}; + +#endif // FILEREADER_H diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp new file mode 100644 index 0000000..47bc411 --- /dev/null +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -0,0 +1,200 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/imu/MotionDetection.h" +#include "../../../sensors/imu/TurnDetection.h" +#include "../../../sensors/offline/FileReader.h" + +/** visualize the motionAxis */ +TEST(MotionDetection, motionAxis) { + + MotionDetection md; + + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + + //Walking with smartphone straight and always parallel to motion axis + //std::string filename = getDataFile("motion/straight_potrait.csv"); + + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); + + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); + + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); + + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); + + FileReader fr(filename); + + K::Gnuplot gp; + K::GnuplotPlot plot; + + gp << "set xrange[-1:1]\n set yrange[-1:1]\n"; + + + Eigen::Vector2f curVec; + float motionAxisAngleRad; + Timestamp ts; + Timestamp lastTs; + + //calc motion axis + for (const FileReader::Entry& e : fr.getEntries()) { + + ts = Timestamp::fromMS(e.ts); + + if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + curVec = md.getCurrentMotionAxis(); + motionAxisAngleRad = md.getMotionChangeInRad(); + } + + // start with the first available timestamp + if (lastTs.isZero()) {lastTs = ts;} + + if(ts - lastTs > Timestamp::fromMS(500)) { + + lastTs = ts; + + K::GnuplotPoint2 raw_p1(0, 0); + K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0)); + K::GnuplotPlotElementLines motionLines; + motionLines.addSegment(raw_p1, raw_p2); + plot.add(&motionLines); + + gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n"; + + gp.draw(plot); + gp.flush(); + //usleep(5000*33); + } + } + + //was passiert bei grenzwerten. 90° oder sowas. + //wie stabil ist die motion axis eigentlich? + //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen? + //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading? + +} + +/** comparing motionAngle and turnAngle */ +TEST(MotionDetection, motionAngle) { + + MotionDetection md; + TurnDetection td; + + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + + //Walking with smartphone straight and always parallel to motion axis + std::string filename = getDataFile("motion/straight_potrait.csv"); + + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + //std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); + + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); + + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); + + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); + + FileReader fr(filename); + Timestamp ts; + + //save for later plotting + std::vector delta_motionAngles; + std::vector delta_turnAngles; + + //calc motion axis + for (const FileReader::Entry& e : fr.getEntries()) { + + ts = Timestamp::fromMS(e.ts); + + if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + delta_motionAngles.push_back(md.getMotionChangeInRad()); + + } else if (e.type == FileReader::Sensor::ACC) { + const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(ts, _acc.data); + + } else if (e.type == FileReader::Sensor::GYRO) { + const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; + delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); + } + + } + + //draw motion + static K::Gnuplot gpMotion; + K::GnuplotPlot plotMotion; + K::GnuplotPlotElementLines motionLines; + + for(int i = 0; i < delta_motionAngles.size() - 1; ++i){ + + K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]); + motionLines.addSegment(raw_p1, raw_p2); + + } + + gpMotion << "set title 'Motion Detection'\n"; + plotMotion.add(&motionLines); + gpMotion.draw(plotMotion); + gpMotion.flush(); + + + //draw rotation + static K::Gnuplot gpTurn; + K::GnuplotPlot plotTurn; + K::GnuplotPlotElementLines turnLines; + + for(int i = 0; i < delta_turnAngles.size() - 1; ++i){ + + K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]); + turnLines.addSegment(raw_p1, raw_p2); + } + + gpTurn << "set title 'Turn Detection'\n"; + plotTurn.add(&turnLines); + gpTurn.draw(plotTurn); + gpTurn.flush(); + + sleep(1); + +} + + +#endif From f7d0d88448140056bdbe9f6f3cd03bd63a8bfd41 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 3 Mar 2017 12:09:19 +0100 Subject: [PATCH 05/43] added WalkModule for von Mises Heading --- .../v2/modules/WalkModuleHeadingVonMises.h | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 grid/walk/v2/modules/WalkModuleHeadingVonMises.h diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h new file mode 100644 index 0000000..9c01a79 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -0,0 +1,123 @@ +#ifndef WALKMODULEHEADINGVONMISES_H +#define WALKMODULEHEADINGVONMISES_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" + + +/** keep the state's heading */ +template class WalkModuleHeadingVonMises : public WalkModule { + +private: + + /** van-Mises distribution */ + Distribution::VonMises dist; + + /** random noise */ + Distribution::Normal distNoise; + + const Control* ctrl; + +public: + + /** ctor 3.0 should be OK! */ + WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) : + dist(Distribution::VonMises(0.0f, 2.0f)), + distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)), + ctrl(ctrl) { + + // ensure the template WalkState inherits from 'WalkStateHeading'! + StaticAssert::AinheritsB(); + + } + + + 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->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 + 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->turnSinceLastTransition_rad + var; + + //set kappa of mises + float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad)); + dist.setKappa(kappa); + + } + + 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; + + // ignore for stairs? + //if (nextNode.getType() == GridNode::TYPE_STAIR) {return;} + + // for elevator edges [same (x,y) but different z] do not adjust anything + 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); + + // get the heading requested by the state + const Heading stateHead = state.heading.direction; + + // get the error (signed difference) between both + const float angularDiff = stateHead.getSignedDiff(head); + + // adjust the error. + // note: the error may get > +/- 2PI but this is not an issue! + // when the error is added to the current heading within getProbability(), + // it is ensured their sum is within [0:2pi] + state.heading.error += angularDiff; + + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (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 (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); + + // compare the heading against the state's heading - the last error + const Heading stateHead = state.heading.direction + state.heading.error; + + // get the difference + const float angularDiff = head.getDiffHalfRAD(stateHead); + + // determine probability + return dist.getProbability(angularDiff); + + } + + +}; + +#endif // WALKMODULEHEADING_H From d5bc1111f5cb87f52e287d2de7b8437cbc6dc68c Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 9 Mar 2017 18:57:47 +0100 Subject: [PATCH 06/43] added kullback leibler for gaussian cases --- main.cpp | 2 +- math/Distributions.h | 1 + math/distribution/Normal.h | 9 + math/distribution/NormalN.h | 50 ++++ math/divergence/KullbackLeibler.h | 90 ++++++++ sensors/beacon/BeaconProbabilityFree.h | 4 +- sensors/radio/WiFiProbabilityFree.h | 7 +- sensors/radio/WiFiProbabilityGrid.h | 5 +- tests/math/divergence/TestKullbackLeibler.cpp | 214 ++++++++++++++++++ 9 files changed, 374 insertions(+), 8 deletions(-) create mode 100644 math/distribution/NormalN.h create mode 100644 math/divergence/KullbackLeibler.h create mode 100644 tests/math/divergence/TestKullbackLeibler.cpp diff --git a/main.cpp b/main.cpp index fbb4e08..b103b91 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*MotionDetection*"; + ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/Distributions.h b/math/Distributions.h index 1879357..93d35c8 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -8,5 +8,6 @@ #include "distribution/VonMises.h" #include "distribution/Region.h" #include "distribution/Triangle.h" +#include "distribution/NormalN.h" #endif // DISTRIBUTIONS_H diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index fc073f8..b40ce67 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -44,6 +44,15 @@ namespace Distribution { gen.seed(seed); } + /** get the mean value */ + const T getMu() { + return this->mu; + } + + /** get the standard deviation */ + const T getSigma() { + return this->sigma; + } /** get the probability for the given value */ static T getProbability(const T mu, const T sigma, const T val) { diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h new file mode 100644 index 0000000..c628828 --- /dev/null +++ b/math/distribution/NormalN.h @@ -0,0 +1,50 @@ +#ifndef NORMALN_H +#define NORMALN_H + +#include + +namespace Distribution { + + class NormalDistributionN { + + private: + + const Eigen::VectorXd mu; + const Eigen::MatrixXd sigma; + + const double _a; + const Eigen::MatrixXd _sigmaInv; + + public: + + /** ctor */ + NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + + } + + + /** get probability for the given value */ + double getProbability(const Eigen::VectorXd val) const { + const double b = ((val-mu).transpose() * _sigmaInv * (val-mu)); + return _a * std::exp(-b/2.0); + } + + /** get the mean vector */ + const Eigen::VectorXd getMu(){ + return this->mu; + } + + /** get covariance matrix */ + const Eigen::MatrixXd getSigma(){ + return this->sigma; + } + + const Eigen::MatrixXd getSigmaInv(){ + return this->_sigmaInv; + } + + }; + +} +#endif // NORMALN_H diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h new file mode 100644 index 0000000..357b1c3 --- /dev/null +++ b/math/divergence/KullbackLeibler.h @@ -0,0 +1,90 @@ +#ifndef KULLBACKLEIBLER_H +#define KULLBACKLEIBLER_H + +#include "../distribution/Normal.h" +#include "../distribution/NormalN.h" + +#include "../../Assertions.h" +#include + +namespace Divergence { + + template class KullbackLeibler { + + public: + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution + * Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/ + */ + static inline Scalar getUnivariateGauss(Distribution::Normal norm1, Distribution::Normal norm2){ + + auto sigma1Quad = norm1.getSigma() * norm1.getSigma(); + auto sigma2Quad = norm2.getSigma() * norm2.getSigma(); + auto mu12Quad = (norm1.getMu() - norm2.getMu()) * (norm1.getMu() - norm2.getMu()); + auto log1 = std::log(norm1.getSigma()); + auto log2 = std::log(norm2.getSigma()); + + // kl = log(sigma_2 / sigma_1) + ((sigma_1^2 + (mu_1 - mu_2)^2) / 2 * sigma_2^2) - 0.5 + double klb = (log2 - log1) + ((sigma1Quad + mu12Quad)/(2.0 * sigma2Quad)) - 0.5; + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution symmetric*/ + static inline Scalar getUnivariateGaussSymmetric(Distribution::Normal norm1, Distribution::Normal norm2){ + return getUnivariateGauss(norm1, norm2) + getUnivariateGauss(norm2, norm1); + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution */ + static inline Scalar getMultivariateGauss(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + + //both gaussian have the same dimension. + Assert::equal(norm1.getMu().rows(), norm2.getMu().rows(), "mean vectors do not have the same dimension"); + Assert::equal(norm1.getSigma().rows(), norm2.getSigma().rows(), "cov matrices do not have the same dimension"); + Assert::equal(norm1.getSigma().cols(), norm2.getSigma().cols(), "cov matrices do not have the same dimension"); + + //log + auto det1 = norm1.getSigma().determinant(); + auto det2 = norm2.getSigma().determinant(); + auto log1 = std::log(det1); + auto log2 = std::log(det2); + + //trace + Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols()); + toTrace = norm2.getSigmaInv() * norm1.getSigma(); + auto trace = toTrace.trace(); + + //transpose + Eigen::VectorXd toTranspose(norm1.getMu().rows()); + toTranspose = norm2.getMu() - norm1.getMu(); + auto transpose = toTranspose.transpose(); + + //rawdensity + auto rawDensity = transpose * norm2.getSigmaInv() * toTranspose; + auto dimension = norm1.getMu().rows(); + + //0.5 * ((log(det(cov_2)/det(cov_1)) + tr(cov_2^-1 cov_1) + (mu_2 - mu_1)^T * cov_2^-1 * (mu_2 - mu_1) - dimension) + double klb = 0.5 * ((log2 - log1) + trace + rawDensity - dimension); + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution symmetric*/ + static inline Scalar getMultivariateGaussSymmetric(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + return getMultivariateGauss(norm1, norm2) + getMultivariateGauss(norm2, norm1); + } + + + }; + +} + +#endif // KULLBACKLEIBLER_H diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h index 46f6475..a82de3e 100644 --- a/sensors/beacon/BeaconProbabilityFree.h +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -57,7 +57,9 @@ public: const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m); // NaN? -> AP not known to the model -> skip - if (modelRSSI != modelRSSI) {continue;} + if (modelRSSI != modelRSSI) { + continue; + } // the scan's RSSI diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 0ffc045..e850101 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -58,7 +58,7 @@ public: 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?"); + //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(); @@ -72,14 +72,15 @@ public: } // 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; } template double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const { - throw Exception("todo??"); + + return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs); } }; diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index 1ac2189..840d1dd 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -73,7 +73,7 @@ public: const Timestamp age = curTime - measurement.getTimestamp(); // sigma grows with measurement age - 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(); @@ -102,8 +102,7 @@ public: } // 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;} + //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); // as not every node has the same number of visible/matching APs // we MUST return something like the average probability diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp new file mode 100644 index 0000000..0a14f92 --- /dev/null +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -0,0 +1,214 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/KullbackLeibler.h" +#include "../../../math/Distributions.h" + +#include + + +TEST(KullbackLeibler, univariateGaussEQ) { + //if the distributions are equal, kld is 0 + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEmu) { + //bigger mu means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm3(0,1); + Distribution::Normal norm4(1,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEsigma) { + //bigger sigma means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm5(0,1); + Distribution::Normal norm6(0,3); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussRAND) { + + for(int i = 0; i < 20; i++){ + auto randMu1 = rand() % 100; + auto randMu2 = rand() % 100 + 100; + + auto randMu3 = rand() % 100; + auto randMu4 = rand() % 100 + 200; + + Distribution::Normal norm7(randMu1,1); + Distribution::Normal norm8(randMu2,1); + + Distribution::Normal norm9(randMu3,1); + Distribution::Normal norm10(randMu4,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGauss(norm8, norm7)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm8, norm7)); + } + +} + +TEST(KullbackLeibler, multivariateGaussEQ) { + + //eq + 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::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; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2)); + + +} + +TEST(KullbackLeibler, multivariateGaussGeMu) { + + //ge mu + 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] = 1.0; + mu4[1] = 3.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) = 1.0; + cov4(0,1) = 0.0; + cov4(1,0) = 0.0; + cov4(1,1) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " > " << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +TEST(KullbackLeibler, multivariateGaussGeCov) { + + //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] = 1.0; + mu4[1] = 1.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) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " >" << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +#endif From cad299cd7cf2e467e6f964d14f4cb7d3fd2b62ec Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 10 Mar 2017 13:47:12 +0100 Subject: [PATCH 07/43] started adding earth-mapping some new helper methods added support for floorplan metadata load/save --- floorplan/v2/Floorplan.h | 120 +++++++++++++++++++++++++++++---- floorplan/v2/FloorplanReader.h | 43 ++++++++---- floorplan/v2/FloorplanWriter.h | 14 +++- geo/EarthMapping.h | 75 +++++++++++++++++++++ geo/EarthPos.h | 21 ++++++ geo/Point2.h | 7 ++ 6 files changed, 252 insertions(+), 28 deletions(-) create mode 100644 geo/EarthMapping.h create mode 100644 geo/EarthPos.h diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 1d624e3..ea88b50 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -9,6 +9,7 @@ #include "../../geo/Point3.h" #include "../../geo/Point2.h" +#include "../../geo/EarthPos.h" namespace Floorplan { @@ -21,20 +22,86 @@ namespace Floorplan { /** a free key-value meta element */ struct Meta { + struct KeyVal { + std::string key; + std::string val; + KeyVal() {;} + KeyVal(const std::string& key, const std::string& val) : key(key), val(val) {;} + }; + const std::string EMPTY = ""; - std::unordered_map params; + +// std::unordered_map params; +// const std::string& getVal(const std::string& key) const { +// const auto it = params.find(key); +// return (it == params.end()) ? (EMPTY) : (it->second); +// } + + std::vector params; + + KeyVal* getKV(const std::string& key) { + for (KeyVal& kv : params) { + if (kv.key == key) {return &kv;} + } + return nullptr; + } + + const KeyVal* getKV(const std::string& key) const { + for (const KeyVal& kv : params) { + if (kv.key == key) {return &kv;} + } + return nullptr; + } const std::string& getVal(const std::string& key) const { - const auto it = params.find(key); - return (it == params.end()) ? (EMPTY) : (it->second); + static const std::string EMPTY = ""; + const KeyVal* kv = getKV(key); + return (kv) ? (kv->val) : (EMPTY); + } + void setVal(const std::string& key, const std::string& val) {(*this)[key] = val;} + + void add(const std::string& key, const std::string& val) {params.push_back(KeyVal(key,val));} + + std::string& operator [] (const std::string& key) { + KeyVal* kv = getKV(key); + if (!kv) {params.push_back(KeyVal(key, ""));} + return getKV(key)->val; } - void setVal(const std::string& key, const std::string& val) {params[key] = val;} float getFloat(const std::string& key) const { return std::stof(getVal(key)); } - void setFloat(const std::string& key, const float val) { params[key] = std::to_string(val); } + void setFloat(const std::string& key, const float val) { (*this)[key] = std::to_string(val); } int getInt(const std::string& key) const { return std::stoi(getVal(key)); } - void setInt(const std::string& key, const int val) { params[key] = std::to_string(val); } + void setInt(const std::string& key, const int val) { (*this)[key] = std::to_string(val); } + + + + size_t size() const {return params.size();} + + const std::string& getKey(const int idx) const {return params[idx].key;} + const std::string& getVal(const int idx) const {return params[idx].val;} + + void set(const int idx, const KeyVal& kv) {params[idx] = kv;} + void setKey(const int idx, const std::string& key) {params[idx].key = key;} + void setVal(const int idx, const std::string& val) {params[idx].val = val;} + + /** delete the idx-th entry */ + void deleteEntry(const int idx) {params.erase(params.begin()+idx);} + + }; + + class HasMeta { + + Meta* meta = nullptr; + + public: + + Meta* getMeta() const {return meta;} + + void setMeta(Meta* meta) { + if (this->meta) {delete this->meta;} + this->meta = meta; + } }; @@ -168,14 +235,14 @@ namespace Floorplan { }; /** an AccessPoint located somewhere on a floor */ - struct AccessPoint { + struct AccessPoint : public HasMeta { std::string name; std::string mac; Point3 pos; // z is relative to the floor's height struct Model { - float txp; - float exp; - float waf; + float txp = 0; + float exp = 0; + float waf = 0; } model; AccessPoint() : name(), mac(), pos() {;} AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;} @@ -388,8 +455,7 @@ namespace Floorplan { /** base-class for stairs */ - struct Stair { - Meta* meta = nullptr; + struct Stair : public HasMeta { virtual std::vector getParts() const = 0; }; @@ -440,7 +506,7 @@ namespace Floorplan { /** an image file that can be added to the map */ - struct UnderlayImage { + struct UnderlayImage { std::string name; std::string filename; Point2 anchor; @@ -450,16 +516,44 @@ namespace Floorplan { + /** one correspondence point: earth <-> map */ + struct EarthPosMapPos { + + EarthPos posOnEarth; + + Point3 posOnMap_m; + + /** ctor */ + EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;} + + }; + + + /** describe the floorplan's location on earth */ + struct EarthRegistration { + + /** all available correspondences: earth <-> map */ + std::vector correspondences; + + }; + /** describes the whole indoor map */ struct IndoorMap { float width; float depth; + std::vector floors; + /** mapping: floorplan <-> earth */ + EarthRegistration earthReg; + IndoorMap() {;} + /** no copy */ IndoorMap(const IndoorMap& o) = delete; + + /** no copy assign */ void operator = (const IndoorMap& o) = delete; }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 2af6df0..8522091 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -13,6 +13,7 @@ namespace Floorplan { using XMLAttr = tinyxml2::XMLAttribute; using XMLElem = tinyxml2::XMLElement; + using XMLNode = tinyxml2::XMLNode; /** * read an IndoorMaps from XML-data @@ -31,7 +32,13 @@ namespace Floorplan { setlocale(LC_NUMERIC, "C"); tinyxml2::XMLDocument doc; const tinyxml2::XMLError res = doc.LoadFile(file.c_str()); - if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);} + if (res != tinyxml2::XMLError::XML_SUCCESS) { + throw Exception( + std::string() + "error while loading XML " + file + "\n" + + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ); + } return parse(doc); } @@ -41,7 +48,13 @@ namespace Floorplan { setlocale(LC_NUMERIC, "C"); tinyxml2::XMLDocument doc; const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length()); - if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");} + if (res != tinyxml2::XMLError::XML_SUCCESS) { + throw Exception( + std::string() + "error while parsing XML\n" + + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ); + } return parse(doc); } @@ -157,9 +170,7 @@ namespace Floorplan { // stair meta information? const XMLElem* meta = el->FirstChildElement("meta"); - if (meta) { - stair->meta = parseMetaElement(meta); - } + if (meta) {stair->setMeta(parseMetaElement(meta));} return stair; @@ -227,10 +238,16 @@ namespace Floorplan { /** parse one element */ static Meta* parseMetaElement(const XMLElem* n) { Meta* elem = new Meta(); - const XMLAttr* attr = n->FirstAttribute(); - while (attr) { - elem->params[attr->Name()] = attr->Value(); - attr = attr->Next(); +// const XMLAttr* attr = n->FirstAttribute(); +// while (attr) { +// elem->setVal(attr->Name(), attr->Value()); +// attr = attr->Next(); +// } + const XMLElem* sub = n->FirstChildElement(); + while(sub) { + // abc + elem->add(sub->Attribute("key"), sub->FirstChild()->Value()); + sub = sub->NextSiblingElement(); } return elem; } @@ -244,6 +261,8 @@ namespace Floorplan { ap->model.txp = n->FloatAttribute("mdl_txp"); ap->model.exp = n->FloatAttribute("mdl_exp"); ap->model.waf = n->FloatAttribute("mdl_waf"); + const XMLElem* meta = n->FirstChildElement("meta"); + if (meta) {ap->setMeta(parseMetaElement(meta));} return ap; } @@ -262,9 +281,9 @@ namespace Floorplan { Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); b->name = n->Attribute("name"); - b->major = n->Attribute("major"); - b->minor = n->Attribute("minor"); - b->uuid = n->Attribute("uuid"); + b->major = n->Attribute("major") ? n->Attribute("major") : ""; + b->minor = n->Attribute("minor") ? n->Attribute("minor") : ""; + b->uuid = n->Attribute("uuid") ? n->Attribute("uuid") : ""; b->model.txp = n->FloatAttribute("mdl_txp"); b->model.exp = n->FloatAttribute("mdl_exp"); b->model.waf = n->FloatAttribute("mdl_waf"); diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 3a927a0..00c672c 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -105,7 +105,7 @@ namespace Floorplan { XMLElem* stairs = doc.NewElement("stairs"); for (const Stair* _stair : mf->stairs) { XMLElem* elem = doc.NewElement("stair"); - addMetaElement(doc, elem, _stair->meta); + addMetaElement(doc, elem, _stair->getMeta()); if (dynamic_cast(_stair)) { const StairFreeform* stair = (StairFreeform*) _stair; elem->SetAttribute("type", 0); // TODO: other types? @@ -156,6 +156,7 @@ namespace Floorplan { accesspoint->SetAttribute("mdl_txp", ap->model.txp); accesspoint->SetAttribute("mdl_exp", ap->model.exp); accesspoint->SetAttribute("mdl_waf", ap->model.waf); + addMetaElement(doc, accesspoint, ap->getMeta()); accesspoints->InsertEndChild(accesspoint); } floor->InsertEndChild(accesspoints); @@ -219,8 +220,15 @@ namespace Floorplan { static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) { if (meta == nullptr) {return;} // nothing to add XMLElem* elem = doc.NewElement("meta"); - for (auto it : meta->params) { - elem->Attribute(it.first.c_str(), it.second.c_str()); +// for (auto it : meta->params) { +// elem->Attribute(it.first.c_str(), it.second.c_str()); +// } + for (const Meta::KeyVal& kv : meta->params) { + // abc + XMLElem* subElem = doc.NewElement("entry"); + subElem->SetAttribute("key", kv.key.c_str()); + subElem->SetText(kv.val.c_str()); + elem->InsertEndChild(subElem); } other->InsertEndChild(elem); } diff --git a/geo/EarthMapping.h b/geo/EarthMapping.h new file mode 100644 index 0000000..8ff10da --- /dev/null +++ b/geo/EarthMapping.h @@ -0,0 +1,75 @@ +#ifndef EARTHMAPPING_H +#define EARTHMAPPING_H + +#include "Point3.h" +#include "EarthPos.h" + +/** + * mapping between positions on earth and positions within the floorplan + */ +class EarthMapping { + +private: + + /** one 3D position within the floorplan */ + Point3 center_map_m; + + /** corresponding 3D position on earth */ + EarthPos center_earth; + + /** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */ + float rotation_deg; + + double m_per_deg_lat; + double m_per_deg_lon; + + +public: + + void build() { + + // TODO + + } + + /** convert from map-coordinates to earth-coordinates */ + EarthPos mapToWorld(const Point3 mapPos_m) const { + + Point3 pos = mapPos_m; + + // move to (0,0,0) + pos -= center_map_m; + + // undo the rotation + const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI); + + // convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center + const double lat = cenLat + (xy.y / m_per_deg_lat); + const double lon = cenLon + (xy.x / m_per_deg_lon); + const float height = pos.z; + + // done + return EarthPos(lat, lon, height); + + } + + /** convert from earth-coordinates to map-coordinates */ + Point3 worldToMap(const EarthPos earthPos) const { + + const double y_m = +(lat-cenLat) * m_per_deg_lat; + const double x_m = +(lon-cenLon) * m_per_deg_lon; + + // rotate (our map is axis aligned) + Point2 pos(x_m, y_m); + pos = pos.rotated(rotDeg / 180 * M_PI); + + // apply movement + pos += mapCenter_m; + + return pos; + + } + +}; + +#endif // EARTHMAPPING_H diff --git a/geo/EarthPos.h b/geo/EarthPos.h new file mode 100644 index 0000000..bb6ccc0 --- /dev/null +++ b/geo/EarthPos.h @@ -0,0 +1,21 @@ +#ifndef EARTHPOS_H +#define EARTHPOS_H + +/** describes the location on the earth's surface */ +struct EarthPos { + + double lat; + + double lon; + + /** height above sea level */ + float height; + + /** ctor with values */ + EarthPos(const double lat, const double lon, const float height) : lon(lon), lat(lat), height(height) { + ; + } + +}; + +#endif // EARTHPOS_H diff --git a/geo/Point2.h b/geo/Point2.h index d226d5c..2d58178 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -17,6 +17,8 @@ struct Point2 { /** ctor */ Point2(const float x, const float y) : x(x), y(y) {;} + Point2 operator - () const {return Point2(-x, -y);} + Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);} @@ -60,4 +62,9 @@ struct Point2 { }; +inline void swap(Point2& p1, Point2& p2) { + std::swap(p1.x, p2.x); + std::swap(p1.y, p2.y); +} + #endif // POINT2_H From a35a22e676857271d6cfc2447c9c3e3b7f0e1490 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 4 Nov 2016 17:25:49 +0100 Subject: [PATCH 08/43] added beacon stuff similiar architecture then wifi \n added activity percentage stuff \n added testcases --- floorplan/v2/Floorplan.h | 1 + .../WalkModuleActivityControlPercent.h | 91 ++++++ .../v2/modules/WalkModuleButterActivity.h | 86 ------ main.cpp | 6 +- sensors/beacon/Beacon.h | 59 ++++ sensors/beacon/BeaconMeasurement.h | 44 +++ sensors/beacon/BeaconMeasurements.h | 26 ++ sensors/beacon/BeaconProbability.h | 15 + sensors/beacon/BeaconProbabilityFree.h | 93 ++++++ sensors/beacon/model/BeaconModel.h | 46 +++ sensors/beacon/model/BeaconModelLogDist.h | 92 ++++++ .../beacon/model/BeaconModelLogDistCeiling.h | 208 ++++++++++++++ sensors/pressure/ActivityButterPressure.h | 24 +- .../pressure/ActivityButterPressurePercent.h | 270 ++++++++++++++++++ sensors/radio/LocatedAccessPoint.h | 27 -- sensors/radio/WiFiMeasurement.h | 22 +- sensors/radio/WiFiProbabilityFree.h | 1 + sensors/radio/WiFiProbabilityGrid.h | 4 +- sensors/radio/model/WiFiModel.h | 3 +- sensors/radio/model/WiFiModelLogDist.h | 2 + sensors/radio/setup/WiFiFingerprint.h | 6 +- sensors/radio/setup/WiFiOptimizer.h | 2 +- tests/Tests.h | 4 +- .../beacon/TestLogDistanceCeilingModel.cpp | 121 ++++++++ tests/sensors/beacon/TestProbabilityFree.cpp | 7 + tests/sensors/pressure/TestBarometer.cpp | 91 +++++- 26 files changed, 1207 insertions(+), 144 deletions(-) create mode 100644 grid/walk/v2/modules/WalkModuleActivityControlPercent.h delete mode 100644 grid/walk/v2/modules/WalkModuleButterActivity.h create mode 100644 sensors/beacon/Beacon.h create mode 100644 sensors/beacon/BeaconMeasurement.h create mode 100644 sensors/beacon/BeaconMeasurements.h create mode 100644 sensors/beacon/BeaconProbability.h create mode 100644 sensors/beacon/BeaconProbabilityFree.h create mode 100644 sensors/beacon/model/BeaconModel.h create mode 100644 sensors/beacon/model/BeaconModelLogDist.h create mode 100644 sensors/beacon/model/BeaconModelLogDistCeiling.h create mode 100644 sensors/pressure/ActivityButterPressurePercent.h delete mode 100644 sensors/radio/LocatedAccessPoint.h create mode 100644 tests/sensors/beacon/TestLogDistanceCeilingModel.cpp create mode 100644 tests/sensors/beacon/TestProbabilityFree.cpp diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 95648d0..0668f03 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -186,6 +186,7 @@ namespace Floorplan { Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} + Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground }; diff --git a/grid/walk/v2/modules/WalkModuleActivityControlPercent.h b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h new file mode 100644 index 0000000..0af1136 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h @@ -0,0 +1,91 @@ +#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H +#define WALKMODULEACTIVITYCONTROLPERCENT_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" +#include "../../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../../grid/GridNode.h" + + +/** favor z-transitions */ +template class WalkModuleActivityControlPercent : public WalkModule { + +private: + + Control* ctrl; + +public: + + /** ctor */ + WalkModuleActivityControlPercent(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; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) state; + (void) startNode; + + + const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; + + //floor and doors + if(potentialNode.getType() == 0 || potentialNode.getType() == 3){ + return ctrl->activityPercent.stay; + } + + //stairs + if(potentialNode.getType() == 1){ + +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown + ctrl->barometer.actProbs.stairsUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;} + return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp); + } + + //elevators + if(potentialNode.getType() == 2){ + +// //we need to do this, that particles are able to walk into an elevator even if the prob for that is low, +// //that happens often since the activity has some delay. +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown + ctrl->barometer.actProbs.elevatorUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;} + + //for walking out of the elevator + return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp); + } + + std::cout << "Node has unknown Type" << std::endl; + return 1.0; + } +}; + +#endif // WALKMODULEACTIVITYCONTROLPERCENT_H diff --git a/grid/walk/v2/modules/WalkModuleButterActivity.h b/grid/walk/v2/modules/WalkModuleButterActivity.h deleted file mode 100644 index bc3f37c..0000000 --- a/grid/walk/v2/modules/WalkModuleButterActivity.h +++ /dev/null @@ -1,86 +0,0 @@ -#ifndef WALKMODULEBUTTERACTIVITY_H -#define WALKMODULEBUTTERACTIVITY_H - -#include "WalkModule.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 { - -public: - - /** ctor */ - WalkModuleButterActivity() { - - // ensure templates WalkState inherits from 'WalkStateBarometerActivity' - StaticAssert::AinheritsB(); - - } - - 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; - } - - double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { - - (void) state; - (void) startNode; - - const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; - - 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.barometer.activity == ActivityButterPressure::Activity::UP){ - if (deltaZ_cm > 0) {return 0;} - if (deltaZ_cm == 0) {return 0.1;} - return 0.9; - } else { - if (deltaZ_cm == 0) {return 0.9;} - return 0.1; - } - - } - - -}; - -#endif // WALKMODULEBUTTERACTIVITY_H diff --git a/main.cpp b/main.cpp index 255a441..03d1e84 100755 --- a/main.cpp +++ b/main.cpp @@ -25,12 +25,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*"; - ::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; + ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + ::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/beacon/Beacon.h b/sensors/beacon/Beacon.h new file mode 100644 index 0000000..1604932 --- /dev/null +++ b/sensors/beacon/Beacon.h @@ -0,0 +1,59 @@ +#ifndef BEACON_H +#define BEACON_H + +#include "../MACAddress.h" + +/** + * represents a single beacon + * a beacon is represented by its MAC-Address and + * may provide a sending power TXP + */ +class Beacon { + +private: + + /** the AP's MAC-Address */ + MACAddress mac; + + /** OPTIONAL the beacons sending power */ + float txp; + +public: + + /** empty ctor */ + Beacon() { + ; + } + + /** ctor with MAC and TXP */ + Beacon(const MACAddress& mac, const float& txp) : mac(mac), txp(txp) { + ; + } + + /** ctor with MAC and TXP */ + Beacon(const std::string& mac, const float& txp) : mac(mac), txp(txp) { + ; + } + + /** ctor with MAC and without TXP */ + Beacon(const MACAddress& mac) : mac(mac), txp() { + ; + } + + /** ctor with MAC and without TXP */ + Beacon(const std::string& mac) : mac(mac), txp() { + ; + } + +public: + + /** get the AP's MAC address */ + inline const MACAddress& getMAC() const {return mac;} + + /** OPTIONAL: get the AP's ssid (if any) */ + inline const float& getTXP() const {return txp;} + + +}; + +#endif // BEACON_H diff --git a/sensors/beacon/BeaconMeasurement.h b/sensors/beacon/BeaconMeasurement.h new file mode 100644 index 0000000..0bc3ea9 --- /dev/null +++ b/sensors/beacon/BeaconMeasurement.h @@ -0,0 +1,44 @@ +#ifndef BEACONMEASUREMENT_H +#define BEACONMEASUREMENT_H + +#include "../MACAddress.h" +#include "../../data/Timestamp.h" +#include "Beacon.h" + +#include + + +/** one observed AP and its signal strength */ +class BeaconMeasurement { + +private: + + /** the timestamp this beacon was discovered at */ + Timestamp ts; + + /** the beacon's mac address */ + Beacon beacon; + + /** signal strength */ + float rssi; + +public: + + /** ctor */ + BeaconMeasurement(const Timestamp ts, const Beacon& beacon, const float rssi) : ts(ts), beacon(beacon), rssi(rssi) {;} + + +public: + + /** get the beacon */ + const Beacon& getBeacon() const {return beacon;} + + /** get the measurements timestamp */ + const Timestamp& getTimestamp() const {return ts;} + + /** get the rssi */ + float getRSSI() const {return rssi;} +}; + + +#endif // BEACONMEASUREMENT_H diff --git a/sensors/beacon/BeaconMeasurements.h b/sensors/beacon/BeaconMeasurements.h new file mode 100644 index 0000000..dbdf489 --- /dev/null +++ b/sensors/beacon/BeaconMeasurements.h @@ -0,0 +1,26 @@ +#ifndef BEACONMEASUREMENTS_H +#define BEACONMEASUREMENTS_H + +#include + +#include "BeaconMeasurement.h" + +/** + * group of several beacon measurements + */ +struct BeaconMeasurements { + + std::vector entries; + + /** remove entries older then 3000 ms*/ + void removeOld(const Timestamp latestTS) { + auto lambda = [latestTS] (const BeaconMeasurement& e) { + Timestamp age = latestTS - e.getTimestamp(); + return age > Timestamp::fromMS(1000*3); + }; + entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end()); + } + +}; + +#endif // BEACONMEASUREMENTS_H diff --git a/sensors/beacon/BeaconProbability.h b/sensors/beacon/BeaconProbability.h new file mode 100644 index 0000000..66c0707 --- /dev/null +++ b/sensors/beacon/BeaconProbability.h @@ -0,0 +1,15 @@ +#ifndef BEACONPROBABILITY_H +#define BEACONPROBABILITY_H + +#include "BeaconMeasurements.h" + +/** + * base class for all Beacon probability calculators. + * such a calculator determines the probabilty for a location (e.g. x,y,z) + * given BeaconMeasurements + */ +class BeaconProbability { + +}; + +#endif // BEACONPROBABILITY_H diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h new file mode 100644 index 0000000..35e902e --- /dev/null +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -0,0 +1,93 @@ +#ifndef BEACONPROBABILITYFREE_H +#define BEACONPROBABILITYFREE_H + +#include "BeaconProbability.h" +#include "BeaconMeasurements.h" +#include "model/BeaconModel.h" +#include "../../math/Distributions.h" +#include "../../data/Timestamp.h" + +#include "../../floorplan/v2/Floorplan.h" + +#include + +/** + * compare BeaconMeasurements within predictions of a given model. + * predictions are just based on the distance to the observed beacon. + */ +class BeaconObserverFree : public BeaconProbability { + +private: + + const float sigma = 8.0f; + + const float sigmaPerSecond = 3.0f; + + /** the RSSI prediction model */ + BeaconModel& model; + + /** the map's floorplan */ + Floorplan::IndoorMap* map; + +public: + + /** ctor */ + BeaconObserverFree(const float sigma, BeaconModel& model) : sigma(sigma), model(model) { + + } + + /** provides the probability for a specific point in space */ + double getProbability(const Point3& pos_m, const Timestamp curTime, const BeaconMeasurements& obs) const { + + double prob = 1.0; + int numMatchingBeacons = 0; + + // process each measured AP + for (const BeaconMeasurement& entry : obs.entries) { + + // sanity check + Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?"); + + // updating the beacons sended txp if available + if(entry.getBeacon().getTXP() != 0.0f){ + //TODO: check if this works + model.updateBeacon(entry); + } + + // get the model's RSSI (if possible!) + const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m); + + // NaN? -> AP not known to the model -> skip + if (modelRSSI != modelRSSI) {continue;} + + + // the scan's RSSI + const float scanRSSI = entry.getRSSI(); + + // 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); + + ++numMatchingBeacons; + + } + + // sanity check + Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); + + return prob; + + } + +}; + +#endif // WIFIPROBABILITYFREE_H diff --git a/sensors/beacon/model/BeaconModel.h b/sensors/beacon/model/BeaconModel.h new file mode 100644 index 0000000..c9ba73f --- /dev/null +++ b/sensors/beacon/model/BeaconModel.h @@ -0,0 +1,46 @@ +#ifndef BEACONMODEL_H +#define BEACONMODEL_H + +#include "../Beacon.h" +#include "../BeaconMeasurement.h" +#include "../../../geo/Point3.h" + +#include + +/** + * interface for signal-strength prediction models. + * + * 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 BeaconModel { + +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 getAllBeacons() const = 0; + + /** + * update the beacons signal strength using the current measurement + * this could happen if the txp is not updated within the floorplan + * + * be careful and don't use fantasy values, this could ruin your localitions + * completely + */ + virtual void updateBeacon(const BeaconMeasurement beacon) = 0; + + + /** + * get the RSSI expected at the given location (in meter) + * for an beacon identified by the given MAC. + * + * if the model can not predict the RSSI for an beacon, it returns NaN! + */ + virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0; + +}; + +#endif // BEACONMODEL_H diff --git a/sensors/beacon/model/BeaconModelLogDist.h b/sensors/beacon/model/BeaconModelLogDist.h new file mode 100644 index 0000000..460cac8 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDist.h @@ -0,0 +1,92 @@ +#ifndef BEACONMODELLOGDIST_H +#define BEACONMODELLOGDIST_H + +#include "BeaconModel.h" +#include "../../radio/model/LogDistanceModel.h" + +#include + +/** + * signal-strength estimation using log-distance model + */ +class BeaconModelLogDist : public BeaconModel { + +public: + + /** parameters describing one beacons to the model */ + struct APEntry { + + Point3 position_m; // the AP's position (in meter) + float txp; // sending power (-40) + float exp; // path-loss-exponent (~2.0 - 4.0) + + /** ctor */ + APEntry(const Point3 position_m, const float txp, const float exp) : + position_m(position_m), txp(txp), exp(exp) {;} + + }; + +private: + + /** map of all beacons (and their parameters) known to the model */ + std::unordered_map beacons; + +public: + + /** ctor */ + BeaconModelLogDist() { + ; + } + + /** get a list of all beacons known to the model */ + std::vector getAllBeacons() const { + std::vector aps; + for (const auto it : beacons) {aps.push_back(Beacon(it.first));} + return aps; + } + + /** make the given beacon (and its parameters) known to the model */ + void addAP(const MACAddress& beacon, const APEntry& params) { + + // sanity check + Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-90:-30]"); + Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]"); + + // add + beacons.insert( std::pair(beacon, params) ); + + } + + void updateBeacon(const BeaconMeasurement beacon) override{ + // try to get the corresponding parameters + const auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC())); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return;} + + it->second.txp = beacon.getBeacon().getTXP(); + } + + virtual float getRSSI(const MACAddress& beacon, const Point3 position_m) const override { + + // try to get the corresponding parameters + const auto it = beacons.find(beacon); + + // AP unknown? -> NAN + if (it == beacons.end()) {return NAN;} + + // the beacons' parameters + const APEntry& params = it->second; + + // free-space (line-of-sight) RSSI + const float distance_m = position_m.getDistance(params.position_m); + const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m); + + // done + return rssiLOS; + + } + +}; + +#endif // BEACONMODELLOGDIST_H diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h new file mode 100644 index 0000000..a10a128 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -0,0 +1,208 @@ +#ifndef BEACONMODELLOGDISTCEILING_H +#define BEACONMODELLOGDISTCEILING_H + +#include "../../../floorplan/v2/Floorplan.h" + +#include "../../../Assertions.h" +#include "BeaconModel.h" +#include "../../radio/model/LogDistanceModel.h" +#include "../BeaconMeasurement.h" + +#include + +/** + * signal-strength estimation using log-distance model + * including ceilings between beacon and position + */ +class BeaconModelLogDistCeiling : public BeaconModel { + +public: + + /** parameters describing one beacon to the model */ + struct APEntry { + + Point3 position_m; // the beacon's position (in meter) + float txp; // sending power (-40) + float exp; // path-loss-exponent (~2.0 - 4.0) + float waf; // attenuation per ceiling/floor (~-8.0) + + /** ctor */ + APEntry(const Point3 position_m, const float txp, const float exp, const float waf) : + position_m(position_m), txp(txp), exp(exp), waf(waf) {;} + + }; + +private: + + /** map of all beacons (and their parameters) known to the model */ + std::unordered_map beacons; + + /** position (height) of all ceilings (in meter) */ + std::vector ceilingsAtHeight_m; + +public: + + /** ctor with floorplan (needed for ceiling position) */ + BeaconModelLogDistCeiling(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); + } + + } + + /** get a list of all beacons known to the model */ + std::vector getAllBeacons() const { + std::vector aps; + for (const auto it : beacons) {aps.push_back(Beacon(it.first));} + return aps; + } + + /** load beacon information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */ + void loadBeaconsFromMap(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::Beacon* beacon : floor->beacons) { + APEntry ape(beacon->getPos(floor), txp, exp, waf); + addBeacon(MACAddress(beacon->mac), ape); + } + } + + } + + /** load beacon information from a vector. use the given fixed TXP/EXP/WAF for all APs */ + void loadBeaconsFromVector(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::Beacon* beacon : floor->beacons) { + APEntry ape(beacon->getPos(floor), txp, exp, waf); + addBeacon(MACAddress(beacon->mac), ape); + } + } + + } + + /** make the given beacon (and its parameters) known to the model */ + void addBeacon(const MACAddress& beacon, const APEntry& params) { + + // sanity check + Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); + 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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); + + // add + beacons.insert( std::pair(beacon, params) ); + + } + + void updateBeacon(const BeaconMeasurement beacon) override { + // try to get the corresponding parameters + auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC())); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return;} + + + // TODO: Check if this works as expected + it->second.txp = beacon.getBeacon().getTXP(); + } + + /** remove all added APs */ + void clear() { + beacons.clear(); + } + + float getRSSI(const MACAddress& beacon, const Point3 position_m) const override { + + // try to get the corresponding parameters + const auto it = beacons.find(beacon); + + // beacon unknown? -> NAN + if (it == beacons.end()) {return NAN;} + + // the access-points' parameters + const APEntry& params = it->second; + + // free-space (line-of-sight) RSSI + const float distance_m = position_m.getDistance(params.position_m); + 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 * numCeilingsBetween(position_m, params.position_m); + + // combine + return rssiLOS + wafLoss; + + } + + + +protected: + + FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilings); + FRIEND_TEST(LogDistanceCeilingModelBeacon, 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 { + + int cnt = 0; + 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;} + } + + return cnt; + + } + +}; + + +#endif // WIFIMODELLOGDISTCEILING_H diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 5940ae6..2d1617c 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -38,12 +38,20 @@ public: Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); - /** change this values for much success */ + /** change this values for much success + * + * Nexus 6: + * butter = Filter::ButterworthLP(10,0.1f,2); + * threshold = 0.025; + * diffSize = 20; + * FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + */ 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.1f,2); - Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.1f,2); + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); public: @@ -68,14 +76,14 @@ public: return STAY; } - input.push_back(History(ts, baro)); + //input.push_back(History(ts, baro)); bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - inputInterp.push_back(History(ts, BarometerData(interpValue))); + //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -113,7 +121,7 @@ public: }else{ actValue = sum; } - sumHist.push_back(actValue); + //sumHist.push_back(actValue); if(actValue > threshold){ currentActivity = DOWN; @@ -127,7 +135,7 @@ public: } } - actHist.push_back(History(ts, BarometerData(currentActivity))); + //actHist.push_back(History(ts, BarometerData(currentActivity))); return currentActivity; diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/pressure/ActivityButterPressurePercent.h new file mode 100644 index 0000000..85290e2 --- /dev/null +++ b/sensors/pressure/ActivityButterPressurePercent.h @@ -0,0 +1,270 @@ +#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H +#define ACTIVITYBUTTERPRESSUREPERCENT_H + +#include "../../data/Timestamp.h" +#include "../../math/filter/Butterworth.h" +#include "../../math/FixedFrequencyInterpolator.h" +#include "../../math/distribution/Normal.h" +#include +#include + + +#include "BarometerData.h" + +/** + * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering + * activity recognition based on a small window given by matlabs diff(window) + */ +class ActivityButterPressurePercent { + +public: + + struct ActivityProbabilities{ + float elevatorDown; + float stairsDown; + float stay; + float stairsUp; + float elevatorUp; + + ActivityProbabilities(float elevatorDown, float stairsDown, + float stay, float stairsUp, float elevatorUp) : + elevatorDown(elevatorDown), stairsDown(stairsDown), + stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;} + + ActivityProbabilities() : + elevatorDown(0.01f), stairsDown(0.01f), + stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;} + }; + + + struct History { + Timestamp ts; + BarometerData data; + History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;} + }; + +private: + //just for debugging and plotting + std::vector input; + std::vector inputInterp; + std::vector output; + std::vector sumHist; + std::vector mvAvgHist; + std::vector actHist; + + bool initialize; + + ActivityProbabilities currentActivity; + + /** change this values for much success */ + const int diffSize = 20; //the number values used for finding the activity. + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + + const float variance = 0.02f; + + const float muStairs = 0.04f; + const float muStay = 0.00f; + const float muEleveator = 0.08; + + std::vector densities = std::vector(5, 1); + std::vector densitiesOld = std::vector(5, 1);; + +public: + + + /** ctor */ + ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){ + initialize = true; + } + + + /** add new sensor readings that were received at the given timestamp */ + ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) { + + //init + if(initialize){ + butter.stepInitialization(baro.hPa); + initialize = false; + + return currentActivity; + } + + //input.push_back(History(ts, baro)); + + bool newInterpolatedValues = false; + + //interpolate & butter + auto callback = [&] (const Timestamp ts, const float val) { + float interpValue = val; + //inputInterp.push_back(History(ts, BarometerData(interpValue))); + + //butter + float butterValue = butter.process(interpValue); + output.push_back(History(ts, BarometerData(butterValue))); + + newInterpolatedValues = true; + + }; + ffi.add(ts, baro.hPa, callback); + + if(newInterpolatedValues == true){ + + //getActivity + if(output.size() > diffSize){ + //diff + std::vector diff; + for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + + float diffVal = output[i+1].data.hPa - output[i].data.hPa; + + diff.push_back(diffVal); + } + + float sum = 0; + for(float val : diff){ + sum += val; + } + + float actValue = sum; + //sumHist.push_back(actValue); + + //calculate the probabilites of walking down/up etc... + densitiesOld = densities; + + //in one building there is an ultra fast elevator, therefore we need to clip the activity value... + if(actValue > muEleveator){ + actValue = muEleveator; + } + if(actValue < -muEleveator){ + actValue = -muEleveator; + } + + float densityElevatorDown = Distribution::Normal::getProbability(muEleveator, variance, actValue); + float densityStairsDown = Distribution::Normal::getProbability(muStairs, variance, actValue); + float densityStay = Distribution::Normal::getProbability(muStay, variance, actValue); + float densityStairsUp = Distribution::Normal::getProbability(-muStairs, variance, actValue); + float densityElevatorUp = Distribution::Normal::getProbability(-muEleveator, variance, actValue); + + _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!"); + _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!"); + _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!"); + _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); + _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); + + //_assertTrue( (densityElevatorDown != 0), "the probability of densityElevatorDown is null!"); + //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!"); + //_assertTrue( (densityStay != 0), "the probability of densityStay is null!"); + //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!"); + //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!"); + + + //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt, + //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV? + + //const Timestamp age = ts - ap.getTimestamp(); + + //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert. + densities[0] = densityElevatorDown; + densities[1] = densityStairsDown; + densities[2] = densityStay; + densities[3] = densityStairsUp; + densities[4] = densityElevatorUp; + + //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end()))); + // if an activity other then staying is detected with a high probability, we are using the previous probability + // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation + // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange. + // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the + // given activity! Since i had no time, this was the fastest solution for now. +// if(highestValueIdx != 2){ +// for(int i = 0; i < densities.size(); ++i){ +// densities[i] *= densitiesOld[i]; +// } +// } + + + //normalize + float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4]; + + for(int i = 0; i < densities.size(); ++i){ + densities[i] /= densitySum; + + //values cant be zero! + densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f); + } + +// densityElevatorDown /= densitySum; +// densityStairsDown /= densitySum; +// densityStay /= densitySum; +// densityStairsUp /= densitySum; +// densityElevatorUp /= densitySum; + + // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility +// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f); +// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f); +// densityStay = (densityStay > 0.0f ? densityStay : 0.01f); +// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f); +// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f); + + currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]); + + } + + //actHist.push_back(currentActivity); + } + + //retruns for every call, indepedent of callback. + return currentActivity; + } + + /** get the current Activity */ + ActivityProbabilities getCurrentActivity() { + return currentActivity; + } + + std::vector getSensorHistory(){ + std::vector tmp; + + for(History val : input){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getInterpolatedHistory(){ + std::vector tmp; + + for(History val : inputInterp){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getOutputHistory(){ + + std::vector tmp; + + for(History val : output){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getSumHistory(){ + return sumHist; + } + + + std::vector getActHistory(){ + + return actHist; + } + + +}; + +#endif // ACTIVITYBUTTERPRESSUREPERCENT_H diff --git a/sensors/radio/LocatedAccessPoint.h b/sensors/radio/LocatedAccessPoint.h deleted file mode 100644 index ddd4693..0000000 --- a/sensors/radio/LocatedAccessPoint.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef LOCATEDACCESSPOINT_H -#define LOCATEDACCESSPOINT_H - -#include "AccessPoint.h" -#include "../../geo/Point3.h" -#include "../../floorplan/v2/Floorplan.h" - -/** - * describes an access-point including its position (in meter) - */ -class LocatedAccessPoint : public AccessPoint, public Point3 { - -public: - - /** ctor */ - LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) { - ; - } - - /** ctor */ - LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) { - ; - } - -}; - -#endif // LOCATEDACCESSPOINT_H diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 3a07386..0b63d75 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -9,7 +9,7 @@ */ class WiFiMeasurement { -public: +private: friend class VAPGrouper; @@ -19,6 +19,9 @@ public: /** the measured signal strength */ float rssi; + /** OPTIONAL. frequence the signal was received */ + float freq; + /** OPTIONAL. timestamp the measurement was recorded at */ Timestamp ts; @@ -29,11 +32,21 @@ public: ; } + /** ctor with freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) { + ; + } + /** ctor with timestamp */ WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { ; } + /** ctor with timestamp and freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { + ; + } + public: /** get the AP we got the measurement for */ @@ -45,6 +58,13 @@ public: /** OPTIONAL: get the measurement's timestamp (if known!) */ const Timestamp& getTimestamp() const {return ts;} + /** OPTIONAL: get the measurement's frequence (if known!) */ + float getFrequency() const {return freq;} + + /** set another signal strength */ + void setRssi(float value){rssi = value;} }; + + #endif // WIFIMEASUREMENT_H diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index a0783f5..0ffc045 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -5,6 +5,7 @@ #include "model/WiFiModel.h" #include "../../math/Distributions.h" #include "VAPGrouper.h" +#include "../../floorplan/v2/Floorplan.h" #include diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index e2cfce2..1ac2189 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -59,7 +59,7 @@ public: // after some runtime, check whether the wifi timestamps make sense // those must not be zero, otherwise something is wrong! if (!obs.entries.empty()) { - Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense..."); + Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().getTimestamp().isZero(), "WiFiMeasurement timestamp is 0. this does not make sense..."); } // process each observed measurement @@ -73,7 +73,7 @@ public: const Timestamp age = curTime - measurement.getTimestamp(); // sigma grows with measurement age - 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(); diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index 05f336c..bebfce4 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -1,7 +1,8 @@ #ifndef WIFIMODEL_H #define WIFIMODEL_H -#include "../LocatedAccessPoint.h" +#include "../AccessPoint.h" +#include "../../../geo/Point3.h" /** * interface for signal-strength prediction models. diff --git a/sensors/radio/model/WiFiModelLogDist.h b/sensors/radio/model/WiFiModelLogDist.h index 417f19c..e30158b 100644 --- a/sensors/radio/model/WiFiModelLogDist.h +++ b/sensors/radio/model/WiFiModelLogDist.h @@ -4,6 +4,8 @@ #include "WiFiModel.h" #include "LogDistanceModel.h" +#include + /** * signal-strength estimation using log-distance model */ diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index d0a2d6b..a92652b 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -46,9 +46,9 @@ struct WiFiFingerprint { 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.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI()); } - avg.rssi /= apMeasurements.entries.size(); + avg.setRssi(avg.getRSSI() / apMeasurements.entries.size()); res.entries.push_back(avg); // add to output } @@ -62,7 +62,7 @@ struct WiFiFingerprint { 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"; + out << wm.getTimestamp().ms() << " " << wm.getAP().getMAC().asString() << " " << wm.getRSSI() << "\n"; } } diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index bcd84d6..5ac22b6 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -89,7 +89,7 @@ public: // add each available AP to its slot (lookup map) for (const WiFiMeasurement& m : measurements.entries) { - const RSSIatPosition rap(fp.pos_m, m.rssi); + const RSSIatPosition rap(fp.pos_m, m.getRSSI()); apMap[m.getAP().getMAC()].push_back(rap); } diff --git a/tests/Tests.h b/tests/Tests.h index fbe9932..3639618 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -6,8 +6,8 @@ #include static inline std::string getDataFile(const std::string& name) { - return "/mnt/data/workspaces/Indoor/tests/data/" + name; - //return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; + //return "/mnt/data/workspaces/Indoor/tests/data/" + name; + return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; } diff --git a/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp new file mode 100644 index 0000000..856394c --- /dev/null +++ b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp @@ -0,0 +1,121 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/beacon/model/BeaconModelLogDistCeiling.h" + +TEST(LogDistanceCeilingModelBeacon, calc) { + + // 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); + + //LocatedAccessPoint ap0("00:00:00:00:00:00", Point3(0,0,0)); + //LocatedAccessPoint ap25("00:00:00:00:00:00", Point3(0,0,2.5)); + + BeaconModelLogDistCeiling model(&map); + + const MACAddress ap0 = MACAddress("00:00:00:00:00:00"); + const MACAddress ap25 = MACAddress("00:00:00:00:00:01"); + + model.addBeacon(ap0, BeaconModelLogDistCeiling::APEntry( Point3(0,0,0), -40, 1.0, -8.0 )); + model.addBeacon(ap25, BeaconModelLogDistCeiling::APEntry( Point3(0,0,2.5), -40, 1.0, -8.0 )); + + + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(1,0,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,1,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,0,1))); + + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(1,0,2.5))); + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(0,1,2.5))); + ASSERT_EQ(-40-8, model.getRSSI(ap25, Point3(0,0,3.5))); // one floor within + + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,8,0))); + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,0,8))+8+8); // two ceilings within + +} + +TEST(LogDistanceCeilingModelBeacon, numCeilings) { + + // 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); + + BeaconModelLogDistCeiling model(&map); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) ); + ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) ); + +} + +TEST(LogDistanceCeilingModelBeacon, 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); + + BeaconModelLogDistCeiling 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/beacon/TestProbabilityFree.cpp b/tests/sensors/beacon/TestProbabilityFree.cpp new file mode 100644 index 0000000..8466c2b --- /dev/null +++ b/tests/sensors/beacon/TestProbabilityFree.cpp @@ -0,0 +1,7 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +//todo + +#endif diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 5dcdfe1..1fef297 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -5,6 +5,7 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" #include "../../../sensors/pressure/ActivityButterPressure.h" +#include "../../../sensors/pressure/ActivityButterPressurePercent.h" #include @@ -78,7 +79,7 @@ TEST(Barometer, LIVE_tendence) { } - sleep(1000); + sleep(1); } @@ -114,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) { } - sleep(1000); + sleep(1); // tendence must be clear and smaller than the sigma @@ -130,6 +131,9 @@ TEST(Barometer, Activity) { std::string filename = getDataFile("barometer/baro1.dat"); std::ifstream infile(filename); + std::vector actHist; + std::vector rawHist; + while (std::getline(infile, line)) { std::istringstream iss(line); @@ -138,35 +142,102 @@ TEST(Barometer, Activity) { while (iss >> ts >> value) { ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); } } - std::vector sum = act.getSumHistory(); - std::vector interpolated = act.getInterpolatedHistory(); - std::vector raw = act.getSensorHistory(); - std::vector butter = act.getOutputHistory(); - std::vector actHist = act.getActHistory(); - K::Gnuplot gp; + K::Gnuplot gpRaw; K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; for(int i=0; i < actHist.size()-1; ++i){ + //raw + K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa); + K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa); + + rawLines.addSegment(raw_p1, raw_p2); + + //results K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); - rawLines.addSegment(input_p1, input_p2); + resultLines.addSegment(input_p1, input_p2); } - plot.add(&rawLines); + plotRaw.add(&rawLines); + plot.add(&resultLines); gp.draw(plot); gp.flush(); + gpRaw.draw(plotRaw); + gpRaw.flush(); + sleep(1); } +TEST(Barometer, ActivityPercent) { + + ActivityButterPressurePercent act; + + //read file + std::string line; + std::string filename = getDataFile("barometer/baro1.dat"); + std::ifstream infile(filename); + + std::vector actHist; + std::vector rawHist; + + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(value); + actHist.push_back(activity); + } + } + + K::Gnuplot gp; + K::Gnuplot gpRaw; + K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; + K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; + + for(int i=0; i < actHist.size()-1; ++i){ + + K::GnuplotPoint2 raw_p1(i, rawHist[i]); + K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]); + + rawLines.addSegment(raw_p1, raw_p2); + + K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown); + K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown); + + resultLines.addSegment(input_p1, input_p2); + } + + plotRaw.add(&rawLines); + plot.add(&resultLines); + + gp.draw(plot); + gp.flush(); + + gpRaw.draw(plotRaw); + gpRaw.flush(); + + sleep(1000); +} + #endif From 0bf8a9c25c63c3bc414e5c933d9b94bde1a83c39 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 16 Nov 2016 12:40:16 +0100 Subject: [PATCH 09/43] small fix. added getter and setter! --- .../beacon/model/BeaconModelLogDistCeiling.h | 2 +- sensors/pressure/ActivityButterPressure.h | 67 +---------- .../pressure/ActivityButterPressurePercent.h | 109 +++--------------- sensors/radio/WiFiMeasurement.h | 23 ++-- tests/sensors/pressure/TestBarometer.cpp | 4 +- 5 files changed, 34 insertions(+), 171 deletions(-) diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h index a10a128..8e794a6 100644 --- a/sensors/beacon/model/BeaconModelLogDistCeiling.h +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -91,7 +91,7 @@ public: // sanity check Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); - Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]"); + Assert::isBetween(params.txp, -90.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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 2d1617c..be53031 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -18,8 +18,6 @@ public: enum Activity {DOWN, STAY, UP}; - - struct History { Timestamp ts; BarometerData data; @@ -27,14 +25,8 @@ public: }; private: - //just for debugging and plotting - std::vector input; - std::vector inputInterp; - std::vector output; - std::vector sumHist; - std::vector mvAvgHist; - std::vector actHist; + std::vector output; Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); @@ -47,8 +39,8 @@ public: * FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); */ const bool additionalLowpassFilter = false; - 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! + const unsigned long diffSize = 20; //the number values used for finding the activity. + const float threshold = 0.025f; // 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 butter2 = Filter::ButterworthLP(10,0.05f,2); @@ -76,14 +68,11 @@ public: return STAY; } - //input.push_back(History(ts, baro)); - bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -97,10 +86,10 @@ public: if(newInterpolatedValues == true){ //getActivity - if((int)output.size() > diffSize){ + if(output.size() > diffSize){ //diff std::vector diff; - for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){ float diffVal = output[i+1].data.hPa - output[i].data.hPa; @@ -121,7 +110,6 @@ public: }else{ actValue = sum; } - //sumHist.push_back(actValue); if(actValue > threshold){ currentActivity = DOWN; @@ -135,8 +123,6 @@ public: } } - //actHist.push_back(History(ts, BarometerData(currentActivity))); - return currentActivity; } @@ -145,49 +131,6 @@ public: Activity getCurrentActivity() { return currentActivity; } - - std::vector getSensorHistory(){ - std::vector tmp; - - for(History val : input){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getInterpolatedHistory(){ - std::vector tmp; - - for(History val : inputInterp){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getOutputHistory(){ - - std::vector tmp; - - for(History val : output){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getSumHistory(){ - return sumHist; - } - - - std::vector getActHistory(){ - - return actHist; - } - - }; #endif // ACTIVITYBUTTERPRESSURE_H diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/pressure/ActivityButterPressurePercent.h index 85290e2..c20583e 100644 --- a/sensors/pressure/ActivityButterPressurePercent.h +++ b/sensors/pressure/ActivityButterPressurePercent.h @@ -14,6 +14,8 @@ /** * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering * activity recognition based on a small window given by matlabs diff(window) + * + * todo: if an elevator is detected, first we have a short time the stairs are more prober. */ class ActivityButterPressurePercent { @@ -44,20 +46,14 @@ public: }; private: - //just for debugging and plotting - std::vector input; - std::vector inputInterp; - std::vector output; - std::vector sumHist; - std::vector mvAvgHist; - std::vector actHist; + std::vector output; bool initialize; ActivityProbabilities currentActivity; /** change this values for much success */ - const int diffSize = 20; //the number values used for finding the activity. + const unsigned long diffSize = 20; //the number values used for finding the activity. Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); @@ -65,10 +61,10 @@ private: const float muStairs = 0.04f; const float muStay = 0.00f; - const float muEleveator = 0.08; + const float muEleveator = 0.08f; std::vector densities = std::vector(5, 1); - std::vector densitiesOld = std::vector(5, 1);; + std::vector densitiesOld = std::vector(5, 1); public: @@ -90,14 +86,11 @@ public: return currentActivity; } - //input.push_back(History(ts, baro)); - bool newInterpolatedValues = false; //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { float interpValue = val; - //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -114,7 +107,7 @@ public: if(output.size() > diffSize){ //diff std::vector diff; - for(int i = output.size() - diffSize; i < output.size() - 1; ++i){ + for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){ float diffVal = output[i+1].data.hPa - output[i].data.hPa; @@ -127,7 +120,6 @@ public: } float actValue = sum; - //sumHist.push_back(actValue); //calculate the probabilites of walking down/up etc... densitiesOld = densities; @@ -152,17 +144,11 @@ public: _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!"); _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!"); - //_assertTrue( (densityElevatorDown != 0), "the probability of densityElevatorDown is null!"); - //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!"); - //_assertTrue( (densityStay != 0), "the probability of densityStay is null!"); - //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!"); - //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!"); - - - //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt, - //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV? - - //const Timestamp age = ts - ap.getTimestamp(); + _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!"); + _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!"); + _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!"); + _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!"); + _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!"); //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert. densities[0] = densityElevatorDown; @@ -171,47 +157,19 @@ public: densities[3] = densityStairsUp; densities[4] = densityElevatorUp; - //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end()))); - // if an activity other then staying is detected with a high probability, we are using the previous probability - // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation - // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange. - // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the - // given activity! Since i had no time, this was the fastest solution for now. -// if(highestValueIdx != 2){ -// for(int i = 0; i < densities.size(); ++i){ -// densities[i] *= densitiesOld[i]; -// } -// } - - //normalize float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4]; - for(int i = 0; i < densities.size(); ++i){ + for(unsigned long i = 0; i < densities.size(); ++i){ densities[i] /= densitySum; //values cant be zero! densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f); } -// densityElevatorDown /= densitySum; -// densityStairsDown /= densitySum; -// densityStay /= densitySum; -// densityStairsUp /= densitySum; -// densityElevatorUp /= densitySum; - - // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility -// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f); -// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f); -// densityStay = (densityStay > 0.0f ? densityStay : 0.01f); -// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f); -// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f); - currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]); } - - //actHist.push_back(currentActivity); } //retruns for every call, indepedent of callback. @@ -223,47 +181,6 @@ public: return currentActivity; } - std::vector getSensorHistory(){ - std::vector tmp; - - for(History val : input){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getInterpolatedHistory(){ - std::vector tmp; - - for(History val : inputInterp){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getOutputHistory(){ - - std::vector tmp; - - for(History val : output){ - tmp.push_back(val.data.hPa); - } - - return tmp; - } - - std::vector getSumHistory(){ - return sumHist; - } - - - std::vector getActHistory(){ - - return actHist; - } - }; diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 0b63d75..d84327d 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -37,10 +37,10 @@ public: ; } - /** ctor with timestamp */ - WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { - ; - } + /** ctor with timestamp */ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { + ; + } /** ctor with timestamp and freq*/ WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { @@ -49,20 +49,23 @@ public: public: - /** get the AP we got the measurement for */ - const AccessPoint& getAP() const {return ap;} + /** get the AP we got the measurement for */ + const AccessPoint& getAP() const {return ap;} - /** get the measurement's signal strength */ - float getRSSI() const {return rssi;} + /** get the measurement's signal strength */ + float getRSSI() const {return rssi;} - /** OPTIONAL: get the measurement's timestamp (if known!) */ - const Timestamp& getTimestamp() const {return ts;} + /** OPTIONAL: get the measurement's timestamp (if known!) */ + const Timestamp& getTimestamp() const {return ts;} /** OPTIONAL: get the measurement's frequence (if known!) */ float getFrequency() const {return freq;} /** set another signal strength */ void setRssi(float value){rssi = value;} + + /** set the timestamp */ + void setTimestamp(const Timestamp& val){ts = val;} }; diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 1fef297..7c3451f 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -178,7 +178,7 @@ TEST(Barometer, Activity) { gpRaw.draw(plotRaw); gpRaw.flush(); - sleep(1); + sleep(5); } @@ -236,7 +236,7 @@ TEST(Barometer, ActivityPercent) { gpRaw.draw(plotRaw); gpRaw.flush(); - sleep(1000); + sleep(5); } From ae357ffd4b391cfd3a56ea1aba29d41720fa8552 Mon Sep 17 00:00:00 2001 From: FrankE Date: Tue, 29 Nov 2016 21:29:10 +0100 Subject: [PATCH 10/43] added new parameters to Floorplan-APs and -Beacons --- floorplan/v2/Floorplan.h | 13 +++++++++++++ floorplan/v2/FloorplanReader.h | 9 +++++++++ floorplan/v2/FloorplanWriter.h | 9 +++++++++ 3 files changed, 31 insertions(+) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 0668f03..64644c8 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -172,6 +172,11 @@ namespace Floorplan { std::string name; std::string mac; Point3 pos; // z is relative to the floor's height + struct Model { + float txp; + float exp; + float waf; + } model; AccessPoint() : name(), mac(), pos() {;} AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;} bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} @@ -182,7 +187,15 @@ namespace Floorplan { struct Beacon { std::string name; std::string mac; + std::string major; + std::string minor; + std::string uuid; Point3 pos; // z is relative to the floor's height + struct Model { + float txp; + float exp; + float waf; + } model; Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 396f338..2af6df0 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -241,6 +241,9 @@ namespace Floorplan { ap->mac = n->Attribute("mac"); ap->name = n->Attribute("name"); ap->pos = parsePoint3(n); + ap->model.txp = n->FloatAttribute("mdl_txp"); + ap->model.exp = n->FloatAttribute("mdl_exp"); + ap->model.waf = n->FloatAttribute("mdl_waf"); return ap; } @@ -259,6 +262,12 @@ namespace Floorplan { Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); b->name = n->Attribute("name"); + b->major = n->Attribute("major"); + b->minor = n->Attribute("minor"); + b->uuid = n->Attribute("uuid"); + b->model.txp = n->FloatAttribute("mdl_txp"); + b->model.exp = n->FloatAttribute("mdl_exp"); + b->model.waf = n->FloatAttribute("mdl_waf"); b->pos = parsePoint3(n); return b; } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index bd0bc4e..3a927a0 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -153,6 +153,9 @@ namespace Floorplan { accesspoint->SetAttribute("x", ap->pos.x); accesspoint->SetAttribute("y", ap->pos.y); accesspoint->SetAttribute("z", ap->pos.z); + accesspoint->SetAttribute("mdl_txp", ap->model.txp); + accesspoint->SetAttribute("mdl_exp", ap->model.exp); + accesspoint->SetAttribute("mdl_waf", ap->model.waf); accesspoints->InsertEndChild(accesspoint); } floor->InsertEndChild(accesspoints); @@ -162,9 +165,15 @@ namespace Floorplan { XMLElem* beacon = doc.NewElement("beacon"); beacon->SetAttribute("name", b->name.c_str()); beacon->SetAttribute("mac", b->mac.c_str()); + beacon->SetAttribute("major", b->major.c_str()); + beacon->SetAttribute("minor", b->minor.c_str()); + beacon->SetAttribute("uuid", b->uuid.c_str()); beacon->SetAttribute("x", b->pos.x); beacon->SetAttribute("y", b->pos.y); beacon->SetAttribute("z", b->pos.z); + beacon->SetAttribute("mdl_txp", b->model.txp); + beacon->SetAttribute("mdl_exp", b->model.exp); + beacon->SetAttribute("mdl_waf", b->model.waf); beacons->InsertEndChild(beacon); } floor->InsertEndChild(beacons); From ef6f44969fa4e03c1641a42c411bac31d7500864 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 1 Dec 2016 19:48:27 +0100 Subject: [PATCH 11/43] added support for ground truth points \n small fixed in beaconprob --- floorplan/v2/Floorplan.h | 12 +++++++++++ floorplan/v2/FloorplanReader.h | 19 ++++++++++++++++ floorplan/v2/FloorplanWriter.h | 10 +++++++++ math/distribution/VonMises.h | 8 +++++-- sensors/MACAddress.h | 30 ++++++++++++++++++-------- sensors/beacon/BeaconProbabilityFree.h | 3 +-- 6 files changed, 69 insertions(+), 13 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 64644c8..c433d17 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -117,6 +117,7 @@ namespace Floorplan { struct POI; struct Stair; struct Elevator; + struct GroundTruthPoint; using FloorOutline = std::vector; using FloorObstacles = std::vector; @@ -127,6 +128,7 @@ namespace Floorplan { using FloorPOIs = std::vector; using FloorStairs = std::vector; using FloorElevators = std::vector; + using FloorGroundTruthPoints = std::vector; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -143,6 +145,7 @@ namespace Floorplan { FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor FloorElevators elevators; // all elevators within one floor + FloorGroundTruthPoints gtpoints; // all ground truth points within one floor //FloorKeyValue other; // other, free elements Floor() {;} @@ -167,6 +170,15 @@ namespace Floorplan { bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);} }; + /** a GroundTruthPoint located somewhere on a floor */ + struct GroundTruthPoint { + int id; //TODO: this value can be changed and isn't set incremental within the indoormap + Point2 pos; + GroundTruthPoint() : id(), pos() {;} + GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;} + bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} + }; + /** an AccessPoint located somewhere on a floor */ struct AccessPoint { std::string name; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 2af6df0..7f00748 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -97,6 +97,7 @@ namespace Floorplan { if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} + if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} } return floor; } @@ -185,6 +186,24 @@ namespace Floorplan { } + /** parse the tag */ + static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } + } + return vec; + } + + /** parse a tag */ + static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { + GroundTruthPoint* gtp = new GroundTruthPoint(); + gtp->id = el->IntAttribute("id"); + gtp->pos = parsePoint2(el); + return gtp; + } + + /** parse the tag */ static std::vector parseFloorAccessPoints(const XMLElem* el) { std::vector vec; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 3a927a0..c21ebbb 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -145,6 +145,16 @@ namespace Floorplan { } floor->InsertEndChild(pois); + XMLElem* gtpoints = doc.NewElement("gtpoints"); + for (const GroundTruthPoint* gtp : mf->gtpoints) { + XMLElem* elem = doc.NewElement("gtpoint"); + elem->SetAttribute("id", gtp->id); + elem->SetAttribute("x", gtp->pos.x); + elem->SetAttribute("y", gtp->pos.y); + gtpoints->InsertEndChild(elem); + } + floor->InsertEndChild(gtpoints); + XMLElem* accesspoints = doc.NewElement("accesspoints"); for (const AccessPoint* ap : mf->accesspoints) { XMLElem* accesspoint = doc.NewElement("accesspoint"); diff --git a/math/distribution/VonMises.h b/math/distribution/VonMises.h index 789e0c8..40b4189 100644 --- a/math/distribution/VonMises.h +++ b/math/distribution/VonMises.h @@ -19,7 +19,7 @@ namespace Distribution { const T mu; /** like 1.0/variance of the distribution */ - const T kappa; + T kappa; /** pre-calcuated look-up-table */ std::vector lut; @@ -30,7 +30,7 @@ namespace Distribution { public: /** ctor */ - VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) { + VonMises(const T mu, T kappa) : mu(mu), kappa(kappa) { } @@ -49,6 +49,10 @@ namespace Distribution { } + void setKappa(T _kappa){ + kappa = _kappa; + } + }; } diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h index 19db998..02aee2e 100644 --- a/sensors/MACAddress.h +++ b/sensors/MACAddress.h @@ -47,15 +47,27 @@ public: MACAddress(const std::string& str) { // sanity check - if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");} - - mac = 0; // all zeros - fields.h5 = hexWordToInt(str[ 0], str[ 1]); - fields.h4 = hexWordToInt(str[ 3], str[ 4]); - fields.h3 = hexWordToInt(str[ 6], str[ 7]); - fields.h2 = hexWordToInt(str[ 9], str[10]); - fields.h1 = hexWordToInt(str[12], str[13]); - fields.h0 = hexWordToInt(str[15], str[16]); + if (str.size() == 17 ){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 3], str[ 4]); + fields.h3 = hexWordToInt(str[ 6], str[ 7]); + fields.h2 = hexWordToInt(str[ 9], str[10]); + fields.h1 = hexWordToInt(str[12], str[13]); + fields.h0 = hexWordToInt(str[15], str[16]); + } + else if (str.size() == 12){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 2], str[ 3]); + fields.h3 = hexWordToInt(str[ 4], str[ 5]); + fields.h2 = hexWordToInt(str[ 6], str[7]); + fields.h1 = hexWordToInt(str[8], str[9]); + fields.h0 = hexWordToInt(str[10], str[11]); + } + else{ + throw Exception("invalid hex string length. must be 17 or 12 (without :)"); + } } diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h index 35e902e..46f6475 100644 --- a/sensors/beacon/BeaconProbabilityFree.h +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -20,7 +20,6 @@ class BeaconObserverFree : public BeaconProbability { private: const float sigma = 8.0f; - const float sigmaPerSecond = 3.0f; /** the RSSI prediction model */ @@ -82,7 +81,7 @@ public: } // sanity check - Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); + //Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?"); return prob; From 54894a0c2308d5af2bb2f38d5619dc06fdcc6fd9 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 2 Mar 2017 18:08:02 +0100 Subject: [PATCH 12/43] many small changes, added filereader with beacons, added motion detection stuff, testcases --- CMakeLists.txt | 2 +- floorplan/v2/FloorplanReader.h | 12 +- main.cpp | 6 +- sensors/imu/GravityData.h | 53 ++++ sensors/imu/LinearAccelerationData.h | 53 ++++ sensors/imu/MotionDetection.h | 163 ++++++++++++ sensors/offline/FileReader.h | 299 ++++++++++++++++++++++ tests/sensors/imu/TestMotionDetection.cpp | 200 +++++++++++++++ 8 files changed, 778 insertions(+), 10 deletions(-) create mode 100644 sensors/imu/GravityData.h create mode 100644 sensors/imu/LinearAccelerationData.h create mode 100644 sensors/imu/MotionDetection.h create mode 100644 sensors/offline/FileReader.h create mode 100644 tests/sensors/imu/TestMotionDetection.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d8d105a..d7b87de 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O0 + #-O0 -march=native -DWITH_TESTS diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 7f00748..66ffe3d 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -281,12 +281,12 @@ namespace Floorplan { Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); b->name = n->Attribute("name"); - b->major = n->Attribute("major"); - b->minor = n->Attribute("minor"); - b->uuid = n->Attribute("uuid"); - b->model.txp = n->FloatAttribute("mdl_txp"); - b->model.exp = n->FloatAttribute("mdl_exp"); - b->model.waf = n->FloatAttribute("mdl_waf"); +// b->major = n->Attribute("major"); +// b->minor = n->Attribute("minor"); +// b->uuid = n->Attribute("uuid"); +// b->model.txp = n->FloatAttribute("mdl_txp"); +// b->model.exp = n->FloatAttribute("mdl_exp"); +// b->model.waf = n->FloatAttribute("mdl_waf"); b->pos = parsePoint3(n); return b; } diff --git a/main.cpp b/main.cpp index 03d1e84..fbb4e08 100755 --- a/main.cpp +++ b/main.cpp @@ -25,12 +25,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - - ::testing::GTEST_FLAG(filter) = "*Barometer*"; + ::testing::GTEST_FLAG(filter) = "*MotionDetection*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/imu/GravityData.h b/sensors/imu/GravityData.h new file mode 100644 index 0000000..d522f20 --- /dev/null +++ b/sensors/imu/GravityData.h @@ -0,0 +1,53 @@ +#ifndef GRAVITYDATA_H +#define GRAVITYDATA_H + +#include +#include + + +/** data received from an accelerometer sensor */ +struct GravityData { + + float x; + float y; + float z; + + GravityData() : x(0), y(0), z(0) {;} + + GravityData(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 ); + } + + GravityData& operator += (const GravityData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + GravityData& operator -= (const GravityData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + GravityData operator - (const GravityData& o) const { + return GravityData(x-o.x, y-o.y, z-o.z); + } + + GravityData operator / (const float val) const { + return GravityData(x/val, y/val, z/val); + } + + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + +}; + +#endif // GRAVITYDATA_H diff --git a/sensors/imu/LinearAccelerationData.h b/sensors/imu/LinearAccelerationData.h new file mode 100644 index 0000000..9ae116c --- /dev/null +++ b/sensors/imu/LinearAccelerationData.h @@ -0,0 +1,53 @@ +#ifndef LINEARACCELERATIONDATA_H +#define LINEARACCELERATIONDATA_H + +#include +#include + + +/** data received from an accelerometer sensor */ +struct LinearAccelerationData { + + float x; + float y; + float z; + + LinearAccelerationData() : x(0), y(0), z(0) {;} + + LinearAccelerationData(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 ); + } + + LinearAccelerationData& operator += (const LinearAccelerationData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + LinearAccelerationData& operator -= (const LinearAccelerationData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + LinearAccelerationData operator - (const LinearAccelerationData& o) const { + return LinearAccelerationData(x-o.x, y-o.y, z-o.z); + } + + LinearAccelerationData operator / (const float val) const { + return LinearAccelerationData(x/val, y/val, z/val); + } + + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + +}; + +#endif // LINEARACCELERATIONDATA_H diff --git a/sensors/imu/MotionDetection.h b/sensors/imu/MotionDetection.h new file mode 100644 index 0000000..76284fe --- /dev/null +++ b/sensors/imu/MotionDetection.h @@ -0,0 +1,163 @@ +#ifndef MOTIONDETECTION_H +#define MOTIONDETECTION_H + +#include "GravityData.h" +#include "LinearAccelerationData.h" +#include "../../data/Timestamp.h" +#include "../../math/MovingAverageTS.h" +#include "../../misc/Debug.h" + +#include + +#include +#include + +#include +#include +#include +#include +#include + + +#include "../../Assertions.h" + +class MotionDetection { + +private: + + bool newAccelerationMeasurementArrived = false; + bool newGravityMeasurementArrived = false; + + Eigen::Vector3f curGravity; + Eigen::Vector3f curLinearAcceleration; + + //fast algo + Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix + + int numMeasurementsPerInterval, updateCnt; + int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms + + struct Motion{ + Eigen::Vector2f vec = Eigen::Vector2f::Identity(); + Timestamp lastEstimation; + }; + + Motion curMotion; + Motion prevMotion; + + const char* name = "MotionDetection"; + +public: + + /** ctor */ + MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) { + ; + } + + void addGravity(const Timestamp& ts, const GravityData& grav){ + + curGravity << grav.x, grav.y, grav.z; + newGravityMeasurementArrived = true; + + updateProjectionVectorFast(ts); + } + + void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) { + + curLinearAcceleration << acc.x, acc.y, acc.z; + newAccelerationMeasurementArrived = true; + + updateProjectionVectorFast(ts); + } + + /** return the current motion axis + * NOTE: if no data is available, this vector is the Identity + */ + Eigen::Vector2f getCurrentMotionAxis(){ + return curMotion.vec; + } + + /** returns the radians between [-pi, pi] between successive motion axis estimations */ + float getMotionChangeInRad(){ + //TODO: put this in an EigenHelper Class within geo + const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x(); + const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f)); + + //nan? + if(std::isnan(ang)){ + Log::add(name, "The motion change angle is nAn, this is not correct!"); + } + + if(updateCnt < 2){ + return 0; + } + + return ang; + } + +private: + + FRIEND_TEST(MotionDetection, motionAngle); + FRIEND_TEST(MotionDetection, motionAxis); + + Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){ + + Eigen::SelfAdjointEigenSolver solver(covarianceMatrix); + return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue + } + + void updateProjectionVectorFast(const Timestamp& ts){ + + //make sure we have both measurements for calculation + if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){ + + numMeasurementsPerInterval++; + + //project acc into perpendicular plane of grav (using standard vector projection) + Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity; + + //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this + if(proj.isZero()){ + proj = curLinearAcceleration; + Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?"); + } + + //we are only interested in x,y + Eigen::Vector2f vec; + vec << proj.x(), proj.y(); + + // sum this up for later averaging. + sumProjectedCov += vec*vec.transpose(); + + // start with the first available timestamp + if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;} + + //update the motion axis depending on the update interval + if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){ + + prevMotion = curMotion; + + //calculate the average of the coveriance matrix + Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval; + + curMotion.vec = updateMotionAxis(Q); + curMotion.lastEstimation = ts; + + reset(); + } + + newGravityMeasurementArrived = false; + newAccelerationMeasurementArrived = false; + } + //do nothing + } + + void reset(){ + numMeasurementsPerInterval = 0; + sumProjectedCov = Eigen::Matrix2f::Zero(); + ++updateCnt; + } + +}; + +#endif // MOTIONDETECTION_H diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h new file mode 100644 index 0000000..bc30410 --- /dev/null +++ b/sensors/offline/FileReader.h @@ -0,0 +1,299 @@ +#ifndef FILEREADER_H +#define FILEREADER_H + +#include +#include +#include +#include + +#include "../../math/Interpolator.h" +#include "../../sensors/radio/WiFiMeasurements.h" +#include "../../sensors/pressure/BarometerData.h" +#include "../../sensors/imu/AccelerometerData.h" +#include "../../sensors/imu/GyroscopeData.h" +#include "../../sensors/imu/GravityData.h" +#include "../../sensors/imu/LinearAccelerationData.h" +#include "../../sensors/beacon/BeaconMeasurements.h" + + +#include "../../geo/Point2.h" +#include "../../grid/factory/v2/GridFactory.h" +#include "../../grid/factory/v2/Importance.h" +#include "../../floorplan/v2/Floorplan.h" + +class FileReader { + +public: + + template struct TS { + const uint64_t ts; + T data; + TS(const uint64_t ts) : ts(ts) {;} + TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;} + }; + + enum class Sensor { + ACC, + GYRO, + WIFI, + POS, + BARO, + BEACON, + LIN_ACC, + GRAVITY, + }; + + /** entry for one sensor */ + struct Entry { + Sensor type; + uint64_t ts; + int idx; + Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;} + }; + + std::vector> groundTruth; + std::vector> wifi; + std::vector> beacon; + std::vector> acc; + std::vector> gyro; + std::vector> barometer; + std::vector> lin_acc; + std::vector> gravity; + + /** ALL entries */ + std::vector entries; + +public: + + FileReader(const std::string& file) { + parse(file); + } + + const std::vector& getEntries() const {return entries;} + + + const std::vector>& getGroundTruth() const {return groundTruth;} + + const std::vector>& getWiFiGroupedByTime() const {return wifi;} + + const std::vector>& getBeacons() const {return beacon;} + + const std::vector>& getAccelerometer() const {return acc;} + + const std::vector>& getGyroscope() const {return gyro;} + + const std::vector>& getBarometer() const {return barometer;} + + const std::vector>& getLinearAcceleration() const {return lin_acc;} + + const std::vector>& getGravity() const {return gravity;} + +private: + + void parse(const std::string& file) { + + std::ifstream inp(file); + if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} + + while(!inp.eof() && !inp.bad()) { + + uint64_t ts; + char delim; + int idx = -1; + std::string data; + + inp >> ts; + inp >> delim; + inp >> idx; + inp >> delim; + inp >> data; + + if (idx == 8) {parseWiFi(ts, data);} + else if (idx == 9) {parseBeacons(ts, data);} + else if (idx == 99) {parseGroundTruth(ts, data);} + else if (idx == 0) {parseAccelerometer(ts, data);} + else if (idx == 3) {parseGyroscope(ts, data);} + else if (idx == 5) {parseBarometer(ts, data);} + else if (idx == 2) {parseLinearAcceleration(ts,data);} + else if (idx == 1) {parseGravity(ts,data);} + + // TODO: this is a hack... + // the loop is called one additional time after the last entry + // and keeps the entries of entry + + } + + inp.close(); + + } + + void parseLinearAcceleration(const uint64_t ts, const std::string& data){ + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z))); + lin_acc.push_back(elem); + entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1)); + } + + void parseGravity(const uint64_t ts, const std::string& data){ + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); + gravity.push_back(elem); + entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); + } + + void parseAccelerometer(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); + acc.push_back(elem); + entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); + + } + + void parseGyroscope(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); + + TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); + gyro.push_back(elem); + entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); + + } + + void parseWiFi(const uint64_t ts, const std::string& data) { + + std::string tmp = data; + + // add new wifi reading + wifi.push_back(TS(ts, WiFiMeasurements())); + entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1)); + + // process all APs + while(!tmp.empty()) { + + auto pos1 = tmp.find(';'); + auto pos2 = tmp.find(';', pos1+1); + auto pos3 = tmp.find(';', pos2+1); + + std::string mac = tmp.substr(0, pos1); + std::string freq = tmp.substr(pos1+1, pos2); + std::string rssi = tmp.substr(pos2+1, pos3); + + tmp = tmp.substr(pos3); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + // append AP to current scan-entry + WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts)); + wifi.back().data.entries.push_back(e); + } + + } + + void parseBeacons(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + const auto pos3 = data.find(';', pos2+1); + + const std::string mac = data.substr(0, pos1); + const std::string rssi = data.substr(pos1+1, pos2); + const std::string txp = data.substr(pos2+1, pos3); + + //yes the timestamp is redundant here, but in case of multiusage... + TS e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi))); + beacon.push_back(e); + entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1)); + + } + + void parseGroundTruth(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + std::string gtIndex = data.substr(0, pos1); + + TS elem(ts, std::stoi(gtIndex)); + groundTruth.push_back(elem); + + } + + void parseBarometer(const uint64_t ts, const std::string& data) { + + const auto pos1 = data.find(';'); + + const std::string hPa = data.substr(0, pos1); + + TS elem(ts, BarometerData(std::stof(hPa))); + barometer.push_back(elem); + entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + + } + +public: + const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { + + // finde alle positionen der waypoints im gtPath aus map + std::unordered_map waypointsMap; + for(Floorplan::Floor* f : map->floors){ + float h = f->atHeight; + for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){ + + //wenn die gleiche id 2x vergeben wurde, knallt es + if(waypointsMap.find(gtp->id) == waypointsMap.end()){ + waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)}); + } + else{ + throw std::string("the floorplan's ground truth contains two points with identical id's!"); + } + + } + } + + // bringe diese in richtige reihenfolge und füge timestamp hinzu + Interpolator interpol; + + int it = 0; + for(int id : gtPath){ + auto itMap = waypointsMap.find(id); + if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");} + + //the time, when the gt button was clicked on the app + uint64_t tsGT = groundTruth[it++].ts; + interpol.add(tsGT, itMap->second); + + } + + if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){ + throw std::string("No Ground Truth points found within the map.xml file"); + } + + return interpol; + } + +}; + +#endif // FILEREADER_H diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp new file mode 100644 index 0000000..47bc411 --- /dev/null +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -0,0 +1,200 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/imu/MotionDetection.h" +#include "../../../sensors/imu/TurnDetection.h" +#include "../../../sensors/offline/FileReader.h" + +/** visualize the motionAxis */ +TEST(MotionDetection, motionAxis) { + + MotionDetection md; + + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + + //Walking with smartphone straight and always parallel to motion axis + //std::string filename = getDataFile("motion/straight_potrait.csv"); + + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); + + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); + + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); + + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); + + FileReader fr(filename); + + K::Gnuplot gp; + K::GnuplotPlot plot; + + gp << "set xrange[-1:1]\n set yrange[-1:1]\n"; + + + Eigen::Vector2f curVec; + float motionAxisAngleRad; + Timestamp ts; + Timestamp lastTs; + + //calc motion axis + for (const FileReader::Entry& e : fr.getEntries()) { + + ts = Timestamp::fromMS(e.ts); + + if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + curVec = md.getCurrentMotionAxis(); + motionAxisAngleRad = md.getMotionChangeInRad(); + } + + // start with the first available timestamp + if (lastTs.isZero()) {lastTs = ts;} + + if(ts - lastTs > Timestamp::fromMS(500)) { + + lastTs = ts; + + K::GnuplotPoint2 raw_p1(0, 0); + K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0)); + K::GnuplotPlotElementLines motionLines; + motionLines.addSegment(raw_p1, raw_p2); + plot.add(&motionLines); + + gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n"; + + gp.draw(plot); + gp.flush(); + //usleep(5000*33); + } + } + + //was passiert bei grenzwerten. 90° oder sowas. + //wie stabil ist die motion axis eigentlich? + //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen? + //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading? + +} + +/** comparing motionAngle and turnAngle */ +TEST(MotionDetection, motionAngle) { + + MotionDetection md; + TurnDetection td; + + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + + //Walking with smartphone straight and always parallel to motion axis + std::string filename = getDataFile("motion/straight_potrait.csv"); + + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + //std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); + + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); + + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); + + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); + + FileReader fr(filename); + Timestamp ts; + + //save for later plotting + std::vector delta_motionAngles; + std::vector delta_turnAngles; + + //calc motion axis + for (const FileReader::Entry& e : fr.getEntries()) { + + ts = Timestamp::fromMS(e.ts); + + if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + delta_motionAngles.push_back(md.getMotionChangeInRad()); + + } else if (e.type == FileReader::Sensor::ACC) { + const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(ts, _acc.data); + + } else if (e.type == FileReader::Sensor::GYRO) { + const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; + delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); + } + + } + + //draw motion + static K::Gnuplot gpMotion; + K::GnuplotPlot plotMotion; + K::GnuplotPlotElementLines motionLines; + + for(int i = 0; i < delta_motionAngles.size() - 1; ++i){ + + K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]); + motionLines.addSegment(raw_p1, raw_p2); + + } + + gpMotion << "set title 'Motion Detection'\n"; + plotMotion.add(&motionLines); + gpMotion.draw(plotMotion); + gpMotion.flush(); + + + //draw rotation + static K::Gnuplot gpTurn; + K::GnuplotPlot plotTurn; + K::GnuplotPlotElementLines turnLines; + + for(int i = 0; i < delta_turnAngles.size() - 1; ++i){ + + K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]); + turnLines.addSegment(raw_p1, raw_p2); + } + + gpTurn << "set title 'Turn Detection'\n"; + plotTurn.add(&turnLines); + gpTurn.draw(plotTurn); + gpTurn.flush(); + + sleep(1); + +} + + +#endif From 62087fe0729104581273a6f4477f4b1e7cb7aab4 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 3 Mar 2017 12:09:19 +0100 Subject: [PATCH 13/43] added WalkModule for von Mises Heading --- .../v2/modules/WalkModuleHeadingVonMises.h | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 grid/walk/v2/modules/WalkModuleHeadingVonMises.h diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h new file mode 100644 index 0000000..9c01a79 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -0,0 +1,123 @@ +#ifndef WALKMODULEHEADINGVONMISES_H +#define WALKMODULEHEADINGVONMISES_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" + + +/** keep the state's heading */ +template class WalkModuleHeadingVonMises : public WalkModule { + +private: + + /** van-Mises distribution */ + Distribution::VonMises dist; + + /** random noise */ + Distribution::Normal distNoise; + + const Control* ctrl; + +public: + + /** ctor 3.0 should be OK! */ + WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) : + dist(Distribution::VonMises(0.0f, 2.0f)), + distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)), + ctrl(ctrl) { + + // ensure the template WalkState inherits from 'WalkStateHeading'! + StaticAssert::AinheritsB(); + + } + + + 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->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 + 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->turnSinceLastTransition_rad + var; + + //set kappa of mises + float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad)); + dist.setKappa(kappa); + + } + + 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; + + // ignore for stairs? + //if (nextNode.getType() == GridNode::TYPE_STAIR) {return;} + + // for elevator edges [same (x,y) but different z] do not adjust anything + 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); + + // get the heading requested by the state + const Heading stateHead = state.heading.direction; + + // get the error (signed difference) between both + const float angularDiff = stateHead.getSignedDiff(head); + + // adjust the error. + // note: the error may get > +/- 2PI but this is not an issue! + // when the error is added to the current heading within getProbability(), + // it is ensured their sum is within [0:2pi] + state.heading.error += angularDiff; + + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (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 (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); + + // compare the heading against the state's heading - the last error + const Heading stateHead = state.heading.direction + state.heading.error; + + // get the difference + const float angularDiff = head.getDiffHalfRAD(stateHead); + + // determine probability + return dist.getProbability(angularDiff); + + } + + +}; + +#endif // WALKMODULEHEADING_H From e48d3bafcdc6ed19b3aae959d23ac33bc258d4bc Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 9 Mar 2017 18:57:47 +0100 Subject: [PATCH 14/43] added kullback leibler for gaussian cases --- main.cpp | 2 +- math/Distributions.h | 1 + math/distribution/Normal.h | 9 + math/distribution/NormalN.h | 50 ++++ math/divergence/KullbackLeibler.h | 90 ++++++++ sensors/beacon/BeaconProbabilityFree.h | 4 +- sensors/radio/WiFiProbabilityFree.h | 7 +- sensors/radio/WiFiProbabilityGrid.h | 5 +- tests/math/divergence/TestKullbackLeibler.cpp | 214 ++++++++++++++++++ 9 files changed, 374 insertions(+), 8 deletions(-) create mode 100644 math/distribution/NormalN.h create mode 100644 math/divergence/KullbackLeibler.h create mode 100644 tests/math/divergence/TestKullbackLeibler.cpp diff --git a/main.cpp b/main.cpp index fbb4e08..b103b91 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*MotionDetection*"; + ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/Distributions.h b/math/Distributions.h index 1879357..93d35c8 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -8,5 +8,6 @@ #include "distribution/VonMises.h" #include "distribution/Region.h" #include "distribution/Triangle.h" +#include "distribution/NormalN.h" #endif // DISTRIBUTIONS_H diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index fc073f8..b40ce67 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -44,6 +44,15 @@ namespace Distribution { gen.seed(seed); } + /** get the mean value */ + const T getMu() { + return this->mu; + } + + /** get the standard deviation */ + const T getSigma() { + return this->sigma; + } /** get the probability for the given value */ static T getProbability(const T mu, const T sigma, const T val) { diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h new file mode 100644 index 0000000..c628828 --- /dev/null +++ b/math/distribution/NormalN.h @@ -0,0 +1,50 @@ +#ifndef NORMALN_H +#define NORMALN_H + +#include + +namespace Distribution { + + class NormalDistributionN { + + private: + + const Eigen::VectorXd mu; + const Eigen::MatrixXd sigma; + + const double _a; + const Eigen::MatrixXd _sigmaInv; + + public: + + /** ctor */ + NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + + } + + + /** get probability for the given value */ + double getProbability(const Eigen::VectorXd val) const { + const double b = ((val-mu).transpose() * _sigmaInv * (val-mu)); + return _a * std::exp(-b/2.0); + } + + /** get the mean vector */ + const Eigen::VectorXd getMu(){ + return this->mu; + } + + /** get covariance matrix */ + const Eigen::MatrixXd getSigma(){ + return this->sigma; + } + + const Eigen::MatrixXd getSigmaInv(){ + return this->_sigmaInv; + } + + }; + +} +#endif // NORMALN_H diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h new file mode 100644 index 0000000..357b1c3 --- /dev/null +++ b/math/divergence/KullbackLeibler.h @@ -0,0 +1,90 @@ +#ifndef KULLBACKLEIBLER_H +#define KULLBACKLEIBLER_H + +#include "../distribution/Normal.h" +#include "../distribution/NormalN.h" + +#include "../../Assertions.h" +#include + +namespace Divergence { + + template class KullbackLeibler { + + public: + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution + * Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/ + */ + static inline Scalar getUnivariateGauss(Distribution::Normal norm1, Distribution::Normal norm2){ + + auto sigma1Quad = norm1.getSigma() * norm1.getSigma(); + auto sigma2Quad = norm2.getSigma() * norm2.getSigma(); + auto mu12Quad = (norm1.getMu() - norm2.getMu()) * (norm1.getMu() - norm2.getMu()); + auto log1 = std::log(norm1.getSigma()); + auto log2 = std::log(norm2.getSigma()); + + // kl = log(sigma_2 / sigma_1) + ((sigma_1^2 + (mu_1 - mu_2)^2) / 2 * sigma_2^2) - 0.5 + double klb = (log2 - log1) + ((sigma1Quad + mu12Quad)/(2.0 * sigma2Quad)) - 0.5; + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution symmetric*/ + static inline Scalar getUnivariateGaussSymmetric(Distribution::Normal norm1, Distribution::Normal norm2){ + return getUnivariateGauss(norm1, norm2) + getUnivariateGauss(norm2, norm1); + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution */ + static inline Scalar getMultivariateGauss(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + + //both gaussian have the same dimension. + Assert::equal(norm1.getMu().rows(), norm2.getMu().rows(), "mean vectors do not have the same dimension"); + Assert::equal(norm1.getSigma().rows(), norm2.getSigma().rows(), "cov matrices do not have the same dimension"); + Assert::equal(norm1.getSigma().cols(), norm2.getSigma().cols(), "cov matrices do not have the same dimension"); + + //log + auto det1 = norm1.getSigma().determinant(); + auto det2 = norm2.getSigma().determinant(); + auto log1 = std::log(det1); + auto log2 = std::log(det2); + + //trace + Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols()); + toTrace = norm2.getSigmaInv() * norm1.getSigma(); + auto trace = toTrace.trace(); + + //transpose + Eigen::VectorXd toTranspose(norm1.getMu().rows()); + toTranspose = norm2.getMu() - norm1.getMu(); + auto transpose = toTranspose.transpose(); + + //rawdensity + auto rawDensity = transpose * norm2.getSigmaInv() * toTranspose; + auto dimension = norm1.getMu().rows(); + + //0.5 * ((log(det(cov_2)/det(cov_1)) + tr(cov_2^-1 cov_1) + (mu_2 - mu_1)^T * cov_2^-1 * (mu_2 - mu_1) - dimension) + double klb = 0.5 * ((log2 - log1) + trace + rawDensity - dimension); + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution symmetric*/ + static inline Scalar getMultivariateGaussSymmetric(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + return getMultivariateGauss(norm1, norm2) + getMultivariateGauss(norm2, norm1); + } + + + }; + +} + +#endif // KULLBACKLEIBLER_H diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h index 46f6475..a82de3e 100644 --- a/sensors/beacon/BeaconProbabilityFree.h +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -57,7 +57,9 @@ public: const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m); // NaN? -> AP not known to the model -> skip - if (modelRSSI != modelRSSI) {continue;} + if (modelRSSI != modelRSSI) { + continue; + } // the scan's RSSI diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 0ffc045..e850101 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -58,7 +58,7 @@ public: 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?"); + //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(); @@ -72,14 +72,15 @@ public: } // 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; } template double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const { - throw Exception("todo??"); + + return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs); } }; diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index 1ac2189..840d1dd 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -73,7 +73,7 @@ public: const Timestamp age = curTime - measurement.getTimestamp(); // sigma grows with measurement age - 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(); @@ -102,8 +102,7 @@ public: } // 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;} + //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); // as not every node has the same number of visible/matching APs // we MUST return something like the average probability diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp new file mode 100644 index 0000000..0a14f92 --- /dev/null +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -0,0 +1,214 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/KullbackLeibler.h" +#include "../../../math/Distributions.h" + +#include + + +TEST(KullbackLeibler, univariateGaussEQ) { + //if the distributions are equal, kld is 0 + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEmu) { + //bigger mu means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm3(0,1); + Distribution::Normal norm4(1,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEsigma) { + //bigger sigma means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm5(0,1); + Distribution::Normal norm6(0,3); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussRAND) { + + for(int i = 0; i < 20; i++){ + auto randMu1 = rand() % 100; + auto randMu2 = rand() % 100 + 100; + + auto randMu3 = rand() % 100; + auto randMu4 = rand() % 100 + 200; + + Distribution::Normal norm7(randMu1,1); + Distribution::Normal norm8(randMu2,1); + + Distribution::Normal norm9(randMu3,1); + Distribution::Normal norm10(randMu4,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGauss(norm8, norm7)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm8, norm7)); + } + +} + +TEST(KullbackLeibler, multivariateGaussEQ) { + + //eq + 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::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; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2)); + + +} + +TEST(KullbackLeibler, multivariateGaussGeMu) { + + //ge mu + 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] = 1.0; + mu4[1] = 3.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) = 1.0; + cov4(0,1) = 0.0; + cov4(1,0) = 0.0; + cov4(1,1) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " > " << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +TEST(KullbackLeibler, multivariateGaussGeCov) { + + //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] = 1.0; + mu4[1] = 1.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) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " >" << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +#endif From b99bb2e2264de3104519ae66b41d0d8d590dc03f Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 10 Mar 2017 15:12:27 +0100 Subject: [PATCH 15/43] added support for adding fingerprint-locations with meta information --- floorplan/v2/Floorplan.h | 13 ++++++++++++- floorplan/v2/FloorplanReader.h | 23 +++++++++++++++++++++++ floorplan/v2/FloorplanWriter.h | 13 +++++++++++++ 3 files changed, 48 insertions(+), 1 deletion(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index ea88b50..f58f749 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -179,6 +179,7 @@ namespace Floorplan { struct FloorObstacle; struct AccessPoint; struct Beacon; + struct FingerprintLocation; struct FloorRegion; struct UnderlayImage; struct POI; @@ -189,6 +190,7 @@ namespace Floorplan { using FloorObstacles = std::vector; using FloorAccessPoints = std::vector; using FloorBeacons = std::vector; + using FloorFingerprintLocations = std::vector; using FloorRegions = std::vector; using FloorUnderlays = std::vector; using FloorPOIs = std::vector; @@ -206,6 +208,7 @@ namespace Floorplan { FloorRegions regions; // all regions within the floor (rooms, ...) FloorAccessPoints accesspoints; FloorBeacons beacons; + FloorFingerprintLocations fpLocations; // potential fingerprint locations FloorUnderlays underlays; // underlay images (used for map-building) FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor @@ -222,7 +225,15 @@ namespace Floorplan { }; - + /** location for fingerprint measurements */ + struct FingerprintLocation : public HasMeta { + std::string name; + Point2 posOnFloor; + float heightAboveFloor = 0; + FingerprintLocation() {;} + FingerprintLocation(const std::string& name, const Point2 posOnFloor, const float heightAboveFloor) : name(name), posOnFloor(posOnFloor), heightAboveFloor(heightAboveFloor) {;} + Point3 getPosition(const Floor& floor) const {return Point3(posOnFloor.x, posOnFloor.y, floor.atHeight + heightAboveFloor);} + }; /** a POI located somewhere on a floor */ struct POI { diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 8522091..e9dee39 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -105,6 +105,7 @@ namespace Floorplan { if (std::string("obstacles") == n->Name()) {floor->obstacles = parseFloorObstacles(n);} if (std::string("accesspoints") == n->Name()) {floor->accesspoints = parseFloorAccessPoints(n);} if (std::string("beacons") == n->Name()) {floor->beacons = parseFloorBeacons(n);} + if (std::string("fingerprints") == n->Name()) {floor->fpLocations = parseFingerprintLocations(n);} if (std::string("regions") == n->Name()) {floor->regions = parseFloorRegions(n);} if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);} if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} @@ -291,6 +292,28 @@ namespace Floorplan { return b; } + /** parse s */ + static std::vector parseFingerprintLocations(const XMLElem* el) { + assertNode("fingerprints", el); + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); } + } + return vec; + } + + /** parse one fingerprint */ + static FingerprintLocation* parseFingerprintLocation(const XMLElem* n) { + assertNode("location", n); + FingerprintLocation* fpl = new FingerprintLocation(); + fpl->name = n->Attribute("name"); + fpl->posOnFloor.x = n->FloatAttribute("x"); + fpl->posOnFloor.y = n->FloatAttribute("y"); + fpl->heightAboveFloor = n->FloatAttribute("dz"); + const XMLElem* meta = n->FirstChildElement("meta"); + if (meta) {fpl->setMeta(parseMetaElement(meta));} + return fpl; + } static std::vector parseFloorRegions(const XMLElem* el) { std::vector vec; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 00c672c..51a3954 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -131,6 +131,7 @@ namespace Floorplan { } + /** add all sorts of POI to the floor */ static void addFloorPOI(XMLDoc& doc, XMLElem* floor, const Floor* mf) { @@ -180,6 +181,18 @@ namespace Floorplan { floor->InsertEndChild(beacons); + XMLElem* fingerprints = doc.NewElement("fingerprints"); + for (const FingerprintLocation* fpl : mf->fpLocations) { + XMLElem* efpl = doc.NewElement("location"); + efpl->SetAttribute("name", fpl->name.c_str()); + efpl->SetAttribute("x", fpl->posOnFloor.x); + efpl->SetAttribute("y", fpl->posOnFloor.y); + efpl->SetAttribute("dz", fpl->heightAboveFloor); + addMetaElement(doc, efpl, fpl->getMeta()); + fingerprints->InsertEndChild(efpl); + } + floor->InsertEndChild(fingerprints); + } static void addFloorOutline(XMLDoc& doc, XMLElem* floor, const Floor* mf) { From b18b1a795dbd5202fff43dfbce03924b2bb3299d Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 10 Mar 2017 15:35:44 +0100 Subject: [PATCH 16/43] merged with master --- floorplan/v2/Floorplan.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 8cc07ab..95359ba 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -184,7 +184,7 @@ namespace Floorplan { struct POI; struct Stair; struct Elevator; - struct GroundTruthPoint; + struct GroundTruthPoint; using FloorOutline = std::vector; using FloorObstacles = std::vector; @@ -195,7 +195,7 @@ namespace Floorplan { using FloorPOIs = std::vector; using FloorStairs = std::vector; using FloorElevators = std::vector; - using FloorGroundTruthPoints = std::vector; + using FloorGroundTruthPoints = std::vector; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -212,7 +212,7 @@ namespace Floorplan { FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor FloorElevators elevators; // all elevators within one floor - FloorGroundTruthPoints gtpoints; // all ground truth points within one floor + FloorGroundTruthPoints gtpoints; // all ground truth points within one floor //FloorKeyValue other; // other, free elements Floor() {;} @@ -237,14 +237,14 @@ namespace Floorplan { bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);} }; - /** a GroundTruthPoint located somewhere on a floor */ - struct GroundTruthPoint { - int id; //TODO: this value can be changed and isn't set incremental within the indoormap - Point2 pos; - GroundTruthPoint() : id(), pos() {;} - GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;} - bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} - }; + /** a GroundTruthPoint located somewhere on a floor */ + struct GroundTruthPoint { + int id; //TODO: this value can be changed and isn't set incremental within the indoormap + Point2 pos; + GroundTruthPoint() : id(), pos() {;} + GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;} + bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} + }; /** an AccessPoint located somewhere on a floor */ struct AccessPoint : public HasMeta { @@ -278,7 +278,7 @@ namespace Floorplan { Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} - Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground + Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground }; From 7ec5fef69743f0ea57f8c0ce9927f3b5c9b7a5e2 Mon Sep 17 00:00:00 2001 From: FrankE Date: Sun, 12 Mar 2017 16:47:29 +0100 Subject: [PATCH 17/43] added "outdoor" support to floorplan and grid-nodes --- floorplan/v2/Floorplan.h | 10 ++--- floorplan/v2/FloorplanReader.h | 1 + floorplan/v2/FloorplanWriter.h | 1 + grid/GridNode.h | 2 + grid/factory/v2/GridFactory.h | 68 +++++++++++++++++++++++----------- 5 files changed, 55 insertions(+), 27 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index d6d4ac0..cfdee4c 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -298,15 +298,13 @@ namespace Floorplan { OutlineMethod method; std::string name; Polygon2 poly; - FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly() {;} - FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly) : method(method), name(name), poly(poly) {;} - bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly);} + bool outdoor; // special marker + FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly(), outdoor(false) {;} + FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly, bool outdoor) : method(method), name(name), poly(poly), outdoor(outdoor) {;} + bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly) && (o.outdoor == outdoor);} }; - - - /** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */ struct FloorObstacle { Material material; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index b57c0b2..cb29bcc 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -412,6 +412,7 @@ namespace Floorplan { static FloorOutlinePolygon* parseFloorPolygon(const XMLElem* el) { FloorOutlinePolygon* poly = new FloorOutlinePolygon(); poly->name = el->Attribute("name"); + poly->outdoor = el->BoolAttribute("outdoor"); poly->method = parseOutlineMethod(el->Attribute("method")); poly->poly = parsePoly2(el); return poly; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index ed4b0e6..c8257d1 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -227,6 +227,7 @@ namespace Floorplan { XMLElem* polygon = doc.NewElement("polygon"); polygon->SetAttribute("name", poly->name.c_str()); polygon->SetAttribute("method", method.c_str()); + polygon->SetAttribute("outdoor", poly->outdoor); dst->InsertEndChild(polygon); for (Point2 p : poly->poly.points) { diff --git a/grid/GridNode.h b/grid/GridNode.h index d648b82..863ee43 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -52,6 +52,8 @@ public: static const uint8_t TYPE_ELEVATOR = 2; static const uint8_t TYPE_DOOR = 3; + static const uint8_t TYPE_OUTDOOR = 100; + public: /** ctor */ diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index d6beece..0303c51 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -114,21 +114,38 @@ public: return bb; } - bool isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) { + enum class PartOfOutline { + NO, + INDOOR, + OUTDOOR, + }; - bool add = false; + /** 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) { + + // assume the point is not part of the outline + PartOfOutline res = PartOfOutline::NO; + + // process every outline polygon for (Floorplan::FloorOutlinePolygon* poly : outline) { - if (poly->method != Floorplan::OutlineMethod::ADD) {continue;} HelperPoly pol(*poly); - if (pol.contains(Point2(x_cm, y_cm))) {add = true;} + if (pol.contains(Point2(x_cm, y_cm))) { + + // belongs to a "remove" polygon? -> directly ignore this location! + if (poly->method == Floorplan::OutlineMethod::REMOVE) { + return PartOfOutline::NO; + } + + // belongs to a "add" polygon? -> remember until all polygons were checked + // [might still belong to a "remove" polygon] + res = poly->outdoor ? PartOfOutline::OUTDOOR : PartOfOutline::INDOOR; + + } } - if (!add) {return false;} - for (Floorplan::FloorOutlinePolygon* poly : outline) { - if (poly->method != Floorplan::OutlineMethod::REMOVE) {continue;} - HelperPoly pol(*poly); - if (pol.contains(Point2(x_cm, y_cm))) {add = false;} // TODO - } - return add; + + // done + return res; + } @@ -155,7 +172,8 @@ public: for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) { // does the outline-polygon contain this position? - if (!isPartOfFloorOutline(x_cm, y_cm, floor->outline)) {continue;} + const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline); + if (part == PartOfOutline::NO) {continue;} // check intersection with the floorplan GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); @@ -164,12 +182,13 @@ public: bbox.grow(0.42345); if (intersects(bbox, floor)) {continue;} - // add to the grid + // add to the grid [once] T t(x_cm, y_cm, z_cm); - t.setType(getType(bbox, floor)); if (grid.hasNodeFor(t)) {continue;} + updateType(t, part, bbox, floor); grid.add(t); + // debug ++numNodes; @@ -487,23 +506,30 @@ private: } - /** does the bbox intersect with any of the floor's walls? */ - static inline int getType(const GridNodeBBox& bbox, const Floorplan::Floor* floor) { + /** adjust the given gridNode's type if needed [e.g. from "floor" to "door"] */ + static inline void updateType(T& t, const PartOfOutline part, const GridNodeBBox& bbox, const Floorplan::Floor* floor) { - // process each obstacle + // first, assume the type of the outline polygon + switch (part) { + case PartOfOutline::OUTDOOR: t.setType(GridNode::TYPE_OUTDOOR); break; + case PartOfOutline::INDOOR: t.setType(GridNode::TYPE_FLOOR); break; + default: throw Exception("should not happen"); + } + + // hereafter, process each obstacle and mark doors for (Floorplan::FloorObstacle* fo : floor->obstacles) { if (dynamic_cast(fo)) { const Floorplan::FloorObstacleDoor* door = (Floorplan::FloorObstacleDoor*) fo; const Line2 l2(door->from*100, door->to*100); - if (bbox.intersects(l2)) {return GridNode::TYPE_DOOR;} - + if (bbox.intersects(l2)) { + t.setType(GridNode::TYPE_DOOR); + return; // done + } } } - return GridNode::TYPE_FLOOR; - } From d065015f7d1c430f09efb5c53db77b21840192c3 Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 13 Mar 2017 12:55:31 +0100 Subject: [PATCH 18/43] added sampling function for NormalN and approximation of normalN using samples + testcases for both --- main.cpp | 2 +- math/distribution/NormalN.h | 39 +++++++++- .../beacon/model/BeaconModelLogDistCeiling.h | 2 +- tests/math/distribution/TestNormalN.cpp | 77 +++++++++++++++++++ 4 files changed, 114 insertions(+), 6 deletions(-) create mode 100644 tests/math/distribution/TestNormalN.cpp diff --git a/main.cpp b/main.cpp index b103b91..07e9163 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; + ::testing::GTEST_FLAG(filter) = "*NormalN*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index c628828..ad2f04e 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -1,8 +1,14 @@ #ifndef NORMALN_H #define NORMALN_H +#include +#include + #include +#include "../../Assertions.h" +#include "../Random.h" + namespace Distribution { class NormalDistributionN { @@ -15,19 +21,30 @@ namespace Distribution { const double _a; const Eigen::MatrixXd _sigmaInv; + const Eigen::SelfAdjointEigenSolver eigenSolver; + Eigen::MatrixXd transform; //can i make this const? + + RandomGenerator gen; + std::normal_distribution<> dist; + public: /** ctor */ NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : - mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()), eigenSolver(sigma) { + transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal(); } - /** get probability for the given value */ double getProbability(const Eigen::VectorXd val) const { - const double b = ((val-mu).transpose() * _sigmaInv * (val-mu)); - return _a * std::exp(-b/2.0); + const double b = ((val-this->mu).transpose() * this->_sigmaInv * (val-this->mu)); + return this->_a * std::exp(-b/2.0); + } + + /** get a randomly drawn sample from the given normalN distribution*/ + Eigen::VectorXd draw() { + return this->mu + this->transform * Eigen::VectorXd{ this->mu.size() }.unaryExpr([&](double x) { return dist(gen); }); } /** get the mean vector */ @@ -44,6 +61,20 @@ namespace Distribution { return this->_sigmaInv; } + /** return a NormalN based on given data */ + static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) { + + const int numElements = data.rows(); + Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the distribution!"); + Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!"); + + const Eigen::VectorXd mean = data.colwise().mean(); + const Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); + const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); + + return NormalDistributionN(mean, cov); + } + }; } diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h index 8e794a6..649d19e 100644 --- a/sensors/beacon/model/BeaconModelLogDistCeiling.h +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -94,7 +94,7 @@ public: Assert::isBetween(params.txp, -90.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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); + //Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); // add beacons.insert( std::pair(beacon, params) ); diff --git a/tests/math/distribution/TestNormalN.cpp b/tests/math/distribution/TestNormalN.cpp new file mode 100644 index 0000000..39f79e6 --- /dev/null +++ b/tests/math/distribution/TestNormalN.cpp @@ -0,0 +1,77 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/KullbackLeibler.h" +#include "../../../math/Distributions.h" + +#include + +#include +#include +#include +#include + + +TEST(NormalN, multivariateGaussSampleData) { + + Eigen::VectorXd mu(2); + mu[0] = 0.0; + mu[1] = 0.0; + + Eigen::MatrixXd cov(2,2); + cov(0,0) = 1.0; + cov(0,1) = 0.0; + cov(1,0) = 0.0; + cov(1,1) = 1.0; + + Distribution::NormalDistributionN normTest(mu, cov); + + // draw + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementPoints pParticles; + + gp << "set terminal qt size 1200,1200 \n"; + gp << "set xrange [-5:5] \n set yrange [-5:5] \n"; + + // gen + std::vector vals; + for(int i = 0; i < 100000; ++i){ + Eigen::VectorXd vec = normTest.draw(); + vals.push_back(vec); + + K::GnuplotPoint2 pos(vec[0], vec[1]); + pParticles.add(pos); + } + + plot.add(&pParticles); + + gp.draw(plot); + gp.flush(); + + sleep(5); + + //create matrix out of the vector + Eigen::MatrixXd m(vals.size(), vals[0].rows()); + + for(int i = 0; i < vals.size(); ++i){ + m(i,0) = vals[i][0]; + m(i,1) = vals[i][1]; + } + + Distribution::NormalDistributionN norm = Distribution::NormalDistributionN::getNormalNFromSamples(m); + + //get distance between original and approximated mu and sigma + Eigen::MatrixXd diffM = cov - norm.getSigma(); + double distM = diffM.norm(); + + Eigen::VectorXd diffV = mu - norm.getMu(); + double distV = diffV.norm(); + + ASSERT_NEAR(0.0, distM, 0.01); + ASSERT_NEAR(0.0, distV, 0.01); + +} + +#endif + From 06e0e0a5aae6f23afbfc9b903344e7c37f0c3e97 Mon Sep 17 00:00:00 2001 From: kazu Date: Mon, 20 Mar 2017 11:19:57 +0100 Subject: [PATCH 19/43] fixed some potential issues with MAC addresses added corresponding test-cases switched to newer version of tinyxml due to some issues adjusted affected code-parts accordingly for better re-use, moved ceiling-calculation to a new class some minor fixes new helper methods worked on wifi-opt --- floorplan/v2/FloorplanCeilings.h | 112 ++++ floorplan/v2/FloorplanHelper.h | 17 + floorplan/v2/FloorplanReader.h | 27 +- geo/EarthPos.h | 2 +- geo/Point2.h | 1 + lib/tinyxml/tinyxml2.cpp | 519 ++++++++++++------ lib/tinyxml/tinyxml2.h | 359 ++++++++---- main.cpp | 2 +- sensors/MACAddress.h | 26 +- sensors/radio/VAPGrouper.h | 7 + sensors/radio/model/WiFiModelLogDistCeiling.h | 75 +-- sensors/radio/setup/WiFiFingerprint.h | 10 +- sensors/radio/setup/WiFiFingerprints.h | 125 +++++ sensors/radio/setup/WiFiOptimizer.h | 262 ++------- .../radio/setup/WiFiOptimizerLogDistCeiling.h | 341 ++++++++++++ sensors/radio/setup/WiFiOptimizerStructs.h | 55 ++ tests/floorplan/TestFloorplanCeilings.cpp | 85 +++ tests/sensors/TestMAC.cpp | 48 ++ .../radio/TestLogDistanceCeilingModel.cpp | 75 --- tests/sensors/radio/TestWiFiOptimizer.cpp | 14 +- 20 files changed, 1486 insertions(+), 676 deletions(-) create mode 100644 floorplan/v2/FloorplanCeilings.h create mode 100644 sensors/radio/setup/WiFiFingerprints.h create mode 100644 sensors/radio/setup/WiFiOptimizerLogDistCeiling.h create mode 100644 sensors/radio/setup/WiFiOptimizerStructs.h create mode 100644 tests/floorplan/TestFloorplanCeilings.cpp diff --git a/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h new file mode 100644 index 0000000..887ed8e --- /dev/null +++ b/floorplan/v2/FloorplanCeilings.h @@ -0,0 +1,112 @@ +#ifndef FLOORPLANCEILINGS_H +#define FLOORPLANCEILINGS_H + +#include "Floorplan.h" + +namespace Floorplan { + + /** + * helper-class for floorplan ceilings + * e.g. to determine the number of ceilings between two given positions + */ + class Ceilings { + + private: + + /** position (height) of all ceilings (in meter) */ + std::vector ceilingsAtHeight_m; + + public: + + /** empty ctor */ + Ceilings() { + ; + } + + /** ctor with the map to work with */ + Ceilings(const IndoorMap* map) { + + // sanity checks + Assert::isTrue(map->floors.size() >= 1, "map has no floors?!"); + + // get position of all ceilings + for (Floorplan::Floor* f : map->floors) { + + const float h1 = f->atHeight; + const float h2 = f->atHeight + f->height; + + if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h1) == ceilingsAtHeight_m.end()) { + ceilingsAtHeight_m.push_back(h1); + } + if (std::find(ceilingsAtHeight_m.begin(), ceilingsAtHeight_m.end(), h2) == ceilingsAtHeight_m.end()) { + ceilingsAtHeight_m.push_back(h2); + } + + } + + } + + /** get the number of ceilings between z1 and z2 */ + float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) 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); + + 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 { + + // sanity checks + Assert::isNot0(ceilingsAtHeight_m.size(), "no ceilings available for testing! incorrect map?"); + + // find min and max height given the to-be-compared points + 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 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! \ + consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) " + ); + } + #endif + + int cnt = 0; + for (const float z : ceilingsAtHeight_m) { + if (zMin < z && zMax > z) {++cnt;} + } + + return cnt; + + } + + }; + +} + +#endif // FLOORPLANCEILINGS_H diff --git a/floorplan/v2/FloorplanHelper.h b/floorplan/v2/FloorplanHelper.h index a62daa9..473202c 100644 --- a/floorplan/v2/FloorplanHelper.h +++ b/floorplan/v2/FloorplanHelper.h @@ -7,11 +7,28 @@ #include "Floorplan.h" +#include "../../sensors/MACAddress.h" + /** * helper methods for the floorplan */ class FloorplanHelper { +public: + + + /** get the AP for the given MAC [if available] */ + static std::pair getAP(const Floorplan::IndoorMap* map, const MACAddress& mac) { + for (Floorplan::Floor* f : map->floors) { + for (Floorplan::AccessPoint* ap : f->accesspoints) { + if (MACAddress(ap->mac) == mac) { + return std::make_pair(ap, f); + } + } + } + return std::make_pair(nullptr, nullptr); + } + public: /** align all floorplan values to the given grid size. needed for the grid factory */ diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index cb29bcc..5d19af6 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -35,11 +35,12 @@ namespace Floorplan { if (res != tinyxml2::XMLError::XML_SUCCESS) { throw Exception( std::string() + "error while loading XML " + file + "\n" + - ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + - ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) ); } - return parse(doc); + IndoorMap* map = parse(doc); + return map; } /** read an IndoorMap from the given XMl-string */ @@ -51,16 +52,17 @@ namespace Floorplan { if (res != tinyxml2::XMLError::XML_SUCCESS) { throw Exception( std::string() + "error while parsing XML\n" + - ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + - ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) ); } - return parse(doc); + IndoorMap* map = parse(doc); + return map; } private: - #define FOREACH_NODE(out, in) for( const XMLElem* out = (XMLElem*) in->FirstChild(); out; out = (XMLElem*) out->NextSibling() ) + #define FOREACH_NODE(out, in) for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() ) static void assertNode(const std::string& node, const XMLElem* el) { std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'"; @@ -69,7 +71,7 @@ namespace Floorplan { /** parse the complete document */ static IndoorMap* parse(tinyxml2::XMLDocument& doc) { - return parseMap((XMLElem*)doc.FirstChild()); + return parseMap(doc.FirstChildElement()); } /** parse the node */ @@ -266,7 +268,9 @@ namespace Floorplan { const XMLElem* sub = n->FirstChildElement(); while(sub) { // abc - elem->add(sub->Attribute("key"), sub->FirstChild()->Value()); + const std::string key = sub->Attribute("key"); + const std::string val = sub->GetText(); + elem->add(key, val); sub = sub->NextSiblingElement(); } return elem; @@ -402,7 +406,7 @@ namespace Floorplan { FloorOutline outline; FOREACH_NODE(n, el) { if (std::string("polygon") == n->Name()) { - outline.push_back(parseFloorPolygon(n)); // TODO + outline.push_back(parseFloorPolygon(n)); } } return outline; @@ -427,6 +431,9 @@ namespace Floorplan { poly.points.push_back(p2); } } + if (poly.points.size() < 4 || poly.points.size() > 1024) { + throw Exception("detected invalid outline-polygon during XML parsing"); + } return poly; } diff --git a/geo/EarthPos.h b/geo/EarthPos.h index bb6ccc0..d038fc1 100644 --- a/geo/EarthPos.h +++ b/geo/EarthPos.h @@ -12,7 +12,7 @@ struct EarthPos { float height; /** ctor with values */ - EarthPos(const double lat, const double lon, const float height) : lon(lon), lat(lat), height(height) { + EarthPos(const double lat, const double lon, const float height) : lat(lat), lon(lon), height(height) { ; } diff --git a/geo/Point2.h b/geo/Point2.h index 2d58178..3b19fd7 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -2,6 +2,7 @@ #define POINT2_H #include +#include /** * 2D Point diff --git a/lib/tinyxml/tinyxml2.cpp b/lib/tinyxml/tinyxml2.cpp index df49d0d..87c1a99 100644 --- a/lib/tinyxml/tinyxml2.cpp +++ b/lib/tinyxml/tinyxml2.cpp @@ -24,7 +24,7 @@ distribution. #include "tinyxml2.h" #include // yes, this one new style header, is in the Android SDK. -#if defined(ANDROID_NDK) || defined(__QNXNTO__) +#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__) # include # include #else @@ -149,6 +149,7 @@ void StrPair::TransferTo( StrPair* other ) // This in effect implements the assignment operator by "moving" // ownership (as in auto_ptr). + TIXMLASSERT( other != 0 ); TIXMLASSERT( other->_flags == 0 ); TIXMLASSERT( other->_start == 0 ); TIXMLASSERT( other->_end == 0 ); @@ -188,9 +189,11 @@ void StrPair::SetStr( const char* str, int flags ) } -char* StrPair::ParseText( char* p, const char* endTag, int strFlags ) +char* StrPair::ParseText( char* p, const char* endTag, int strFlags, int* curLineNumPtr ) { + TIXMLASSERT( p ); TIXMLASSERT( endTag && *endTag ); + TIXMLASSERT(curLineNumPtr); char* start = p; char endChar = *endTag; @@ -201,8 +204,11 @@ char* StrPair::ParseText( char* p, const char* endTag, int strFlags ) if ( *p == endChar && strncmp( p, endTag, length ) == 0 ) { Set( start, p, strFlags ); return p + length; + } else if (*p == '\n') { + ++(*curLineNumPtr); } ++p; + TIXMLASSERT( p ); } return 0; } @@ -233,15 +239,15 @@ void StrPair::CollapseWhitespace() // Adjusting _start would cause undefined behavior on delete[] TIXMLASSERT( ( _flags & NEEDS_DELETE ) == 0 ); // Trim leading space. - _start = XMLUtil::SkipWhiteSpace( _start ); + _start = XMLUtil::SkipWhiteSpace( _start, 0 ); if ( *_start ) { - char* p = _start; // the read pointer + const char* p = _start; // the read pointer char* q = _start; // the write pointer while( *p ) { if ( XMLUtil::IsWhiteSpace( *p )) { - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, 0 ); if ( *p == 0 ) { break; // don't write to q; this trims the trailing space. } @@ -266,7 +272,7 @@ const char* StrPair::GetStr() _flags ^= NEEDS_FLUSH; if ( _flags ) { - char* p = _start; // the read pointer + const char* p = _start; // the read pointer char* q = _start; // the write pointer while( p < _end ) { @@ -280,7 +286,8 @@ const char* StrPair::GetStr() else { ++p; } - *q++ = LF; + *q = LF; + ++q; } else if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == LF ) { if ( *(p+1) == CR ) { @@ -289,7 +296,8 @@ const char* StrPair::GetStr() else { ++p; } - *q++ = LF; + *q = LF; + ++q; } else if ( (_flags & NEEDS_ENTITY_PROCESSING) && *p == '&' ) { // Entities handled by tinyXML2: @@ -360,6 +368,19 @@ const char* StrPair::GetStr() // --------- XMLUtil ----------- // +const char* XMLUtil::writeBoolTrue = "true"; +const char* XMLUtil::writeBoolFalse = "false"; + +void XMLUtil::SetBoolSerialization(const char* writeTrue, const char* writeFalse) +{ + static const char* defTrue = "true"; + static const char* defFalse = "false"; + + writeBoolTrue = (writeTrue) ? writeTrue : defTrue; + writeBoolFalse = (writeFalse) ? writeFalse : defFalse; +} + + const char* XMLUtil::ReadBOM( const char* p, bool* bom ) { TIXMLASSERT( p ); @@ -471,7 +492,7 @@ const char* XMLUtil::GetCharacterRef( const char* p, char* value, int* length ) else { return 0; } - TIXMLASSERT( digit >= 0 && digit < 16); + TIXMLASSERT( digit < 16 ); TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); const unsigned int digitScaled = mult * digit; TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); @@ -501,7 +522,7 @@ const char* XMLUtil::GetCharacterRef( const char* p, char* value, int* length ) while ( *q != '#' ) { if ( *q >= '0' && *q <= '9' ) { const unsigned int digit = *q - '0'; - TIXMLASSERT( digit >= 0 && digit < 10); + TIXMLASSERT( digit < 10 ); TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); const unsigned int digitScaled = mult * digit; TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); @@ -537,7 +558,7 @@ void XMLUtil::ToStr( unsigned v, char* buffer, int bufferSize ) void XMLUtil::ToStr( bool v, char* buffer, int bufferSize ) { - TIXML_SNPRINTF( buffer, bufferSize, "%d", v ? 1 : 0 ); + TIXML_SNPRINTF( buffer, bufferSize, "%s", v ? writeBoolTrue : writeBoolFalse); } /* @@ -556,6 +577,13 @@ void XMLUtil::ToStr( double v, char* buffer, int bufferSize ) } +void XMLUtil::ToStr(int64_t v, char* buffer, int bufferSize) +{ + // horrible syntax trick to make the compiler happy about %lld + TIXML_SNPRINTF(buffer, bufferSize, "%lld", (long long)v); +} + + bool XMLUtil::ToInt( const char* str, int* value ) { if ( TIXML_SSCANF( str, "%d", value ) == 1 ) { @@ -599,6 +627,7 @@ bool XMLUtil::ToFloat( const char* str, float* value ) return false; } + bool XMLUtil::ToDouble( const char* str, double* value ) { if ( TIXML_SSCANF( str, "%lf", value ) == 1 ) { @@ -608,12 +637,24 @@ bool XMLUtil::ToDouble( const char* str, double* value ) } +bool XMLUtil::ToInt64(const char* str, int64_t* value) +{ + long long v = 0; // horrible syntax trick to make the compiler happy about %lld + if (TIXML_SSCANF(str, "%lld", &v) == 1) { + *value = (int64_t)v; + return true; + } + return false; +} + + char* XMLDocument::Identify( char* p, XMLNode** node ) { TIXMLASSERT( node ); TIXMLASSERT( p ); char* const start = p; - p = XMLUtil::SkipWhiteSpace( p ); + int const startLine = _parseCurLineNum; + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); if( !*p ) { *node = 0; TIXMLASSERT( p ); @@ -637,42 +678,37 @@ char* XMLDocument::Identify( char* p, XMLNode** node ) TIXMLASSERT( sizeof( XMLComment ) == sizeof( XMLDeclaration ) ); // use same memory pool XMLNode* returnNode = 0; if ( XMLUtil::StringEqual( p, xmlHeader, xmlHeaderLen ) ) { - TIXMLASSERT( sizeof( XMLDeclaration ) == _commentPool.ItemSize() ); - returnNode = new (_commentPool.Alloc()) XMLDeclaration( this ); - returnNode->_memPool = &_commentPool; + returnNode = CreateUnlinkedNode( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; p += xmlHeaderLen; } else if ( XMLUtil::StringEqual( p, commentHeader, commentHeaderLen ) ) { - TIXMLASSERT( sizeof( XMLComment ) == _commentPool.ItemSize() ); - returnNode = new (_commentPool.Alloc()) XMLComment( this ); - returnNode->_memPool = &_commentPool; + returnNode = CreateUnlinkedNode( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; p += commentHeaderLen; } else if ( XMLUtil::StringEqual( p, cdataHeader, cdataHeaderLen ) ) { - TIXMLASSERT( sizeof( XMLText ) == _textPool.ItemSize() ); - XMLText* text = new (_textPool.Alloc()) XMLText( this ); + XMLText* text = CreateUnlinkedNode( _textPool ); returnNode = text; - returnNode->_memPool = &_textPool; + returnNode->_parseLineNum = _parseCurLineNum; p += cdataHeaderLen; text->SetCData( true ); } else if ( XMLUtil::StringEqual( p, dtdHeader, dtdHeaderLen ) ) { - TIXMLASSERT( sizeof( XMLUnknown ) == _commentPool.ItemSize() ); - returnNode = new (_commentPool.Alloc()) XMLUnknown( this ); - returnNode->_memPool = &_commentPool; + returnNode = CreateUnlinkedNode( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; p += dtdHeaderLen; } else if ( XMLUtil::StringEqual( p, elementHeader, elementHeaderLen ) ) { - TIXMLASSERT( sizeof( XMLElement ) == _elementPool.ItemSize() ); - returnNode = new (_elementPool.Alloc()) XMLElement( this ); - returnNode->_memPool = &_elementPool; + returnNode = CreateUnlinkedNode( _elementPool ); + returnNode->_parseLineNum = _parseCurLineNum; p += elementHeaderLen; } else { - TIXMLASSERT( sizeof( XMLText ) == _textPool.ItemSize() ); - returnNode = new (_textPool.Alloc()) XMLText( this ); - returnNode->_memPool = &_textPool; + returnNode = CreateUnlinkedNode( _textPool ); + returnNode->_parseLineNum = _parseCurLineNum; // Report line of first non-whitespace character p = start; // Back it up, all the text counts. + _parseCurLineNum = startLine; } TIXMLASSERT( returnNode ); @@ -701,8 +737,10 @@ bool XMLDocument::Accept( XMLVisitor* visitor ) const XMLNode::XMLNode( XMLDocument* doc ) : _document( doc ), _parent( 0 ), + _parseLineNum( 0 ), _firstChild( 0 ), _lastChild( 0 ), _prev( 0 ), _next( 0 ), + _userData( 0 ), _memPool( 0 ) { } @@ -718,7 +756,7 @@ XMLNode::~XMLNode() const char* XMLNode::Value() const { - // Catch an edge case: XMLDocuments don't have a a Value. Carefully return nullptr. + // Edge case: XMLDocuments don't have a Value. Return null. if ( this->ToDocument() ) return 0; return _value.GetStr(); @@ -739,11 +777,7 @@ void XMLNode::DeleteChildren() { while( _firstChild ) { TIXMLASSERT( _lastChild ); - TIXMLASSERT( _firstChild->_document == _document ); - XMLNode* node = _firstChild; - Unlink( node ); - - DeleteNode( node ); + DeleteChild( _firstChild ); } _firstChild = _lastChild = 0; } @@ -876,11 +910,9 @@ XMLNode* XMLNode::InsertAfterChild( XMLNode* afterThis, XMLNode* addThis ) const XMLElement* XMLNode::FirstChildElement( const char* name ) const { for( const XMLNode* node = _firstChild; node; node = node->_next ) { - const XMLElement* element = node->ToElement(); + const XMLElement* element = node->ToElementWithName( name ); if ( element ) { - if ( !name || XMLUtil::StringEqual( element->Name(), name ) ) { - return element; - } + return element; } } return 0; @@ -890,11 +922,9 @@ const XMLElement* XMLNode::FirstChildElement( const char* name ) const const XMLElement* XMLNode::LastChildElement( const char* name ) const { for( const XMLNode* node = _lastChild; node; node = node->_prev ) { - const XMLElement* element = node->ToElement(); + const XMLElement* element = node->ToElementWithName( name ); if ( element ) { - if ( !name || XMLUtil::StringEqual( element->Name(), name ) ) { - return element; - } + return element; } } return 0; @@ -904,9 +934,8 @@ const XMLElement* XMLNode::LastChildElement( const char* name ) const const XMLElement* XMLNode::NextSiblingElement( const char* name ) const { for( const XMLNode* node = _next; node; node = node->_next ) { - const XMLElement* element = node->ToElement(); - if ( element - && (!name || XMLUtil::StringEqual( name, element->Name() ))) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { return element; } } @@ -917,9 +946,8 @@ const XMLElement* XMLNode::NextSiblingElement( const char* name ) const const XMLElement* XMLNode::PreviousSiblingElement( const char* name ) const { for( const XMLNode* node = _prev; node; node = node->_prev ) { - const XMLElement* element = node->ToElement(); - if ( element - && (!name || XMLUtil::StringEqual( name, element->Name() ))) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { return element; } } @@ -927,7 +955,7 @@ const XMLElement* XMLNode::PreviousSiblingElement( const char* name ) const } -char* XMLNode::ParseDeep( char* p, StrPair* parentEnd ) +char* XMLNode::ParseDeep( char* p, StrPair* parentEnd, int* curLineNumPtr ) { // This is a recursive method, but thinking about it "at the current level" // it is a pretty simple flat list: @@ -950,29 +978,42 @@ char* XMLNode::ParseDeep( char* p, StrPair* parentEnd ) XMLNode* node = 0; p = _document->Identify( p, &node ); + TIXMLASSERT( p ); if ( node == 0 ) { break; } + int initialLineNum = node->_parseLineNum; + StrPair endTag; - p = node->ParseDeep( p, &endTag ); + p = node->ParseDeep( p, &endTag, curLineNumPtr ); if ( !p ) { DeleteNode( node ); if ( !_document->Error() ) { - _document->SetError( XML_ERROR_PARSING, 0, 0 ); + _document->SetError( XML_ERROR_PARSING, 0, 0, initialLineNum); } break; } XMLDeclaration* decl = node->ToDeclaration(); if ( decl ) { - // A declaration can only be the first child of a document. - // Set error, if document already has children. - if ( !_document->NoChildren() ) { - _document->SetError( XML_ERROR_PARSING_DECLARATION, decl->Value(), 0); - DeleteNode( decl ); + // Declarations are only allowed at document level + bool wellLocated = ( ToDocument() != 0 ); + if ( wellLocated ) { + // Multiple declarations are allowed but all declarations + // must occur before anything else + for ( const XMLNode* existingNode = _document->FirstChild(); existingNode; existingNode = existingNode->NextSibling() ) { + if ( !existingNode->ToDeclaration() ) { + wellLocated = false; break; + } } + } + if ( !wellLocated ) { + _document->SetError( XML_ERROR_PARSING_DECLARATION, decl->Value(), 0, initialLineNum); + DeleteNode( node ); + break; + } } XMLElement* ele = node->ToElement(); @@ -1004,7 +1045,7 @@ char* XMLNode::ParseDeep( char* p, StrPair* parentEnd ) } } if ( mismatch ) { - _document->SetError( XML_ERROR_MISMATCHED_ELEMENT, ele->Name(), 0 ); + _document->SetError( XML_ERROR_MISMATCHED_ELEMENT, ele->Name(), 0, initialLineNum); DeleteNode( node ); break; } @@ -1035,14 +1076,29 @@ void XMLNode::InsertChildPreamble( XMLNode* insertThis ) const insertThis->_memPool->SetTracked(); } +const XMLElement* XMLNode::ToElementWithName( const char* name ) const +{ + const XMLElement* element = this->ToElement(); + if ( element == 0 ) { + return 0; + } + if ( name == 0 ) { + return element; + } + if ( XMLUtil::StringEqual( element->Name(), name ) ) { + return element; + } + return 0; +} + // --------- XMLText ---------- // -char* XMLText::ParseDeep( char* p, StrPair* ) +char* XMLText::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) { const char* start = p; if ( this->CData() ) { - p = _value.ParseText( p, "]]>", StrPair::NEEDS_NEWLINE_NORMALIZATION ); + p = _value.ParseText( p, "]]>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); if ( !p ) { - _document->SetError( XML_ERROR_PARSING_CDATA, start, 0 ); + _document->SetError( XML_ERROR_PARSING_CDATA, start, 0, _parseLineNum ); } return p; } @@ -1052,12 +1108,12 @@ char* XMLText::ParseDeep( char* p, StrPair* ) flags |= StrPair::NEEDS_WHITESPACE_COLLAPSING; } - p = _value.ParseText( p, "<", flags ); + p = _value.ParseText( p, "<", flags, curLineNumPtr ); if ( p && *p ) { return p-1; } if ( !p ) { - _document->SetError( XML_ERROR_PARSING_TEXT, start, 0 ); + _document->SetError( XML_ERROR_PARSING_TEXT, start, 0, _parseLineNum ); } } return 0; @@ -1101,13 +1157,13 @@ XMLComment::~XMLComment() } -char* XMLComment::ParseDeep( char* p, StrPair* ) +char* XMLComment::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) { // Comment parses as text. const char* start = p; - p = _value.ParseText( p, "-->", StrPair::COMMENT ); + p = _value.ParseText( p, "-->", StrPair::COMMENT, curLineNumPtr ); if ( p == 0 ) { - _document->SetError( XML_ERROR_PARSING_COMMENT, start, 0 ); + _document->SetError( XML_ERROR_PARSING_COMMENT, start, 0, _parseLineNum ); } return p; } @@ -1151,13 +1207,13 @@ XMLDeclaration::~XMLDeclaration() } -char* XMLDeclaration::ParseDeep( char* p, StrPair* ) +char* XMLDeclaration::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) { // Declaration parses as text. const char* start = p; - p = _value.ParseText( p, "?>", StrPair::NEEDS_NEWLINE_NORMALIZATION ); + p = _value.ParseText( p, "?>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); if ( p == 0 ) { - _document->SetError( XML_ERROR_PARSING_DECLARATION, start, 0 ); + _document->SetError( XML_ERROR_PARSING_DECLARATION, start, 0, _parseLineNum ); } return p; } @@ -1200,14 +1256,14 @@ XMLUnknown::~XMLUnknown() } -char* XMLUnknown::ParseDeep( char* p, StrPair* ) +char* XMLUnknown::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) { // Unknown parses as text. const char* start = p; - p = _value.ParseText( p, ">", StrPair::NEEDS_NEWLINE_NORMALIZATION ); + p = _value.ParseText( p, ">", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); if ( !p ) { - _document->SetError( XML_ERROR_PARSING_UNKNOWN, start, 0 ); + _document->SetError( XML_ERROR_PARSING_UNKNOWN, start, 0, _parseLineNum ); } return p; } @@ -1249,7 +1305,7 @@ const char* XMLAttribute::Value() const return _value.GetStr(); } -char* XMLAttribute::ParseDeep( char* p, bool processEntities ) +char* XMLAttribute::ParseDeep( char* p, bool processEntities, int* curLineNumPtr ) { // Parse using the name rules: bug fix, was using ParseText before p = _name.ParseName( p ); @@ -1258,13 +1314,13 @@ char* XMLAttribute::ParseDeep( char* p, bool processEntities ) } // Skip white space before = - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); if ( *p != '=' ) { return 0; } ++p; // move up to opening quote - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); if ( *p != '\"' && *p != '\'' ) { return 0; } @@ -1272,7 +1328,7 @@ char* XMLAttribute::ParseDeep( char* p, bool processEntities ) char endTag[2] = { *p, 0 }; ++p; // move past opening quote - p = _value.ParseText( p, endTag, processEntities ? StrPair::ATTRIBUTE_VALUE : StrPair::ATTRIBUTE_VALUE_LEAVE_ENTITIES ); + p = _value.ParseText( p, endTag, processEntities ? StrPair::ATTRIBUTE_VALUE : StrPair::ATTRIBUTE_VALUE_LEAVE_ENTITIES, curLineNumPtr ); return p; } @@ -1286,7 +1342,7 @@ void XMLAttribute::SetName( const char* n ) XMLError XMLAttribute::QueryIntValue( int* value ) const { if ( XMLUtil::ToInt( Value(), value )) { - return XML_NO_ERROR; + return XML_SUCCESS; } return XML_WRONG_ATTRIBUTE_TYPE; } @@ -1295,16 +1351,25 @@ XMLError XMLAttribute::QueryIntValue( int* value ) const XMLError XMLAttribute::QueryUnsignedValue( unsigned int* value ) const { if ( XMLUtil::ToUnsigned( Value(), value )) { - return XML_NO_ERROR; + return XML_SUCCESS; } return XML_WRONG_ATTRIBUTE_TYPE; } +XMLError XMLAttribute::QueryInt64Value(int64_t* value) const +{ + if (XMLUtil::ToInt64(Value(), value)) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + XMLError XMLAttribute::QueryBoolValue( bool* value ) const { if ( XMLUtil::ToBool( Value(), value )) { - return XML_NO_ERROR; + return XML_SUCCESS; } return XML_WRONG_ATTRIBUTE_TYPE; } @@ -1313,7 +1378,7 @@ XMLError XMLAttribute::QueryBoolValue( bool* value ) const XMLError XMLAttribute::QueryFloatValue( float* value ) const { if ( XMLUtil::ToFloat( Value(), value )) { - return XML_NO_ERROR; + return XML_SUCCESS; } return XML_WRONG_ATTRIBUTE_TYPE; } @@ -1322,7 +1387,7 @@ XMLError XMLAttribute::QueryFloatValue( float* value ) const XMLError XMLAttribute::QueryDoubleValue( double* value ) const { if ( XMLUtil::ToDouble( Value(), value )) { - return XML_NO_ERROR; + return XML_SUCCESS; } return XML_WRONG_ATTRIBUTE_TYPE; } @@ -1350,6 +1415,15 @@ void XMLAttribute::SetAttribute( unsigned v ) } +void XMLAttribute::SetAttribute(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + _value.SetStr(buf); +} + + + void XMLAttribute::SetAttribute( bool v ) { char buf[BUF_SIZE]; @@ -1413,6 +1487,47 @@ const char* XMLElement::Attribute( const char* name, const char* value ) const return 0; } +int XMLElement::IntAttribute(const char* name, int defaultValue) const +{ + int i = defaultValue; + QueryIntAttribute(name, &i); + return i; +} + +unsigned XMLElement::UnsignedAttribute(const char* name, unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedAttribute(name, &i); + return i; +} + +int64_t XMLElement::Int64Attribute(const char* name, int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Attribute(name, &i); + return i; +} + +bool XMLElement::BoolAttribute(const char* name, bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolAttribute(name, &b); + return b; +} + +double XMLElement::DoubleAttribute(const char* name, double defaultValue) const +{ + double d = defaultValue; + QueryDoubleAttribute(name, &d); + return d; +} + +float XMLElement::FloatAttribute(const char* name, float defaultValue) const +{ + float f = defaultValue; + QueryFloatAttribute(name, &f); + return f; +} const char* XMLElement::GetText() const { @@ -1450,7 +1565,15 @@ void XMLElement::SetText( unsigned v ) } -void XMLElement::SetText( bool v ) +void XMLElement::SetText(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + SetText(buf); +} + + +void XMLElement::SetText( bool v ) { char buf[BUF_SIZE]; XMLUtil::ToStr( v, buf, BUF_SIZE ); @@ -1500,6 +1623,19 @@ XMLError XMLElement::QueryUnsignedText( unsigned* uval ) const } +XMLError XMLElement::QueryInt64Text(int64_t* ival) const +{ + if (FirstChild() && FirstChild()->ToText()) { + const char* t = FirstChild()->Value(); + if (XMLUtil::ToInt64(t, ival)) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + XMLError XMLElement::QueryBoolText( bool* bval ) const { if ( FirstChild() && FirstChild()->ToText() ) { @@ -1538,6 +1674,47 @@ XMLError XMLElement::QueryFloatText( float* fval ) const return XML_NO_TEXT_NODE; } +int XMLElement::IntText(int defaultValue) const +{ + int i = defaultValue; + QueryIntText(&i); + return i; +} + +unsigned XMLElement::UnsignedText(unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedText(&i); + return i; +} + +int64_t XMLElement::Int64Text(int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Text(&i); + return i; +} + +bool XMLElement::BoolText(bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolText(&b); + return b; +} + +double XMLElement::DoubleText(double defaultValue) const +{ + double d = defaultValue; + QueryDoubleText(&d); + return d; +} + +float XMLElement::FloatText(float defaultValue) const +{ + float f = defaultValue; + QueryFloatText(&f); + return f; +} XMLAttribute* XMLElement::FindOrCreateAttribute( const char* name ) @@ -1552,17 +1729,17 @@ XMLAttribute* XMLElement::FindOrCreateAttribute( const char* name ) } } if ( !attrib ) { - TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() ); - attrib = new (_document->_attributePool.Alloc() ) XMLAttribute(); - attrib->_memPool = &_document->_attributePool; + attrib = CreateAttribute(); + TIXMLASSERT( attrib ); if ( last ) { + TIXMLASSERT( last->_next == 0 ); last->_next = attrib; } else { + TIXMLASSERT( _rootAttribute == 0 ); _rootAttribute = attrib; } attrib->SetName( name ); - attrib->_memPool->SetTracked(); // always created and linked. } return attrib; } @@ -1587,30 +1764,31 @@ void XMLElement::DeleteAttribute( const char* name ) } -char* XMLElement::ParseAttributes( char* p ) +char* XMLElement::ParseAttributes( char* p, int* curLineNumPtr ) { const char* start = p; XMLAttribute* prevAttribute = 0; // Read the attributes. while( p ) { - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); if ( !(*p) ) { - _document->SetError( XML_ERROR_PARSING_ELEMENT, start, Name() ); + _document->SetError( XML_ERROR_PARSING_ELEMENT, start, Name(), _parseLineNum ); return 0; } // attribute. if (XMLUtil::IsNameStartChar( *p ) ) { - TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() ); - XMLAttribute* attrib = new (_document->_attributePool.Alloc() ) XMLAttribute(); - attrib->_memPool = &_document->_attributePool; - attrib->_memPool->SetTracked(); + XMLAttribute* attrib = CreateAttribute(); + TIXMLASSERT( attrib ); + attrib->_parseLineNum = _document->_parseCurLineNum; - p = attrib->ParseDeep( p, _document->ProcessEntities() ); + int attrLineNum = attrib->_parseLineNum; + + p = attrib->ParseDeep( p, _document->ProcessEntities(), curLineNumPtr ); if ( !p || Attribute( attrib->Name() ) ) { DeleteAttribute( attrib ); - _document->SetError( XML_ERROR_PARSING_ATTRIBUTE, start, p ); + _document->SetError( XML_ERROR_PARSING_ATTRIBUTE, start, p, attrLineNum ); return 0; } // There is a minor bug here: if the attribute in the source xml @@ -1619,9 +1797,11 @@ char* XMLElement::ParseAttributes( char* p ) // avoids re-scanning the attribute list. Preferring performance for // now, may reconsider in the future. if ( prevAttribute ) { + TIXMLASSERT( prevAttribute->_next == 0 ); prevAttribute->_next = attrib; } else { + TIXMLASSERT( _rootAttribute == 0 ); _rootAttribute = attrib; } prevAttribute = attrib; @@ -1637,7 +1817,7 @@ char* XMLElement::ParseAttributes( char* p ) return p+2; // done; sealed element. } else { - _document->SetError( XML_ERROR_PARSING_ELEMENT, start, p ); + _document->SetError( XML_ERROR_PARSING_ELEMENT, start, p, _parseLineNum ); return 0; } } @@ -1654,14 +1834,24 @@ void XMLElement::DeleteAttribute( XMLAttribute* attribute ) pool->Free( attribute ); } +XMLAttribute* XMLElement::CreateAttribute() +{ + TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() ); + XMLAttribute* attrib = new (_document->_attributePool.Alloc() ) XMLAttribute(); + TIXMLASSERT( attrib ); + attrib->_memPool = &_document->_attributePool; + attrib->_memPool->SetTracked(); + return attrib; +} + // // // foobar // -char* XMLElement::ParseDeep( char* p, StrPair* strPair ) +char* XMLElement::ParseDeep( char* p, StrPair* strPair, int* curLineNumPtr ) { // Read the element name. - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); // The closing element is the form. It is // parsed just like a regular element then deleted from @@ -1676,12 +1866,12 @@ char* XMLElement::ParseDeep( char* p, StrPair* strPair ) return 0; } - p = ParseAttributes( p ); + p = ParseAttributes( p, curLineNumPtr ); if ( !p || !*p || _closingType ) { return p; } - p = XMLNode::ParseDeep( p, strPair ); + p = XMLNode::ParseDeep( p, strPair, curLineNumPtr ); return p; } @@ -1767,15 +1957,15 @@ const char* XMLDocument::_errorNames[XML_ERROR_COUNT] = { }; -XMLDocument::XMLDocument( bool processEntities, Whitespace whitespace ) : +XMLDocument::XMLDocument( bool processEntities, Whitespace whitespaceMode ) : XMLNode( 0 ), _writeBOM( false ), _processEntities( processEntities ), - _errorID( XML_NO_ERROR ), - _whitespace( whitespace ), - _errorStr1( 0 ), - _errorStr2( 0 ), - _charBuffer( 0 ) + _errorID(XML_SUCCESS), + _whitespaceMode( whitespaceMode ), + _errorLineNum( 0 ), + _charBuffer( 0 ), + _parseCurLineNum( 0 ) { // avoid VC++ C4355 warning about 'this' in initializer list (C4355 is off by default in VS2012+) _document = this; @@ -1795,9 +1985,7 @@ void XMLDocument::Clear() #ifdef DEBUG const bool hadError = Error(); #endif - _errorID = XML_NO_ERROR; - _errorStr1 = 0; - _errorStr2 = 0; + ClearError(); delete [] _charBuffer; _charBuffer = 0; @@ -1822,9 +2010,7 @@ void XMLDocument::Clear() XMLElement* XMLDocument::NewElement( const char* name ) { - TIXMLASSERT( sizeof( XMLElement ) == _elementPool.ItemSize() ); - XMLElement* ele = new (_elementPool.Alloc()) XMLElement( this ); - ele->_memPool = &_elementPool; + XMLElement* ele = CreateUnlinkedNode( _elementPool ); ele->SetName( name ); return ele; } @@ -1832,9 +2018,7 @@ XMLElement* XMLDocument::NewElement( const char* name ) XMLComment* XMLDocument::NewComment( const char* str ) { - TIXMLASSERT( sizeof( XMLComment ) == _commentPool.ItemSize() ); - XMLComment* comment = new (_commentPool.Alloc()) XMLComment( this ); - comment->_memPool = &_commentPool; + XMLComment* comment = CreateUnlinkedNode( _commentPool ); comment->SetValue( str ); return comment; } @@ -1842,9 +2026,7 @@ XMLComment* XMLDocument::NewComment( const char* str ) XMLText* XMLDocument::NewText( const char* str ) { - TIXMLASSERT( sizeof( XMLText ) == _textPool.ItemSize() ); - XMLText* text = new (_textPool.Alloc()) XMLText( this ); - text->_memPool = &_textPool; + XMLText* text = CreateUnlinkedNode( _textPool ); text->SetValue( str ); return text; } @@ -1852,9 +2034,7 @@ XMLText* XMLDocument::NewText( const char* str ) XMLDeclaration* XMLDocument::NewDeclaration( const char* str ) { - TIXMLASSERT( sizeof( XMLDeclaration ) == _commentPool.ItemSize() ); - XMLDeclaration* dec = new (_commentPool.Alloc()) XMLDeclaration( this ); - dec->_memPool = &_commentPool; + XMLDeclaration* dec = CreateUnlinkedNode( _commentPool ); dec->SetValue( str ? str : "xml version=\"1.0\" encoding=\"UTF-8\"" ); return dec; } @@ -1862,9 +2042,7 @@ XMLDeclaration* XMLDocument::NewDeclaration( const char* str ) XMLUnknown* XMLDocument::NewUnknown( const char* str ) { - TIXMLASSERT( sizeof( XMLUnknown ) == _commentPool.ItemSize() ); - XMLUnknown* unk = new (_commentPool.Alloc()) XMLUnknown( this ); - unk->_memPool = &_commentPool; + XMLUnknown* unk = CreateUnlinkedNode( _commentPool ); unk->SetValue( str ); return unk; } @@ -1908,7 +2086,7 @@ XMLError XMLDocument::LoadFile( const char* filename ) Clear(); FILE* fp = callfopen( filename, "rb" ); if ( !fp ) { - SetError( XML_ERROR_FILE_NOT_FOUND, filename, 0 ); + SetError( XML_ERROR_FILE_NOT_FOUND, filename, 0, 0 ); return _errorID; } LoadFile( fp ); @@ -1932,10 +2110,12 @@ struct LongFitsIntoSizeTMinusOne { }; template <> -bool LongFitsIntoSizeTMinusOne::Fits( unsigned long /*value*/ ) -{ - return true; -} +struct LongFitsIntoSizeTMinusOne { + static bool Fits( unsigned long ) + { + return true; + } +}; XMLError XMLDocument::LoadFile( FILE* fp ) { @@ -1943,7 +2123,7 @@ XMLError XMLDocument::LoadFile( FILE* fp ) fseek( fp, 0, SEEK_SET ); if ( fgetc( fp ) == EOF && ferror( fp ) != 0 ) { - SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0, 0 ); return _errorID; } @@ -1951,19 +2131,19 @@ XMLError XMLDocument::LoadFile( FILE* fp ) const long filelength = ftell( fp ); fseek( fp, 0, SEEK_SET ); if ( filelength == -1L ) { - SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0, 0 ); return _errorID; } TIXMLASSERT( filelength >= 0 ); if ( !LongFitsIntoSizeTMinusOne<>::Fits( filelength ) ) { // Cannot handle files which won't fit in buffer together with null terminator - SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0, 0 ); return _errorID; } if ( filelength == 0 ) { - SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0, 0 ); return _errorID; } @@ -1972,7 +2152,7 @@ XMLError XMLDocument::LoadFile( FILE* fp ) _charBuffer = new char[size+1]; size_t read = fread( _charBuffer, 1, size, fp ); if ( read != size ) { - SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 ); + SetError( XML_ERROR_FILE_READ_ERROR, 0, 0, 0 ); return _errorID; } @@ -1987,7 +2167,7 @@ XMLError XMLDocument::SaveFile( const char* filename, bool compact ) { FILE* fp = callfopen( filename, "w" ); if ( !fp ) { - SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, filename, 0 ); + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, filename, 0, 0 ); return _errorID; } SaveFile(fp, compact); @@ -2000,7 +2180,7 @@ XMLError XMLDocument::SaveFile( FILE* fp, bool compact ) { // Clear any error from the last save, otherwise it will get reported // for *this* call. - SetError( XML_NO_ERROR, 0, 0 ); + ClearError(); XMLPrinter stream( fp, compact ); Print( &stream ); return _errorID; @@ -2012,7 +2192,7 @@ XMLError XMLDocument::Parse( const char* p, size_t len ) Clear(); if ( len == 0 || !p || !*p ) { - SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0, 0 ); return _errorID; } if ( len == (size_t)(-1) ) { @@ -2050,20 +2230,32 @@ void XMLDocument::Print( XMLPrinter* streamer ) const } -void XMLDocument::SetError( XMLError error, const char* str1, const char* str2 ) +void XMLDocument::SetError( XMLError error, const char* str1, const char* str2, int lineNum ) { TIXMLASSERT( error >= 0 && error < XML_ERROR_COUNT ); _errorID = error; - _errorStr1 = str1; - _errorStr2 = str2; + + _errorStr1.Reset(); + _errorStr2.Reset(); + _errorLineNum = lineNum; + + if (str1) + _errorStr1.SetStr(str1); + if (str2) + _errorStr2.SetStr(str2); +} + +/*static*/ const char* XMLDocument::ErrorIDToName(XMLError errorID) +{ + TIXMLASSERT( errorID >= 0 && errorID < XML_ERROR_COUNT ); + const char* errorName = _errorNames[errorID]; + TIXMLASSERT( errorName && errorName[0] ); + return errorName; } const char* XMLDocument::ErrorName() const { - TIXMLASSERT( _errorID >= 0 && _errorID < XML_ERROR_COUNT ); - const char* errorName = _errorNames[_errorID]; - TIXMLASSERT( errorName && errorName[0] ); - return errorName; + return ErrorIDToName(_errorID); } void XMLDocument::PrintError() const @@ -2073,18 +2265,18 @@ void XMLDocument::PrintError() const char buf1[LEN] = { 0 }; char buf2[LEN] = { 0 }; - if ( _errorStr1 ) { - TIXML_SNPRINTF( buf1, LEN, "%s", _errorStr1 ); + if ( !_errorStr1.Empty() ) { + TIXML_SNPRINTF( buf1, LEN, "%s", _errorStr1.GetStr() ); } - if ( _errorStr2 ) { - TIXML_SNPRINTF( buf2, LEN, "%s", _errorStr2 ); + if ( !_errorStr2.Empty() ) { + TIXML_SNPRINTF( buf2, LEN, "%s", _errorStr2.GetStr() ); } // Should check INT_MIN <= _errorID && _errorId <= INT_MAX, but that // causes a clang "always true" -Wtautological-constant-out-of-range-compare warning TIXMLASSERT( 0 <= _errorID && XML_ERROR_COUNT - 1 <= INT_MAX ); - printf( "XMLDocument error id=%d '%s' str1=%s str2=%s\n", - static_cast( _errorID ), ErrorName(), buf1, buf2 ); + printf( "XMLDocument error id=%d '%s' str1=%s str2=%s line=%d\n", + static_cast( _errorID ), ErrorName(), buf1, buf2, _errorLineNum ); } } @@ -2092,14 +2284,16 @@ void XMLDocument::Parse() { TIXMLASSERT( NoChildren() ); // Clear() must have been called previously TIXMLASSERT( _charBuffer ); + _parseCurLineNum = 1; + _parseLineNum = 1; char* p = _charBuffer; - p = XMLUtil::SkipWhiteSpace( p ); + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); p = const_cast( XMLUtil::ReadBOM( p, &_writeBOM ) ); if ( !*p ) { - SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 ); + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0, 0 ); return; } - ParseDeep(p, 0 ); + ParseDeep(p, 0, &_parseCurLineNum ); } XMLPrinter::XMLPrinter( FILE* file, bool compact, int depth ) : @@ -2117,7 +2311,7 @@ XMLPrinter::XMLPrinter( FILE* file, bool compact, int depth ) : } for( int i=0; i # include # include +# if defined(__PS3__) +# include +# endif #else # include # include @@ -37,6 +40,7 @@ distribution. # include # include #endif +#include /* TODO: intern strings instead of allocation. @@ -68,6 +72,8 @@ distribution. # else # define TINYXML2_LIB # endif +#elif __GNUC__ >= 4 +# define TINYXML2_LIB __attribute__((visibility("default"))) #else # define TINYXML2_LIB #endif @@ -92,9 +98,9 @@ distribution. /* Versioning, past 1.0.14: http://semver.org/ */ -static const int TIXML2_MAJOR_VERSION = 3; +static const int TIXML2_MAJOR_VERSION = 4; static const int TIXML2_MINOR_VERSION = 0; -static const int TIXML2_PATCH_VERSION = 0; +static const int TIXML2_PATCH_VERSION = 1; namespace tinyxml2 { @@ -121,18 +127,20 @@ public: NEEDS_NEWLINE_NORMALIZATION = 0x02, NEEDS_WHITESPACE_COLLAPSING = 0x04, - TEXT_ELEMENT = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + TEXT_ELEMENT = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, TEXT_ELEMENT_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, - ATTRIBUTE_NAME = 0, - ATTRIBUTE_VALUE = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, - ATTRIBUTE_VALUE_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, - COMMENT = NEEDS_NEWLINE_NORMALIZATION + ATTRIBUTE_NAME = 0, + ATTRIBUTE_VALUE = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + ATTRIBUTE_VALUE_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, + COMMENT = NEEDS_NEWLINE_NORMALIZATION }; StrPair() : _flags( 0 ), _start( 0 ), _end( 0 ) {} ~StrPair(); void Set( char* start, char* end, int flags ) { + TIXMLASSERT( start ); + TIXMLASSERT( end ); Reset(); _start = start; _end = end; @@ -152,13 +160,13 @@ public: void SetStr( const char* str, int flags=0 ); - char* ParseText( char* in, const char* endTag, int strFlags ); + char* ParseText( char* in, const char* endTag, int strFlags, int* curLineNumPtr ); char* ParseName( char* in ); void TransferTo( StrPair* other ); + void Reset(); private: - void Reset(); void CollapseWhitespace(); enum { @@ -203,7 +211,8 @@ public: void Push( T t ) { TIXMLASSERT( _size < INT_MAX ); EnsureCapacity( _size+1 ); - _mem[_size++] = t; + _mem[_size] = t; + ++_size; } T* PushArr( int count ) { @@ -217,7 +226,8 @@ public: T Pop() { TIXMLASSERT( _size > 0 ); - return _mem[--_size]; + --_size; + return _mem[_size]; } void PopArr( int count ) { @@ -311,7 +321,7 @@ public: /* Template child class to create pools of the correct type. */ -template< int SIZE > +template< int ITEM_SIZE > class MemPoolT : public MemPool { public: @@ -334,7 +344,7 @@ public: } virtual int ItemSize() const { - return SIZE; + return ITEM_SIZE; } int CurrentAllocs() const { return _currentAllocs; @@ -346,21 +356,23 @@ public: Block* block = new Block(); _blockPtrs.Push( block ); - for( int i=0; ichunk[i].next = &block->chunk[i+1]; + Item* blockItems = block->items; + for( int i = 0; i < ITEMS_PER_BLOCK - 1; ++i ) { + blockItems[i].next = &(blockItems[i + 1]); } - block->chunk[COUNT-1].next = 0; - _root = block->chunk; + blockItems[ITEMS_PER_BLOCK - 1].next = 0; + _root = blockItems; } - void* result = _root; + Item* const result = _root; + TIXMLASSERT( result != 0 ); _root = _root->next; ++_currentAllocs; if ( _currentAllocs > _maxAllocs ) { _maxAllocs = _currentAllocs; } - _nAllocs++; - _nUntracked++; + ++_nAllocs; + ++_nUntracked; return result; } @@ -369,20 +381,21 @@ public: return; } --_currentAllocs; - Chunk* chunk = static_cast( mem ); + Item* item = static_cast( mem ); #ifdef DEBUG - memset( chunk, 0xfe, sizeof(Chunk) ); + memset( item, 0xfe, sizeof( *item ) ); #endif - chunk->next = _root; - _root = chunk; + item->next = _root; + _root = item; } void Trace( const char* name ) { printf( "Mempool %s watermark=%d [%dk] current=%d size=%d nAlloc=%d blocks=%d\n", - name, _maxAllocs, _maxAllocs*SIZE/1024, _currentAllocs, SIZE, _nAllocs, _blockPtrs.Size() ); + name, _maxAllocs, _maxAllocs * ITEM_SIZE / 1024, _currentAllocs, + ITEM_SIZE, _nAllocs, _blockPtrs.Size() ); } void SetTracked() { - _nUntracked--; + --_nUntracked; } int Untracked() const { @@ -398,21 +411,23 @@ public: // 16k: 5200 // 32k: 4300 // 64k: 4000 21000 - enum { COUNT = (4*1024)/SIZE }; // Some compilers do not accept to use COUNT in private part if COUNT is private + // Declared public because some compilers do not accept to use ITEMS_PER_BLOCK + // in private part if ITEMS_PER_BLOCK is private + enum { ITEMS_PER_BLOCK = (4 * 1024) / ITEM_SIZE }; private: MemPoolT( const MemPoolT& ); // not supported void operator=( const MemPoolT& ); // not supported - union Chunk { - Chunk* next; - char mem[SIZE]; + union Item { + Item* next; + char itemData[ITEM_SIZE]; }; struct Block { - Chunk chunk[COUNT]; + Item items[ITEMS_PER_BLOCK]; }; DynArray< Block*, 10 > _blockPtrs; - Chunk* _root; + Item* _root; int _currentAllocs; int _nAllocs; @@ -485,7 +500,6 @@ public: // WARNING: must match XMLDocument::_errorNames[] enum XMLError { XML_SUCCESS = 0, - XML_NO_ERROR = 0, XML_NO_ATTRIBUTE, XML_WRONG_ATTRIBUTE_TYPE, XML_ERROR_FILE_NOT_FOUND, @@ -513,19 +527,23 @@ enum XMLError { /* Utility functionality. */ -class XMLUtil +class TINYXML2_LIB XMLUtil { public: - static const char* SkipWhiteSpace( const char* p ) { + static const char* SkipWhiteSpace( const char* p, int* curLineNumPtr ) { TIXMLASSERT( p ); + while( IsWhiteSpace(*p) ) { + if (curLineNumPtr && *p == '\n') { + ++(*curLineNumPtr); + } ++p; } TIXMLASSERT( p ); return p; } - static char* SkipWhiteSpace( char* p ) { - return const_cast( SkipWhiteSpace( const_cast(p) ) ); + static char* SkipWhiteSpace( char* p, int* curLineNumPtr ) { + return const_cast( SkipWhiteSpace( const_cast(p), curLineNumPtr ) ); } // Anything in the high order range of UTF-8 is assumed to not be whitespace. This isn't @@ -556,6 +574,9 @@ public: if ( p == q ) { return true; } + TIXMLASSERT( p ); + TIXMLASSERT( q ); + TIXMLASSERT( nChar >= 0 ); return strncmp( p, q, nChar ) == 0; } @@ -575,6 +596,7 @@ public: static void ToStr( bool v, char* buffer, int bufferSize ); static void ToStr( float v, char* buffer, int bufferSize ); static void ToStr( double v, char* buffer, int bufferSize ); + static void ToStr(int64_t v, char* buffer, int bufferSize); // converts strings to primitive types static bool ToInt( const char* str, int* value ); @@ -582,6 +604,18 @@ public: static bool ToBool( const char* str, bool* value ); static bool ToFloat( const char* str, float* value ); static bool ToDouble( const char* str, double* value ); + static bool ToInt64(const char* str, int64_t* value); + + // Changes what is serialized for a boolean value. + // Default to "true" and "false". Shouldn't be changed + // unless you have a special testing or compatibility need. + // Be careful: static, global, & not thread safe. + // Be sure to set static const memory as parameters. + static void SetBoolSerialization(const char* writeTrue, const char* writeFalse); + +private: + static const char* writeBoolTrue; + static const char* writeBoolFalse; }; @@ -687,6 +721,9 @@ public: */ void SetValue( const char* val, bool staticMem=false ); + /// Gets the line number the node is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + /// Get the parent of this node on the DOM. const XMLNode* Parent() const { return _parent; @@ -852,15 +889,30 @@ public: */ virtual bool Accept( XMLVisitor* visitor ) const = 0; + /** + Set user data into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void SetUserData(void* userData) { _userData = userData; } + + /** + Get user data set into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void* GetUserData() const { return _userData; } + protected: XMLNode( XMLDocument* ); virtual ~XMLNode(); - virtual char* ParseDeep( char*, StrPair* ); + virtual char* ParseDeep( char*, StrPair*, int* ); XMLDocument* _document; XMLNode* _parent; mutable StrPair _value; + int _parseLineNum; XMLNode* _firstChild; XMLNode* _lastChild; @@ -868,11 +920,14 @@ protected: XMLNode* _prev; XMLNode* _next; + void* _userData; + private: MemPool* _memPool; void Unlink( XMLNode* child ); static void DeleteNode( XMLNode* node ); void InsertChildPreamble( XMLNode* insertThis ) const; + const XMLElement* ToElementWithName( const char* name ) const; XMLNode( const XMLNode& ); // not supported XMLNode& operator=( const XMLNode& ); // not supported @@ -893,7 +948,6 @@ private: */ class TINYXML2_LIB XMLText : public XMLNode { - friend class XMLBase; friend class XMLDocument; public: virtual bool Accept( XMLVisitor* visitor ) const; @@ -921,7 +975,7 @@ protected: XMLText( XMLDocument* doc ) : XMLNode( doc ), _isCData( false ) {} virtual ~XMLText() {} - char* ParseDeep( char*, StrPair* endTag ); + char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr ); private: bool _isCData; @@ -952,7 +1006,7 @@ protected: XMLComment( XMLDocument* doc ); virtual ~XMLComment(); - char* ParseDeep( char*, StrPair* endTag ); + char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr); private: XMLComment( const XMLComment& ); // not supported @@ -991,7 +1045,7 @@ protected: XMLDeclaration( XMLDocument* doc ); virtual ~XMLDeclaration(); - char* ParseDeep( char*, StrPair* endTag ); + char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr ); private: XMLDeclaration( const XMLDeclaration& ); // not supported @@ -1026,7 +1080,7 @@ protected: XMLUnknown( XMLDocument* doc ); virtual ~XMLUnknown(); - char* ParseDeep( char*, StrPair* endTag ); + char* ParseDeep( char*, StrPair* endTag, int* curLineNumPtr ); private: XMLUnknown( const XMLUnknown& ); // not supported @@ -1051,6 +1105,9 @@ public: /// The value of the attribute. const char* Value() const; + /// Gets the line number the attribute is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + /// The next attribute in the list. const XMLAttribute* Next() const { return _next; @@ -1060,11 +1117,18 @@ public: If the value isn't an integer, 0 will be returned. There is no error checking; use QueryIntValue() if you need error checking. */ - int IntValue() const { - int i=0; - QueryIntValue( &i ); - return i; - } + int IntValue() const { + int i = 0; + QueryIntValue(&i); + return i; + } + + int64_t Int64Value() const { + int64_t i = 0; + QueryInt64Value(&i); + return i; + } + /// Query as an unsigned integer. See IntValue() unsigned UnsignedValue() const { unsigned i=0; @@ -1091,13 +1155,15 @@ public: } /** QueryIntValue interprets the attribute as an integer, and returns the value - in the provided parameter. The function will return XML_NO_ERROR on success, + in the provided parameter. The function will return XML_SUCCESS on success, and XML_WRONG_ATTRIBUTE_TYPE if the conversion is not successful. */ XMLError QueryIntValue( int* value ) const; /// See QueryIntValue XMLError QueryUnsignedValue( unsigned int* value ) const; - /// See QueryIntValue + /// See QueryIntValue + XMLError QueryInt64Value(int64_t* value) const; + /// See QueryIntValue XMLError QueryBoolValue( bool* value ) const; /// See QueryIntValue XMLError QueryDoubleValue( double* value ) const; @@ -1110,7 +1176,9 @@ public: void SetAttribute( int value ); /// Set the attribute to value. void SetAttribute( unsigned value ); - /// Set the attribute to value. + /// Set the attribute to value. + void SetAttribute(int64_t value); + /// Set the attribute to value. void SetAttribute( bool value ); /// Set the attribute to value. void SetAttribute( double value ); @@ -1120,17 +1188,18 @@ public: private: enum { BUF_SIZE = 200 }; - XMLAttribute() : _next( 0 ), _memPool( 0 ) {} + XMLAttribute() : _parseLineNum( 0 ), _next( 0 ), _memPool( 0 ) {} virtual ~XMLAttribute() {} XMLAttribute( const XMLAttribute& ); // not supported void operator=( const XMLAttribute& ); // not supported void SetName( const char* name ); - char* ParseDeep( char* p, bool processEntities ); + char* ParseDeep( char* p, bool processEntities, int* curLineNumPtr ); mutable StrPair _name; mutable StrPair _value; + int _parseLineNum; XMLAttribute* _next; MemPool* _memPool; }; @@ -1142,7 +1211,6 @@ private: */ class TINYXML2_LIB XMLElement : public XMLNode { - friend class XMLBase; friend class XMLDocument; public: /// Get the name of an element (which is the Value() of the node.) @@ -1188,42 +1256,25 @@ public: const char* Attribute( const char* name, const char* value=0 ) const; /** Given an attribute name, IntAttribute() returns the value - of the attribute interpreted as an integer. 0 will be - returned if there is an error. For a method with error - checking, see QueryIntAttribute() + of the attribute interpreted as an integer. The default + value will be returned if the attribute isn't present, + or if there is an error. (For a method with error + checking, see QueryIntAttribute()). */ - int IntAttribute( const char* name ) const { - int i=0; - QueryIntAttribute( name, &i ); - return i; - } + int IntAttribute(const char* name, int defaultValue = 0) const; /// See IntAttribute() - unsigned UnsignedAttribute( const char* name ) const { - unsigned i=0; - QueryUnsignedAttribute( name, &i ); - return i; - } + unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const; + /// See IntAttribute() + int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const; + /// See IntAttribute() + bool BoolAttribute(const char* name, bool defaultValue = false) const; /// See IntAttribute() - bool BoolAttribute( const char* name ) const { - bool b=false; - QueryBoolAttribute( name, &b ); - return b; - } + double DoubleAttribute(const char* name, double defaultValue = 0) const; /// See IntAttribute() - double DoubleAttribute( const char* name ) const { - double d=0; - QueryDoubleAttribute( name, &d ); - return d; - } - /// See IntAttribute() - float FloatAttribute( const char* name ) const { - float f=0; - QueryFloatAttribute( name, &f ); - return f; - } + float FloatAttribute(const char* name, float defaultValue = 0) const; /** Given an attribute name, QueryIntAttribute() returns - XML_NO_ERROR, XML_WRONG_ATTRIBUTE_TYPE if the conversion + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion can't be performed, or XML_NO_ATTRIBUTE if the attribute doesn't exist. If successful, the result of the conversion will be written to 'value'. If not successful, nothing will @@ -1242,7 +1293,8 @@ public: } return a->QueryIntValue( value ); } - /// See QueryIntAttribute() + + /// See QueryIntAttribute() XMLError QueryUnsignedAttribute( const char* name, unsigned int* value ) const { const XMLAttribute* a = FindAttribute( name ); if ( !a ) { @@ -1250,7 +1302,17 @@ public: } return a->QueryUnsignedValue( value ); } - /// See QueryIntAttribute() + + /// See QueryIntAttribute() + XMLError QueryInt64Attribute(const char* name, int64_t* value) const { + const XMLAttribute* a = FindAttribute(name); + if (!a) { + return XML_NO_ATTRIBUTE; + } + return a->QueryInt64Value(value); + } + + /// See QueryIntAttribute() XMLError QueryBoolAttribute( const char* name, bool* value ) const { const XMLAttribute* a = FindAttribute( name ); if ( !a ) { @@ -1277,7 +1339,7 @@ public: /** Given an attribute name, QueryAttribute() returns - XML_NO_ERROR, XML_WRONG_ATTRIBUTE_TYPE if the conversion + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion can't be performed, or XML_NO_ATTRIBUTE if the attribute doesn't exist. It is overloaded for the primitive types, and is a generally more convenient replacement of @@ -1301,6 +1363,10 @@ public: return QueryUnsignedAttribute( name, value ); } + int QueryAttribute(const char* name, int64_t* value) const { + return QueryInt64Attribute(name, value); + } + int QueryAttribute( const char* name, bool* value ) const { return QueryBoolAttribute( name, value ); } @@ -1328,7 +1394,14 @@ public: XMLAttribute* a = FindOrCreateAttribute( name ); a->SetAttribute( value ); } - /// Sets the named attribute to value. + + /// Sets the named attribute to value. + void SetAttribute(const char* name, int64_t value) { + XMLAttribute* a = FindOrCreateAttribute(name); + a->SetAttribute(value); + } + + /// Sets the named attribute to value. void SetAttribute( const char* name, bool value ) { XMLAttribute* a = FindOrCreateAttribute( name ); a->SetAttribute( value ); @@ -1425,7 +1498,9 @@ public: void SetText( int value ); /// Convenience method for setting text inside an element. See SetText() for important limitations. void SetText( unsigned value ); - /// Convenience method for setting text inside an element. See SetText() for important limitations. + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText(int64_t value); + /// Convenience method for setting text inside an element. See SetText() for important limitations. void SetText( bool value ); /// Convenience method for setting text inside an element. See SetText() for important limitations. void SetText( double value ); @@ -1461,13 +1536,28 @@ public: XMLError QueryIntText( int* ival ) const; /// See QueryIntText() XMLError QueryUnsignedText( unsigned* uval ) const; - /// See QueryIntText() + /// See QueryIntText() + XMLError QueryInt64Text(int64_t* uval) const; + /// See QueryIntText() XMLError QueryBoolText( bool* bval ) const; /// See QueryIntText() XMLError QueryDoubleText( double* dval ) const; /// See QueryIntText() XMLError QueryFloatText( float* fval ) const; + int IntText(int defaultValue = 0) const; + + /// See QueryIntText() + unsigned UnsignedText(unsigned defaultValue = 0) const; + /// See QueryIntText() + int64_t Int64Text(int64_t defaultValue = 0) const; + /// See QueryIntText() + bool BoolText(bool defaultValue = false) const; + /// See QueryIntText() + double DoubleText(double defaultValue = 0) const; + /// See QueryIntText() + float FloatText(float defaultValue = 0) const; + // internal: enum { OPEN, // @@ -1481,7 +1571,7 @@ public: virtual bool ShallowEqual( const XMLNode* compare ) const; protected: - char* ParseDeep( char* p, StrPair* endTag ); + char* ParseDeep( char* p, StrPair* endTag, int* curLineNumPtr ); private: XMLElement( XMLDocument* doc ); @@ -1494,8 +1584,9 @@ private: } XMLAttribute* FindOrCreateAttribute( const char* name ); //void LinkAttribute( XMLAttribute* attrib ); - char* ParseAttributes( char* p ); + char* ParseAttributes( char* p, int* curLineNumPtr ); static void DeleteAttribute( XMLAttribute* attribute ); + XMLAttribute* CreateAttribute(); enum { BUF_SIZE = 200 }; int _closingType; @@ -1522,7 +1613,7 @@ class TINYXML2_LIB XMLDocument : public XMLNode friend class XMLElement; public: /// constructor - XMLDocument( bool processEntities = true, Whitespace = PRESERVE_WHITESPACE ); + XMLDocument( bool processEntities = true, Whitespace whitespaceMode = PRESERVE_WHITESPACE ); ~XMLDocument(); virtual XMLDocument* ToDocument() { @@ -1536,7 +1627,7 @@ public: /** Parse an XML file from a character string. - Returns XML_NO_ERROR (0) on success, or + Returns XML_SUCCESS (0) on success, or an errorID. You may optionally pass in the 'nBytes', which is @@ -1548,7 +1639,7 @@ public: /** Load an XML file from disk. - Returns XML_NO_ERROR (0) on success, or + Returns XML_SUCCESS (0) on success, or an errorID. */ XMLError LoadFile( const char* filename ); @@ -1561,14 +1652,14 @@ public: not text in order for TinyXML-2 to correctly do newline normalization. - Returns XML_NO_ERROR (0) on success, or + Returns XML_SUCCESS (0) on success, or an errorID. */ XMLError LoadFile( FILE* ); /** Save the XML file to disk. - Returns XML_NO_ERROR (0) on success, or + Returns XML_SUCCESS (0) on success, or an errorID. */ XMLError SaveFile( const char* filename, bool compact = false ); @@ -1577,7 +1668,7 @@ public: Save the XML file to disk. You are responsible for providing and closing the FILE*. - Returns XML_NO_ERROR (0) on success, or + Returns XML_SUCCESS (0) on success, or an errorID. */ XMLError SaveFile( FILE* fp, bool compact = false ); @@ -1586,7 +1677,7 @@ public: return _processEntities; } Whitespace WhitespaceMode() const { - return _whitespace; + return _whitespaceMode; } /** @@ -1671,25 +1762,35 @@ public: */ void DeleteNode( XMLNode* node ); - void SetError( XMLError error, const char* str1, const char* str2 ); + void SetError( XMLError error, const char* str1, const char* str2, int lineNum ); + + void ClearError() { + SetError(XML_SUCCESS, 0, 0, 0); + } /// Return true if there was an error parsing the document. bool Error() const { - return _errorID != XML_NO_ERROR; + return _errorID != XML_SUCCESS; } /// Return the errorID. XMLError ErrorID() const { return _errorID; } const char* ErrorName() const; + static const char* ErrorIDToName(XMLError errorID); /// Return a possibly helpful diagnostic location or string. const char* GetErrorStr1() const { - return _errorStr1; + return _errorStr1.GetStr(); } /// Return a possibly helpful secondary diagnostic location or string. const char* GetErrorStr2() const { - return _errorStr2; + return _errorStr2.GetStr(); + } + /// Return the line where the error occured, or zero if unknown. + int GetErrorLineNum() const + { + return _errorLineNum; } /// If there is an error, print it to stdout. void PrintError() const; @@ -1711,13 +1812,15 @@ private: XMLDocument( const XMLDocument& ); // not supported void operator=( const XMLDocument& ); // not supported - bool _writeBOM; - bool _processEntities; - XMLError _errorID; - Whitespace _whitespace; - const char* _errorStr1; - const char* _errorStr2; - char* _charBuffer; + bool _writeBOM; + bool _processEntities; + XMLError _errorID; + Whitespace _whitespaceMode; + mutable StrPair _errorStr1; + mutable StrPair _errorStr2; + int _errorLineNum; + char* _charBuffer; + int _parseCurLineNum; MemPoolT< sizeof(XMLElement) > _elementPool; MemPoolT< sizeof(XMLAttribute) > _attributePool; @@ -1727,8 +1830,21 @@ private: static const char* _errorNames[XML_ERROR_COUNT]; void Parse(); + + template + NodeType* CreateUnlinkedNode( MemPoolT& pool ); }; +template +inline NodeType* XMLDocument::CreateUnlinkedNode( MemPoolT& pool ) +{ + TIXMLASSERT( sizeof( NodeType ) == PoolElementSize ); + TIXMLASSERT( sizeof( NodeType ) == pool.ItemSize() ); + NodeType* returnNode = new (pool.Alloc()) NodeType( this ); + TIXMLASSERT( returnNode ); + returnNode->_memPool = &pool; + return returnNode; +} /** A XMLHandle is a class that wraps a node pointer with null checks; this is @@ -1845,19 +1961,19 @@ public: } /// Safe cast to XMLElement. This can return null. XMLElement* ToElement() { - return ( ( _node == 0 ) ? 0 : _node->ToElement() ); + return ( _node ? _node->ToElement() : 0 ); } /// Safe cast to XMLText. This can return null. XMLText* ToText() { - return ( ( _node == 0 ) ? 0 : _node->ToText() ); + return ( _node ? _node->ToText() : 0 ); } /// Safe cast to XMLUnknown. This can return null. XMLUnknown* ToUnknown() { - return ( ( _node == 0 ) ? 0 : _node->ToUnknown() ); + return ( _node ? _node->ToUnknown() : 0 ); } /// Safe cast to XMLDeclaration. This can return null. XMLDeclaration* ToDeclaration() { - return ( ( _node == 0 ) ? 0 : _node->ToDeclaration() ); + return ( _node ? _node->ToDeclaration() : 0 ); } private: @@ -1917,16 +2033,16 @@ public: return _node; } const XMLElement* ToElement() const { - return ( ( _node == 0 ) ? 0 : _node->ToElement() ); + return ( _node ? _node->ToElement() : 0 ); } const XMLText* ToText() const { - return ( ( _node == 0 ) ? 0 : _node->ToText() ); + return ( _node ? _node->ToText() : 0 ); } const XMLUnknown* ToUnknown() const { - return ( ( _node == 0 ) ? 0 : _node->ToUnknown() ); + return ( _node ? _node->ToUnknown() : 0 ); } const XMLDeclaration* ToDeclaration() const { - return ( ( _node == 0 ) ? 0 : _node->ToDeclaration() ); + return ( _node ? _node->ToDeclaration() : 0 ); } private: @@ -1998,7 +2114,8 @@ public: void PushAttribute( const char* name, const char* value ); void PushAttribute( const char* name, int value ); void PushAttribute( const char* name, unsigned value ); - void PushAttribute( const char* name, bool value ); + void PushAttribute(const char* name, int64_t value); + void PushAttribute( const char* name, bool value ); void PushAttribute( const char* name, double value ); /// If streaming, close the Element. virtual void CloseElement( bool compactMode=false ); @@ -2009,7 +2126,9 @@ public: void PushText( int value ); /// Add a text node from an unsigned. void PushText( unsigned value ); - /// Add a text node from a bool. + /// Add a text node from an unsigned. + void PushText(int64_t value); + /// Add a text node from a bool. void PushText( bool value ); /// Add a text node from a float. void PushText( float value ); diff --git a/main.cpp b/main.cpp index 07e9163..52a00bc 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*NormalN*"; + ::testing::GTEST_FLAG(filter) = "*FloorplanCeilings*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h index 02aee2e..8c31237 100644 --- a/sensors/MACAddress.h +++ b/sensors/MACAddress.h @@ -24,7 +24,7 @@ private: } fields; /** store as 64-bit integer */ - uint64_t mac; + uint64_t mac = 0; public: @@ -61,8 +61,8 @@ public: fields.h5 = hexWordToInt(str[ 0], str[ 1]); fields.h4 = hexWordToInt(str[ 2], str[ 3]); fields.h3 = hexWordToInt(str[ 4], str[ 5]); - fields.h2 = hexWordToInt(str[ 6], str[7]); - fields.h1 = hexWordToInt(str[8], str[9]); + fields.h2 = hexWordToInt(str[ 6], str[ 7]); + fields.h1 = hexWordToInt(str[ 8], str[ 9]); fields.h0 = hexWordToInt(str[10], str[11]); } else{ @@ -89,7 +89,9 @@ public: /** get the mac address as a long-int value */ uint64_t asLong() const { - return mac; + // even if we initialized all 8 bytes with zero + // we mask the 2 unsued bytes to be absolutely safe + return mac & 0x0000FFFFFFFFFFFF; } /** equal? */ @@ -105,13 +107,19 @@ public: private: /** convert the given hex char [0-F] to an integer [0-15] */ - static uint8_t hexCharToInt(const char _hex) { - - // to upper case - const char hex = (_hex >= 'a') ? (_hex - ('a' - 'A')) : (_hex); + static uint8_t hexCharToInt(const char hex) { // convert - return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10); + if (hex >= '0' && hex <= '9') { // digits + return (hex - '0'); + } else if (hex >= 'a' && hex <= 'f') { // lower case + return (hex - 'a' + 10); + } else if (hex >= 'A' && hex <= 'F') { // upper case + return (hex - 'A' + 10); + } + + // found an invalid character + throw Exception("invalid character within MAC address"); } diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index bdf2292..b8a7545 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -7,6 +7,8 @@ #include "../../math/Stats.h" +#include "../../misc/Debug.h" + #include "WiFiMeasurements.h" class VAPGrouper { @@ -40,6 +42,8 @@ public: private: + static constexpr const char* name = "VAPGrp"; + /** the mode to use for grouping VAPs */ const Mode mode; @@ -85,6 +89,9 @@ public: } + Log::add(name, "grouped " + std::to_string(original.entries.size()) + " measurements into " + std::to_string(result.entries.size()), true); + + // done return result; diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index d9834d6..f0a7e05 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -2,6 +2,7 @@ #define WIFIMODELLOGDISTCEILING_H #include "../../../floorplan/v2/Floorplan.h" +#include "../../../floorplan/v2/FloorplanCeilings.h" #include "../../../Assertions.h" #include "WiFiModel.h" @@ -36,21 +37,18 @@ private: /** map of all APs (and their parameters) known to the model */ std::unordered_map accessPoints; - /** position (height) of all ceilings (in meter) */ - std::vector ceilingsAtHeight_m; +// /** position (height) of all ceilings (in meter) */ +// std::vector ceilingsAtHeight_m; + Floorplan::Ceilings ceilings; public: /** ctor with floorplan (needed for ceiling position) */ - WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) { + WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) : ceilings(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); - } } @@ -123,74 +121,13 @@ 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 * numCeilingsBetween(position_m, params.position_m); + const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m); // combine return rssiLOS + wafLoss; } - - -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 { - - int cnt = 0; - 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;} - } - - return cnt; - - } - }; diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index a92652b..940faa4 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -8,10 +8,10 @@ #include /** - * denotes a wifi fingerprint - * known position and several measurements conducted at this position + * denotes a wifi fingerprint: + * known position and 1-n measurements [wifi-scan on all channels] conducted at this position * - * as several measurements were conducted, each AP is usually contained more than once! + * if more than one measurement is conducted, each AP is contained more than once! */ struct WiFiFingerprint { @@ -19,7 +19,7 @@ struct WiFiFingerprint { /** real-world-position that was measured */ Point3 pos_m; - /** measurements (APs) at the given location */ + /** seen APs at the given location */ WiFiMeasurements measurements; @@ -31,7 +31,7 @@ struct WiFiFingerprint { - /** as each AP is contained more than once (scanned more than once), group them by MAC and use the average RSSI */ + /** as each AP might be 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) diff --git a/sensors/radio/setup/WiFiFingerprints.h b/sensors/radio/setup/WiFiFingerprints.h new file mode 100644 index 0000000..9dd1b2f --- /dev/null +++ b/sensors/radio/setup/WiFiFingerprints.h @@ -0,0 +1,125 @@ +#ifndef WIFIFINGERPRINTS_H +#define WIFIFINGERPRINTS_H + +#include + +#include "WiFiFingerprint.h" + + +/** + * helper class to load and save fingerprints. + * fingerprints are wifi-measurements given at some known location + * those can be used to e.g. optimize access-point models etc. + */ +class WiFiFingerprints { + +private: + + /** the file to save the calibration model to */ + std::string file; + + /** all fingerprints (position -> measurements) within the model */ + std::vector fingerprints; + +public: + + WiFiFingerprints(const std::string& file) : file(file) { + load(); + } + + const std::vector& getFingerprints() const { + return fingerprints; + } + + /** get all fingerprints that measured exactly the given mac [no VAP grouping] */ + const std::vector getFingerprintsFor(const MACAddress& mac) const { + + std::vector res; + + // process each fingerprint location + for (const WiFiFingerprint& _fp : fingerprints) { + + // start with an empty copy + WiFiFingerprint fp = _fp; fp.measurements.entries.clear(); + + // only add the measurements that belong to the requested mac + for (const WiFiMeasurement& _m : _fp.measurements.entries) { + if (_m.getAP().getMAC() == mac) { + fp.measurements.entries.push_back(_m); + } + } + if (fp.measurements.entries.size() > 0) { // got some measurements for this AP? + res.push_back(fp); + } + } + + return res; + + } + + /** deserialize the model */ + void load() { + + // open and check + std::ifstream inp(file.c_str()); + if (inp.bad() || inp.eof() || ! inp.is_open()) { return; } + + // read all entries + while (!inp.eof()) { + + // each section starts with [fingerprint] + std::string section; + inp >> section; + if (inp.eof()) {break;} + if (section != "[fingerprint]") {throw Exception("error!");} + + // deserialize it + WiFiFingerprint wfp; + wfp.read(inp); + if (wfp.measurements.entries.empty()) {continue;} + fingerprints.push_back(wfp); + + } + + inp.close(); + + } + + + /** serialize the model */ + void save() { + + // open and check + std::ofstream out(file.c_str()); + if (out.bad()) {throw Exception("error while opening " + file + " for write");} + + // write all entries + for (const WiFiFingerprint& wfp : fingerprints) { + out << "[fingerprint]\n"; + wfp.write(out); + } + + // done + out.close(); + + } + + /** get the fingerprint for the given location. if no fingerprint exists, an empty one is created! */ + WiFiFingerprint& getFingerprint(const Point3 pos_m) { + + // try to find an existing one + for (WiFiFingerprint& wfp : fingerprints) { + // get within range of floating-point rounding issues + if (wfp.pos_m.getDistance(pos_m) < 0.01) {return wfp;} + } + + // create a new one and return its reference + WiFiFingerprint wfp(pos_m); + fingerprints.push_back(wfp); + return fingerprints.back(); + + } + +}; + +#endif // WIFIFINGERPRINTS_H diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index 5ac22b6..c9996c9 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -1,247 +1,61 @@ -#ifndef OPTIMIZER_H -#define OPTIMIZER_H - -#include "../../../floorplan/v2/Floorplan.h" -#include "../../../floorplan/v2/FloorplanHelper.h" +#ifndef WIFIOPTIMIZER_H +#define WIFIOPTIMIZER_H +#include "WiFiFingerprints.h" +#include "WiFiOptimizerStructs.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 +namespace WiFiOptimizer { -#include -#include + /** base-class for all WiFiOptimizers */ + class Base { -struct WiFiOptimizer { + protected: -private: + /** each MAC-Adress has several position->rssi entries */ + std::unordered_map> apMap; - /** combine one RSSI measurement with the position the signal was measured at */ - struct RSSIatPosition { + /** how to handle virtual access points [group, not-group, ..] */ + const VAPGrouper vg; - /** real-world position (in meter) */ - const Point3 pos_m; - - /** measured signal strength (for one AP) */ - const float rssi; + public: /** ctor */ - RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;} + Base(const VAPGrouper vg) : vg(vg) {;} - }; - -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.getRSSI()); - apMap[m.getAP().getMAC()].push_back(rap); + /** get a list of all to-be-optimized access-points (given by their mac-address) */ + virtual std::vector getAllMACs() const { + std::vector res; + for (const auto& it : apMap) {res.push_back(it.first);} + return res; } - } + /** add a new fingerprint to the optimizer as data-source */ + virtual void addFingerprint(const WiFiFingerprint& fp) { - /** 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;} + // 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.getRSSI()); + apMap[m.getAP().getMAC()].push_back(rap); } - err /= cnt; - err = std::sqrt(err); + } - if (params->txp < -50) {err += 999999;} - if (params->txp > -35) {err += 999999;} + /** add new fingerprints to the optimizer as data-source */ + virtual void addFingerprints(const WiFiFingerprints& fps) { + for (const WiFiFingerprint& fp : fps.getFingerprints()) { + addFingerprint(fp); + } + } - 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 +#endif // WIFIOPTIMIZER_H diff --git a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h new file mode 100644 index 0000000..8bd3e42 --- /dev/null +++ b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h @@ -0,0 +1,341 @@ +#ifndef WIFI_OPTIMIZER_LOG_DIST_CEILING_H +#define WIFI_OPTIMIZER_LOG_DIST_CEILING_H + +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../floorplan/v2/FloorplanHelper.h" + +#include "../../../geo/BBox3.h" +#include "../../../misc/Debug.h" + +#include "WiFiFingerprint.h" +#include "WiFiFingerprints.h" + +#include "../model/WiFiModel.h" +#include "../model/WiFiModelLogDistCeiling.h" + +#include +#include +#include + +#include +#include + +#include "WiFiOptimizer.h" +#include "WiFiOptimizerStructs.h" + +#include + +namespace WiFiOptimizer { + + /** + * optimize access-point parameters, + * given several fingerprints using the log-dist-ceiling model + */ + struct LogDistCeiling : public Base { + + public: + + enum class Mode { + FAST, + MEDIUM, + QUALITY, + }; + + /** + * resulting optimization stats for one AP + */ + struct Stats { + + /** average model<->scan error after optimzing */ + float error_db; + + /** number of fingerprints [= locations] that were used for optimzing */ + int usedFingerprins; + + /** resulting model<->scan error after optimzing for each individual fingerprints [= location] */ + std::vector errors; + + /** get the location where the model estimation reaches the highest negative value [model estimation too low] */ + ErrorAtPosition getEstErrorMaxNeg() const { + auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();}; + return *std::min_element(errors.begin(), errors.end(), cmpErrAtPos); + } + + /** get the location where the model estimation reaches the highest positive value [model estimation too high] */ + ErrorAtPosition getEstErrorMaxPos() const { + auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();}; + return *std::max_element(errors.begin(), errors.end(), cmpErrAtPos); + } + + }; + + /** parameters for one AP when using the LogDistCeiling model */ + struct APParams { + + float x; + float y; + float z; + + float txp; + float exp; + float waf; + + Point3 getPos() const {return Point3(x,y,z);} + + /** ctor */ + APParams() {;} + + /** ctor */ + 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() const { + 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) {;} + }; + + class APParamsList { + + std::vector lst; + + public: + + /** ctor */ + APParamsList(const std::vector& lst) : lst(lst) { + + } + + /** get the list */ + const std::vector& get() const { + return lst; + } + + /** get params for the given mac [if known, otherwise nullptr] */ + const APParamsMAC* get (const MACAddress& mac) const { + for (const APParamsMAC& ap : lst) { + if (ap.mac == mac) {return ≈} + } + return nullptr; + } + + }; + + using APFilter = std::function; + + const APFilter NONE = [] (const int numFingerprints, const MACAddress& mac) { + (void) numFingerprints; (void) mac; + return false; + }; + + const APFilter MIN_8_FPS = [] (const int numFingerprints, const MACAddress& mac) { + (void) mac; + return numFingerprints < 8; + }; + + private: + + Mode mode = Mode::QUALITY; + + Floorplan::IndoorMap* map; + + const char* name = "WiFiOptLDC"; + + public: + + /** ctor */ + LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) { + ; + } + + /** ctor */ + LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const WiFiFingerprints& fps, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) { + addFingerprints(fps); + } + + /** optimize all known APs */ + APParamsList optimizeAll(APFilter filter) const { + + // sanity check + Assert::isFalse(getAllMACs().empty(), "no APs found for optimization! call addFingerprint() first!"); + + float errSum = 0; int errCnt = 0; + std::vector res; + for (const MACAddress& mac : getAllMACs()) { + Stats stats; + const APParams params = optimize(mac, stats); + if (!filter(stats.usedFingerprins, mac)) { + res.push_back(APParamsMAC(mac, params)); + errSum += stats.error_db; + ++errCnt; + } else { + std::cout << "ignored due to filter!" << std::endl; + } + } + + const float avgErr = errSum / errCnt; + Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB"); + + // done + return APParamsList(res); + + } + + /** optimize the given AP */ + APParams optimize(const MACAddress& mac, Stats& res) const { + + // starting parameters do not matter for the current optimizer! + APParams params(0,0,0, -40, 2.5, -4.0); + + // get all position->rssi measurements for this AP to compare them with the corresponding model estimations + const std::vector& entries = apMap.find(mac)->second; + + // log + Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false); + Log::tick(); + + // get the map's size + 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, 5), // exp + LeOpt::MinMax(-15, -0), // waf + }; + + + + + LeOpt opt(valRegion); + + switch(mode) { + case Mode::FAST: + opt.setPopulationSize(100); + opt.setNumIerations(50); + break; + case Mode::MEDIUM: + opt.setPopulationSize(200); + opt.setNumIerations(100); + break; + case Mode::QUALITY: + opt.setPopulationSize(500); + opt.setNumIerations(150); + break; + } + + // error function + auto func = [&] (const float* params) { + return getErrorLogDistCeiling(mac, entries, params, nullptr); + }; + + 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); + res.error_db = getErrorLogDistCeiling(mac, entries, (float*)¶ms, &res); + res.usedFingerprins = entries.size(); + + Log::tock(); + Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(res.error_db) +" dB err"); + + return params; + + } + + + private: + + float getErrorLogDistCeiling(const MACAddress& mac, const std::vector& entries, const float* data, Stats* stats = nullptr) const { + + constexpr float hugeError = 1e10; + 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 + // signal-strength-prediction-model... + WiFiModelLogDistCeiling model(map); + 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); + + // add error to stats object? + if (stats) { + stats->errors.push_back(ErrorAtPosition(reading.pos_m, reading.rssi, rssiModel)); + } + + // 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; + + } + + + + }; + +} + + +#endif // WIFI_OPTIMIZER_LOG_DIST_CEILING_H diff --git a/sensors/radio/setup/WiFiOptimizerStructs.h b/sensors/radio/setup/WiFiOptimizerStructs.h new file mode 100644 index 0000000..63faac3 --- /dev/null +++ b/sensors/radio/setup/WiFiOptimizerStructs.h @@ -0,0 +1,55 @@ +#ifndef WIFIOPTIMIZERSTRUCTS_H +#define WIFIOPTIMIZERSTRUCTS_H + +#include "../../../geo/Point3.h" + +namespace WiFiOptimizer { + + /** + * one entry that is used during optimization: + * 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) {;} + + }; + + /** + * for statistics + * after optimiziaton + * + * denotes the difference [error] between one fingerprinted rssi + * at location (x,y,z) and the model estimation for this location + */ + struct ErrorAtPosition { + + /** real-world position (in meter) */ + const Point3 pos_m; + + /** measured signal strength (for one AP) */ + const float scan_rssi; + + /** final model's prediction */ + const float model_rssi; + + /** ctor */ + ErrorAtPosition(const Point3 pos_m, const float scan_rssi, const float model_rssi) : pos_m(pos_m), scan_rssi(scan_rssi), model_rssi(model_rssi) {;} + + /** get the difference [error] between model-estimated-rssi and fingerprinted-rssi */ + float getError_db() const { + return model_rssi - scan_rssi; + } + + }; + +} + +#endif // WIFIOPTIMIZERSTRUCTS_H diff --git a/tests/floorplan/TestFloorplanCeilings.cpp b/tests/floorplan/TestFloorplanCeilings.cpp new file mode 100644 index 0000000..21a5457 --- /dev/null +++ b/tests/floorplan/TestFloorplanCeilings.cpp @@ -0,0 +1,85 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../floorplan/v2/FloorplanCeilings.h" +#include + + + +TEST(FloorplanCeilings, numCeilings) { + + // 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); + + Floorplan::Ceilings model(&map); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) ); + ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) ); + +} + +TEST(FloorplanCeilings, 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); + + Floorplan::Ceilings 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/TestMAC.cpp b/tests/sensors/TestMAC.cpp index 4638bf3..60e21d8 100644 --- a/tests/sensors/TestMAC.cpp +++ b/tests/sensors/TestMAC.cpp @@ -20,11 +20,59 @@ TEST(MAC, caseInsensitive) { } +TEST(MAC, invalidLength) { + + ASSERT_THROW(MACAddress("12:34:56:78:9A:B"), Exception); + ASSERT_THROW(MACAddress("2:34:56:78:9A:BC"), Exception); + + ASSERT_THROW(MACAddress("12:34:56:78:9A:"), Exception); + ASSERT_THROW(MACAddress("12:34:56:78:9A"), Exception); + ASSERT_THROW(MACAddress("12:34:56:78:9A:11:"), Exception); + ASSERT_THROW(MACAddress("12:34:56:78:9A:11:11"), Exception); + + +} + +TEST(MAC, invalidChars) { + + ASSERT_THROW(MACAddress("g2:34:56:78:9A:BC"), Exception); + ASSERT_THROW(MACAddress("a2:34:56:78:9A:BG"), Exception); + ASSERT_THROW(MACAddress("a2:34:5!:78:9A:BC"), Exception); + ASSERT_THROW(MACAddress("a2:34:51:78:?A:BC"), Exception); + +} + +TEST(MAC, ends) { + + MACAddress mac1("12:34:56:78:9A:BC"); + MACAddress mac2("12:34:56:78:9a:bc"); + MACAddress mac3("12:34:56:78:9a:bd"); + MACAddress mac4("02:34:56:78:9a:bc"); + MACAddress mac5("13:34:56:78:9a:bc"); + MACAddress mac6("12:34:56:78:9a:cc"); + + ASSERT_EQ(mac1, mac2); ASSERT_EQ(mac1.asString(), mac2.asString()); + + ASSERT_NE(mac1, mac3); ASSERT_NE(mac1.asString(), mac3.asString()); + ASSERT_NE(mac2, mac3); ASSERT_NE(mac2.asString(), mac3.asString()); + + ASSERT_NE(mac1, mac4); ASSERT_NE(mac1.asString(), mac4.asString()); + ASSERT_NE(mac2, mac4); ASSERT_NE(mac2.asString(), mac4.asString()); + + ASSERT_NE(mac1, mac5); ASSERT_NE(mac1.asString(), mac5.asString()); + ASSERT_NE(mac2, mac5); ASSERT_NE(mac2.asString(), mac5.asString()); + + ASSERT_NE(mac1, mac6); ASSERT_NE(mac1.asString(), mac6.asString()); + ASSERT_NE(mac2, mac6); ASSERT_NE(mac2.asString(), mac6.asString()); + +} + TEST(MAC, convertLong) { MACAddress mac1("12:34:56:78:9A:BC"); MACAddress mac2 = MACAddress( mac1.asLong() ); ASSERT_EQ(mac1, mac2); + ASSERT_EQ(mac1.asString(), mac2.asString()); } diff --git a/tests/sensors/radio/TestLogDistanceCeilingModel.cpp b/tests/sensors/radio/TestLogDistanceCeilingModel.cpp index f734d44..246b255 100644 --- a/tests/sensors/radio/TestLogDistanceCeilingModel.cpp +++ b/tests/sensors/radio/TestLogDistanceCeilingModel.cpp @@ -41,81 +41,6 @@ TEST(LogDistanceCeilingModel, calc) { } -TEST(LogDistanceCeilingModel, numCeilings) { - - // 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); - - ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) ); - ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) ); - - ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) ); - ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) ); - - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) ); - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) ); - - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) ); - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) ); - - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) ); - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) ); - - ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) ); - - ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) ); - ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) ); - -} - -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/TestWiFiOptimizer.cpp b/tests/sensors/radio/TestWiFiOptimizer.cpp index c640650..d91bf92 100644 --- a/tests/sensors/radio/TestWiFiOptimizer.cpp +++ b/tests/sensors/radio/TestWiFiOptimizer.cpp @@ -1,7 +1,7 @@ #ifdef WITH_TESTS #include "../../Tests.h" -#include "../../../sensors/radio/setup/WiFiOptimizer.h" +#include "../../../sensors/radio/setup/WiFiOptimizerLogDistCeiling.h" #include "../../../sensors/radio/setup/WiFiFingerprint.h" #include "../../../misc/Debug.h" @@ -55,23 +55,23 @@ TEST(WiFiOptimizer, optimize) { } - WiFiOptimizer opt(&map, vg); + WiFiOptimizer::LogDistCeiling opt(&map, vg); for (const WiFiFingerprint& fp : fingerprints) { opt.addFingerprint(fp); } ASSERT_EQ(2, opt.getAllMACs().size()); - float errRes; + WiFiOptimizer::LogDistCeiling::Stats errRes; - const WiFiOptimizer::APParams params1 = opt.optimize(mac1, errRes); - ASSERT_TRUE(errRes < 0.1); + const WiFiOptimizer::LogDistCeiling::APParams params1 = opt.optimize(mac1, errRes); + ASSERT_TRUE(errRes.error_db < 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); + const WiFiOptimizer::LogDistCeiling::APParams params2 = opt.optimize(mac2, errRes); + ASSERT_TRUE(errRes.error_db < 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); From 485be83b449048efdef2523f2d931941b756ea8b Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 20 Mar 2017 11:42:46 +0100 Subject: [PATCH 20/43] fixed typo in normal, added gt point 3d --- floorplan/v2/Floorplan.h | 4 ++-- floorplan/v2/FloorplanReader.h | 32 ++++++++++++++++---------------- floorplan/v2/FloorplanWriter.h | 19 ++++++++++--------- math/distribution/NormalN.h | 13 +++++++++++++ 4 files changed, 41 insertions(+), 27 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index cfdee4c..0a71fb0 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -251,9 +251,9 @@ namespace Floorplan { /** a GroundTruthPoint located somewhere on a floor */ struct GroundTruthPoint { int id; //TODO: this value can be changed and isn't set incremental within the indoormap - Point2 pos; + Point3 pos; GroundTruthPoint() : id(), pos() {;} - GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;} + GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;} bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index cb29bcc..ba26232 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -111,7 +111,7 @@ namespace Floorplan { if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} - if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} + if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} } return floor; } @@ -198,22 +198,22 @@ namespace Floorplan { } - /** parse the tag */ - static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { - std::vector vec; - FOREACH_NODE(n, el) { - if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } - } - return vec; - } + /** parse the tag */ + static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } + } + return vec; + } - /** parse a tag */ - static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { - GroundTruthPoint* gtp = new GroundTruthPoint(); - gtp->id = el->IntAttribute("id"); - gtp->pos = parsePoint2(el); - return gtp; - } + /** parse a tag */ + static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { + GroundTruthPoint* gtp = new GroundTruthPoint(); + gtp->id = el->IntAttribute("id"); + gtp->pos = parsePoint3(el); + return gtp; + } /** parse the tag */ diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index c8257d1..3c09292 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -146,15 +146,16 @@ namespace Floorplan { } floor->InsertEndChild(pois); - XMLElem* gtpoints = doc.NewElement("gtpoints"); - for (const GroundTruthPoint* gtp : mf->gtpoints) { - XMLElem* elem = doc.NewElement("gtpoint"); - elem->SetAttribute("id", gtp->id); - elem->SetAttribute("x", gtp->pos.x); - elem->SetAttribute("y", gtp->pos.y); - gtpoints->InsertEndChild(elem); - } - floor->InsertEndChild(gtpoints); + XMLElem* gtpoints = doc.NewElement("gtpoints"); + for (const GroundTruthPoint* gtp : mf->gtpoints) { + XMLElem* elem = doc.NewElement("gtpoint"); + elem->SetAttribute("id", gtp->id); + elem->SetAttribute("x", gtp->pos.x); + elem->SetAttribute("y", gtp->pos.y); + elem->SetAttribute("z", gtp->pos.z); + gtpoints->InsertEndChild(elem); + } + floor->InsertEndChild(gtpoints); XMLElem* accesspoints = doc.NewElement("accesspoints"); for (const AccessPoint* ap : mf->accesspoints) { diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index ad2f04e..d98c4d9 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -75,6 +75,19 @@ namespace Distribution { return NormalDistributionN(mean, cov); } + /** return a NormalN based on given data and a given mean vector mu*/ + static NormalDistributionN getNormalNFromSamplesAndMean(const Eigen::MatrixXd& data, const Eigen::VectorXd mean) { + + const int numElements = data.rows(); + Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the 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); + + return NormalDistributionN(mean, cov); + } + }; } From 3868e42937f59a7355853e0f59cd155ed712b213 Mon Sep 17 00:00:00 2001 From: kazu Date: Mon, 20 Mar 2017 12:10:34 +0100 Subject: [PATCH 21/43] started adding floorplan LINT new helper methods --- floorplan/v2/FloorplanLINT.h | 79 ++++++++++++++++++++++++++++++++++ floorplan/v2/FloorplanReader.h | 6 +-- geo/BBox2.h | 5 ++- geo/Point2.h | 4 ++ 4 files changed, 90 insertions(+), 4 deletions(-) create mode 100644 floorplan/v2/FloorplanLINT.h diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h new file mode 100644 index 0000000..a2a840d --- /dev/null +++ b/floorplan/v2/FloorplanLINT.h @@ -0,0 +1,79 @@ +#ifndef FLOORPLANLINT_H +#define FLOORPLANLINT_H + +#include "Floorplan.h" +#include "../../geo/BBox2.h" + +namespace Floorplan { + + class LINT { + + public: + + static void check(IndoorMap* map) { + + for (const Floor* floor : map->floors) { + + // outline present? + if (floor->outline.empty()) {throw Exception("floor " + floor->name + " has no outline");} + + // check outline + for (FloorOutlinePolygon* poly : floor->outline) { + checkOutline(floor, poly); + } + + // check obstacles + for (FloorObstacle* obs : floor->obstacles) { + checkObstacle(floor, obs); + } + + } + + } + + private: + + /** check a floor's outline */ + static void checkOutline(const Floor* floor, const FloorOutlinePolygon* poly) { + + // number of points valid? + if (poly->poly.points.size() < 3) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' needs at least 3 edges");} + if (poly->poly.points.size() > 1024) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' has too many edges");} + + // outline size [bbox] valid? + BBox2 outline; + for (const Point2 pt : poly->poly.points) { outline.add(pt); } + const Point2 size = outline.getSize(); + if (size.x < 1.0 || size.y < 1.0) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' seems too small");} + + } + + /** check walls, doors, ... */ + static void checkObstacle(const Floor* floor, const FloorObstacle* fo) { + + // line? -> check + const FloorObstacleLine* line = dynamic_cast(fo); + if (line) { + const float len_m = line->from.getDistance(line->to); + if (len_m < 0.15) { + throw Exception("floor '" + floor->name + "' line-obstacle is too short: " + std::to_string(len_m) + " meter from " + line->from.asString() + " to " + line->to.asString()); + } + } + + // door? -> check + const FloorObstacleDoor* door = dynamic_cast(fo); + if (door) { + const float len_m = door->from.getDistance(door->to); + if (len_m < 0.40) { + throw Exception("floor '" + floor->name + "' door is too narrow: " + std::to_string(len_m) + " meter from " + door->from.asString() + " to " + door->to.asString()); + } + } + + } + + }; + +} + + +#endif // FLOORPLANLINT_H diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 5d19af6..4959145 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -4,6 +4,7 @@ #include #include "Floorplan.h" +#include "FloorplanLINT.h" #include "../../misc/Debug.h" #include "../../Assertions.h" @@ -40,6 +41,7 @@ namespace Floorplan { ); } IndoorMap* map = parse(doc); + Floorplan::LINT::check(map); return map; } @@ -57,6 +59,7 @@ namespace Floorplan { ); } IndoorMap* map = parse(doc); + Floorplan::LINT::check(map); return map; } @@ -431,9 +434,6 @@ namespace Floorplan { poly.points.push_back(p2); } } - if (poly.points.size() < 4 || poly.points.size() > 1024) { - throw Exception("detected invalid outline-polygon during XML parsing"); - } return poly; } diff --git a/geo/BBox2.h b/geo/BBox2.h index c4833c3..1503f01 100644 --- a/geo/BBox2.h +++ b/geo/BBox2.h @@ -37,7 +37,7 @@ public: } /** returns true if the bbox is not yet configured */ - const bool isInvalid() const { + bool isInvalid() const { return p1.x == MAX || p1.y == MAX || p2.x == MIN || p2.y == MIN; } @@ -47,6 +47,9 @@ public: /** get the bbox's maximum */ const Point2& getMax() const {return p2;} + /** get the bbox's size [max-min] */ + const Point2 getSize() const {return p2-p1;} + /** get the bbox's center point */ Point2 getCenter() const { return (p1+p2) / 2; } diff --git a/geo/Point2.h b/geo/Point2.h index 3b19fd7..0d94f12 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -61,6 +61,10 @@ struct Point2 { return std::sqrt(dx*dx + dy*dy); } + std::string asString() const { + return "(" + std::to_string(x) + "," + std::to_string(y) + ")"; + } + }; inline void swap(Point2& p1, Point2& p2) { From 4ac248d08ecf9452d18e2f2f6e6f6dc524f310d4 Mon Sep 17 00:00:00 2001 From: FrankE Date: Mon, 20 Mar 2017 14:44:17 +0100 Subject: [PATCH 22/43] adjusted floorplan-lint for GUI integration --- floorplan/v2/FloorplanLINT.h | 155 ++++++++++++++++++++++++++++++--- floorplan/v2/FloorplanReader.h | 2 - 2 files changed, 142 insertions(+), 15 deletions(-) diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index a2a840d..ebb5db9 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -4,59 +4,132 @@ #include "Floorplan.h" #include "../../geo/BBox2.h" +#include +#include + namespace Floorplan { class LINT { public: - static void check(IndoorMap* map) { + /** possible issue types */ + enum class Type { + WARN, + ERROR, + }; + + /** type -> string */ + static std::string getTypeStr(const Type t) { + switch(t) { + case Type::WARN: return "WARNING"; + case Type::ERROR: return "ERROR"; + default: throw Exception("code error. invalid type. todo!"); + } + } + + /** a detected issue */ + struct Issue { + + Type type; + const Floor* floor; + std::string desc; + + Issue(const Type type, const Floor* floor, const std::string& desc) : type(type), floor(floor), desc(desc) {;} + + /** issue to string */ + std::string asString() const { + return getTypeStr(type) + ": " + "floor '" + floor->name + "': " + desc; + } + + }; + + using Issues = std::vector; + + + + public: + + /** throw in case of errors within the map */ + static void assertOK(IndoorMap* map) { + + const Issues issues = check(map); + int err = 0; + for (const Issue& i : issues) { + std::cout << i.asString() << std::endl; + if (i.type == Type::ERROR) {++err;} + } + if (err > 0) { + throw Exception("detected floorplan errors"); + } + + } + + /** get all errors within the map */ + static Issues check(IndoorMap* map) { + + Issues res; for (const Floor* floor : map->floors) { // outline present? - if (floor->outline.empty()) {throw Exception("floor " + floor->name + " has no outline");} + if (floor->outline.empty()) { + res.push_back(Issue(Type::ERROR, floor, "has no outline")); + } // check outline - for (FloorOutlinePolygon* poly : floor->outline) { - checkOutline(floor, poly); + for (const FloorOutlinePolygon* poly : floor->outline) { + checkOutline(res, floor, poly); } // check obstacles - for (FloorObstacle* obs : floor->obstacles) { - checkObstacle(floor, obs); + for (const FloorObstacle* obs : floor->obstacles) { + checkObstacle(res, floor, obs); + } + + // check fingerprints + for (const FingerprintLocation* fpl : floor->fpLocations) { + checkFingerprintLoc(res, floor, fpl); + } + + // check stairs + for (const Stair* s : floor->stairs) { + checkStair(res, floor, s); } } + // done + return res; + } private: /** check a floor's outline */ - static void checkOutline(const Floor* floor, const FloorOutlinePolygon* poly) { + static void checkOutline(Issues& res, const Floor* floor, const FloorOutlinePolygon* poly) { // number of points valid? - if (poly->poly.points.size() < 3) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' needs at least 3 edges");} - if (poly->poly.points.size() > 1024) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' has too many edges");} + if (poly->poly.points.size() < 3) {res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' needs at least 3 edges"));} + if (poly->poly.points.size() > 1024) {res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' has too many edges"));} // outline size [bbox] valid? BBox2 outline; for (const Point2 pt : poly->poly.points) { outline.add(pt); } const Point2 size = outline.getSize(); - if (size.x < 1.0 || size.y < 1.0) {throw Exception("floor '" + floor->name + "' outline '" + poly->name + "' seems too small");} + if (size.x < 1.0 || size.y < 1.0) {res.push_back(Issue(Type::ERROR, floor, "' outline '" + poly->name + "' seems too small"));} } /** check walls, doors, ... */ - static void checkObstacle(const Floor* floor, const FloorObstacle* fo) { + static void checkObstacle(Issues& res, const Floor* floor, const FloorObstacle* fo) { // line? -> check const FloorObstacleLine* line = dynamic_cast(fo); if (line) { const float len_m = line->from.getDistance(line->to); if (len_m < 0.15) { - throw Exception("floor '" + floor->name + "' line-obstacle is too short: " + std::to_string(len_m) + " meter from " + line->from.asString() + " to " + line->to.asString()); + res.push_back(Issue(Type::WARN, floor, "' line-obstacle is too short: " + std::to_string(len_m) + " meter from " + line->from.asString() + " to " + line->to.asString())); } } @@ -65,10 +138,66 @@ namespace Floorplan { if (door) { const float len_m = door->from.getDistance(door->to); if (len_m < 0.40) { - throw Exception("floor '" + floor->name + "' door is too narrow: " + std::to_string(len_m) + " meter from " + door->from.asString() + " to " + door->to.asString()); + res.push_back(Issue(Type::ERROR, floor, "' door is too narrow: " + std::to_string(len_m) + " meter from " + door->from.asString() + " to " + door->to.asString())); } } + // pillar? -> check + const FloorObstacleCircle* circle = dynamic_cast(fo); + if (circle) { + const float len_m = circle->radius; + if (len_m < 0.20) { + res.push_back(Issue(Type::ERROR, floor, "' pillar is too narrow: " + std::to_string(len_m) + " meter at " + circle->center.asString())); + } + } + + } + + static void checkFingerprintLoc(Issues& res, const Floor* floor, const FingerprintLocation* fpl) { + + const std::string note = "does it belong to a stair? if so: fine! Note: fingerprints are currently measured using smartphones and smartphone are held by the pedestian. thus: fingerprints are ~1.3 meter above ground"; + if (fpl->heightAboveFloor < 0.8) { + res.push_back(Issue(Type::ERROR, floor, std::string() + "fingerprint " + fpl->name + " @ " + fpl->getPosition(*floor).asString() + " is too near to the floor. " + note)); + } else if (fpl->heightAboveFloor > 2.0) { + res.push_back(Issue(Type::WARN, floor, std::string() + "fingerprint " + fpl->name + " @ " + fpl->getPosition(*floor).asString() + " is too high above the floor. " + note)); + } + + } + + static void checkStair(Issues& res, const Floor* floor, const Stair* stair) { + + const std::vector parts = stair->getParts(); + const std::vector quads = Floorplan::getQuads(parts, floor); + + const Floorplan::Quad3& quadS = quads.front(); + const Floorplan::Quad3& quadE = quads.back(); + + // disconnected start? + if (quadS.p1.z != floor->getStartingZ()) { + res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the starting floor's ground! [open stair start]")); + } + + // disconnected end? + if (quadE.p3.z != floor->getEndingZ()) { + res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the ending floor's ground! [open stair end]")); + } + + + for (int i = 0; i < (int) parts.size(); ++i) { + + const Floorplan::Quad3& quad = quads[i]; + + // disconnected within? + if (i > 0) { + if (quads[i-1].p4.z != quads[i-0].p1.z) { + res.push_back(Issue(Type::ERROR, floor, "stair is disconnected within!")); + } + } + + } + + + } }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 4959145..b34d9ec 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -41,7 +41,6 @@ namespace Floorplan { ); } IndoorMap* map = parse(doc); - Floorplan::LINT::check(map); return map; } @@ -59,7 +58,6 @@ namespace Floorplan { ); } IndoorMap* map = parse(doc); - Floorplan::LINT::check(map); return map; } From 18f48e23a8927e2c03758900ae067efe6e441879 Mon Sep 17 00:00:00 2001 From: FrankE Date: Mon, 20 Mar 2017 19:24:29 +0100 Subject: [PATCH 23/43] fixed LINT issue --- floorplan/v2/FloorplanLINT.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index ebb5db9..34ca7b8 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -166,6 +166,11 @@ namespace Floorplan { static void checkStair(Issues& res, const Floor* floor, const Stair* stair) { + if (stair->getParts().empty()) { + res.push_back(Issue(Type::ERROR, floor, "stair does not contain any parts! [empty stair]")); + return; + } + const std::vector parts = stair->getParts(); const std::vector quads = Floorplan::getQuads(parts, floor); From bb43e7f0fef044c0212b4c8f7db08af6dc549d63 Mon Sep 17 00:00:00 2001 From: kazu Date: Tue, 21 Mar 2017 16:25:36 +0100 Subject: [PATCH 24/43] fixed some compiler warnings added equality checks to sensor-data classes more robust sensor reader [fixed some issues] added support for gps added support for compass added sensor-data-writer added test-cases minor changes --- geo/Point2.h | 1 + main.cpp | 2 +- math/filter/Butterworth.h | 4 +- sensors/gps/GPSData.h | 21 + sensors/imu/AccelerometerData.h | 24 +- sensors/imu/CompassData.h | 56 +++ sensors/imu/GravityData.h | 24 +- sensors/imu/GyroscopeData.h | 24 +- sensors/imu/LinearAccelerationData.h | 24 +- sensors/offline/FileReader.h | 422 ++++++++++-------- sensors/offline/FileWriter.h | 92 ++++ sensors/offline/Listener.h | 33 ++ sensors/offline/OfflineAndroid.h | 91 +++- sensors/offline/Sensors.h | 38 ++ sensors/offline/Splitter.h | 53 +++ sensors/pressure/BarometerData.h | 13 + sensors/radio/WiFiMeasurement.h | 4 +- .../radio/setup/WiFiOptimizerLogDistCeiling.h | 34 +- tests/sensors/imu/TestMotionDetection.cpp | 24 +- tests/sensors/offline/TestReadWrite.cpp | 89 ++++ 20 files changed, 807 insertions(+), 266 deletions(-) create mode 100644 sensors/imu/CompassData.h create mode 100644 sensors/offline/FileWriter.h create mode 100644 sensors/offline/Listener.h create mode 100644 sensors/offline/Sensors.h create mode 100644 sensors/offline/Splitter.h create mode 100644 tests/sensors/offline/TestReadWrite.cpp diff --git a/geo/Point2.h b/geo/Point2.h index 0d94f12..f28e23d 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -3,6 +3,7 @@ #include #include +#include /** * 2D Point diff --git a/main.cpp b/main.cpp index 52a00bc..de06c02 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*FloorplanCeilings*"; + ::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/filter/Butterworth.h b/math/filter/Butterworth.h index c41fc71..f83d4aa 100644 --- a/math/filter/Butterworth.h +++ b/math/filter/Butterworth.h @@ -77,9 +77,9 @@ namespace Filter { const Scalar _b0, _b1, _b2, _a1, _a2, _gain; - const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2; + Scalar _z1, _z2; - Scalar _z1, _z2; + const Scalar _preCompStateSpaceOutputVec1, _preCompStateSpaceOutputVec2; }; diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index 0f52963..e314d85 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -3,6 +3,7 @@ #include "../../data/Timestamp.h" + struct GPSData { /** time this measurement was received (NOT the GPS-time) */ @@ -15,12 +16,32 @@ struct GPSData { float accuracy; // m [might be NAN] float speed; // m/s [might be NAN] + /** ctor for invalid/unknown data */ GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;} + /** ctor */ 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) {;} + /** 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) {;} + /** data valid? */ + bool isValid() const { + return (lat == lat) && (lon == lon); + } + + bool operator == (const GPSData& o) const { + return EQ_OR_NAN(lat, o.lat) && + EQ_OR_NAN(lon, o.lon) && + EQ_OR_NAN(alt, o.alt) && + EQ_OR_NAN(accuracy, o.accuracy) && + EQ_OR_NAN(speed, o.speed); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + }; #endif // GPSDATA_H diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index be9f262..4a1566f 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -42,11 +42,25 @@ struct AccelerometerData { return AccelerometerData(x/val, y/val, z/val); } - std::string asString() const { - std::stringstream ss; - ss << "(" << x << "," << y << "," << z << ")"; - return ss.str(); - } + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + + bool isValid() const { + return (x == x) && (y == y) && (z == z); + } + + bool operator == (const AccelerometerData& o ) const { + return EQ_OR_NAN(x, o.x) && + EQ_OR_NAN(y, o.y) && + EQ_OR_NAN(z, o.z); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/CompassData.h b/sensors/imu/CompassData.h new file mode 100644 index 0000000..c336091 --- /dev/null +++ b/sensors/imu/CompassData.h @@ -0,0 +1,56 @@ +#ifndef COMPASSDATA_H +#define COMPASSDATA_H + + +#include +#include + + +/** data received from a compass sensor */ +struct CompassData { + + /** azimuth angle. NAN if not available */ + float azimuth = NAN; + + /** describes the sensor's quality */ + float quality01 = 0; + + + /** empty ctor */ + CompassData() : azimuth(NAN) {;} + + /** data ctor */ + CompassData(const float azimuth) : azimuth(azimuth), quality01(0) {;} + + /** data ctor */ + CompassData(const float azimuth, const float quality01) : azimuth(azimuth), quality01(quality01) {;} + + /** get an instance describing invalid data */ + static CompassData INVALID() { + return CompassData(NAN); + } + + /** convert to string */ + std::string asString() const { + std::stringstream ss; + ss << "(" << azimuth << ")"; + return ss.str(); + } + + /** is the compass data valid? [compass present] */ + bool isValid() const { + return azimuth == azimuth; + } + + bool operator == (const CompassData& o) const { + return EQ_OR_NAN(azimuth, o.azimuth) && + EQ_OR_NAN(quality01, o.quality01); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + +}; + +#endif // COMPASSDATA_H diff --git a/sensors/imu/GravityData.h b/sensors/imu/GravityData.h index d522f20..3d87b0b 100644 --- a/sensors/imu/GravityData.h +++ b/sensors/imu/GravityData.h @@ -42,11 +42,25 @@ struct GravityData { return GravityData(x/val, y/val, z/val); } - std::string asString() const { - std::stringstream ss; - ss << "(" << x << "," << y << "," << z << ")"; - return ss.str(); - } + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + + bool isValid() const { + return (x == x) && (y == y) && (z == z); + } + + bool operator == (const GravityData& o ) const { + return EQ_OR_NAN(x, o.x) && + EQ_OR_NAN(y, o.y) && + EQ_OR_NAN(z, o.z); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/GyroscopeData.h b/sensors/imu/GyroscopeData.h index de7e725..d4f0ee3 100644 --- a/sensors/imu/GyroscopeData.h +++ b/sensors/imu/GyroscopeData.h @@ -23,11 +23,25 @@ struct GyroscopeData { return std::sqrt( x*x + y*y + z*z ); } - std::string asString() const { - std::stringstream ss; - ss << "(" << x << "," << y << "," << z << ")"; - return ss.str(); - } + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + + bool isValid() const { + return (x == x) && (y == y) && (z == z); + } + + bool operator == (const GyroscopeData& o ) const { + return EQ_OR_NAN(x, o.x) && + EQ_OR_NAN(y, o.y) && + EQ_OR_NAN(z, o.z); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/imu/LinearAccelerationData.h b/sensors/imu/LinearAccelerationData.h index 9ae116c..df6962e 100644 --- a/sensors/imu/LinearAccelerationData.h +++ b/sensors/imu/LinearAccelerationData.h @@ -42,11 +42,25 @@ struct LinearAccelerationData { return LinearAccelerationData(x/val, y/val, z/val); } - std::string asString() const { - std::stringstream ss; - ss << "(" << x << "," << y << "," << z << ")"; - return ss.str(); - } + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + + bool isValid() const { + return (x == x) && (y == y) && (z == z); + } + + bool operator == (const LinearAccelerationData& o ) const { + return EQ_OR_NAN(x, o.x) && + EQ_OR_NAN(y, o.y) && + EQ_OR_NAN(z, o.z); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} }; diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index bc30410..99d0fd4 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -14,286 +14,320 @@ #include "../../sensors/imu/GravityData.h" #include "../../sensors/imu/LinearAccelerationData.h" #include "../../sensors/beacon/BeaconMeasurements.h" - +#include "../../sensors/gps/GPSData.h" +#include "../../sensors/imu/CompassData.h" #include "../../geo/Point2.h" #include "../../grid/factory/v2/GridFactory.h" #include "../../grid/factory/v2/Importance.h" #include "../../floorplan/v2/Floorplan.h" -class FileReader { +#include "Splitter.h" +#include "Sensors.h" -public: +#warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]" - template struct TS { - const uint64_t ts; - T data; - TS(const uint64_t ts) : ts(ts) {;} - TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;} - }; +namespace Offline { - enum class Sensor { - ACC, - GYRO, - WIFI, - POS, - BARO, - BEACON, - LIN_ACC, - GRAVITY, - }; + class FileReader { - /** entry for one sensor */ - struct Entry { - Sensor type; - uint64_t ts; - int idx; - Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;} - }; + public: - std::vector> groundTruth; - std::vector> wifi; - std::vector> beacon; - std::vector> acc; - std::vector> gyro; - std::vector> barometer; - std::vector> lin_acc; - std::vector> gravity; + std::vector> groundTruth; + std::vector> wifi; + std::vector> beacon; + std::vector> acc; + std::vector> gyro; + std::vector> barometer; + std::vector> lin_acc; + std::vector> gravity; + std::vector> gps; + std::vector> compass; - /** ALL entries */ - std::vector entries; + /** ALL entries */ + std::vector entries; -public: + static constexpr char sep = ';'; - FileReader(const std::string& file) { - parse(file); - } + public: - const std::vector& getEntries() const {return entries;} + /** empty ctor. call open() */ + FileReader() { + ; + } + /** ctor with filename */ + FileReader(const std::string& file) { + open(file); + } - const std::vector>& getGroundTruth() const {return groundTruth;} + /** open the given file */ + void open(const std::string& file) { + parse(file); + } - const std::vector>& getWiFiGroupedByTime() const {return wifi;} + const std::vector& getEntries() const {return entries;} - const std::vector>& getBeacons() const {return beacon;} - const std::vector>& getAccelerometer() const {return acc;} + const std::vector>& getGroundTruth() const {return groundTruth;} - const std::vector>& getGyroscope() const {return gyro;} + const std::vector>& getWiFiGroupedByTime() const {return wifi;} - const std::vector>& getBarometer() const {return barometer;} + const std::vector>& getBeacons() const {return beacon;} - const std::vector>& getLinearAcceleration() const {return lin_acc;} + const std::vector>& getAccelerometer() const {return acc;} - const std::vector>& getGravity() const {return gravity;} + const std::vector>& getGyroscope() const {return gyro;} -private: + const std::vector>& getGPS() const {return gps;} - void parse(const std::string& file) { + const std::vector>& getCompass() const {return compass;} - std::ifstream inp(file); - if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} + const std::vector>& getBarometer() const {return barometer;} - while(!inp.eof() && !inp.bad()) { + const std::vector>& getLinearAcceleration() const {return lin_acc;} - uint64_t ts; - char delim; - int idx = -1; - std::string data; + const std::vector>& getGravity() const {return gravity;} - inp >> ts; - inp >> delim; - inp >> idx; - inp >> delim; - inp >> data; + private: - if (idx == 8) {parseWiFi(ts, data);} - else if (idx == 9) {parseBeacons(ts, data);} - else if (idx == 99) {parseGroundTruth(ts, data);} - else if (idx == 0) {parseAccelerometer(ts, data);} - else if (idx == 3) {parseGyroscope(ts, data);} - else if (idx == 5) {parseBarometer(ts, data);} - else if (idx == 2) {parseLinearAcceleration(ts,data);} - else if (idx == 1) {parseGravity(ts,data);} + void parse(const std::string& file) { - // TODO: this is a hack... - // the loop is called one additional time after the last entry - // and keeps the entries of entry + std::ifstream inp(file); + if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} - } + while(!inp.eof() && !inp.bad()) { - inp.close(); + uint64_t ts; + char delim; + int idx = -1; + std::string data; - } + inp >> ts; + inp >> delim; + inp >> idx; + inp >> delim; + inp >> data; - void parseLinearAcceleration(const uint64_t ts, const std::string& data){ + if (idx == (int)Sensor::WIFI) {parseWiFi(ts, data);} + else if (idx == (int)Sensor::BEACON) {parseBeacons(ts, data);} + else if (idx == (int)Sensor::GROUND_TRUTH) {parseGroundTruth(ts, data);} + else if (idx == (int)Sensor::ACC) {parseAccelerometer(ts, data);} + else if (idx == (int)Sensor::GYRO) {parseGyroscope(ts, data);} + else if (idx == (int)Sensor::BARO) {parseBarometer(ts, data);} + else if (idx == (int)Sensor::LIN_ACC) {parseLinearAcceleration(ts,data);} + else if (idx == (int)Sensor::GRAVITY) {parseGravity(ts,data);} + else if (idx == (int)Sensor::COMPASS) {parseCompass(ts,data);} + else if (idx == (int)Sensor::GPS) {parseGPS(ts,data);} - const auto pos1 = data.find(';'); - const auto pos2 = data.find(';', pos1+1); + // TODO: this is a hack... + // the loop is called one additional time after the last entry + // and keeps the entries of entry - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); + } - TS elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z))); - lin_acc.push_back(elem); - entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1)); - } + inp.close(); - void parseGravity(const uint64_t ts, const std::string& data){ + } - const auto pos1 = data.find(';'); - const auto pos2 = data.find(';', pos1+1); + void parseLinearAcceleration(const uint64_t ts, const std::string& data){ - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); - TS elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); - gravity.push_back(elem); - entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); - } + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); - void parseAccelerometer(const uint64_t ts, const std::string& data) { + TS elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z))); + lin_acc.push_back(elem); + entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1)); + } - const auto pos1 = data.find(';'); - const auto pos2 = data.find(';', pos1+1); + void parseGravity(const uint64_t ts, const std::string& data){ - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); - TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); - acc.push_back(elem); - entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); - } + TS elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); + gravity.push_back(elem); + entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); + } - void parseGyroscope(const uint64_t ts, const std::string& data) { + void parseAccelerometer(const uint64_t ts, const std::string& data) { - const auto pos1 = data.find(';'); - const auto pos2 = data.find(';', pos1+1); + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); - TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); - gyro.push_back(elem); - entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); + TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); + acc.push_back(elem); + entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); - } + } - void parseWiFi(const uint64_t ts, const std::string& data) { + void parseGyroscope(const uint64_t ts, const std::string& data) { - std::string tmp = data; + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); - // add new wifi reading - wifi.push_back(TS(ts, WiFiMeasurements())); - entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1)); + const std::string x = data.substr(0, pos1); + const std::string y = data.substr(pos1+1, pos2-pos1-1); + const std::string z = data.substr(pos2+1); - // process all APs - while(!tmp.empty()) { + TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); + gyro.push_back(elem); + entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); - auto pos1 = tmp.find(';'); - auto pos2 = tmp.find(';', pos1+1); - auto pos3 = tmp.find(';', pos2+1); + } - std::string mac = tmp.substr(0, pos1); - std::string freq = tmp.substr(pos1+1, pos2); - std::string rssi = tmp.substr(pos2+1, pos3); + void parseWiFi(const uint64_t ts, const std::string& data) { - tmp = tmp.substr(pos3); - assert(tmp[0] == ';'); tmp = tmp.substr(1); + WiFiMeasurements wifi; + Splitter s(data, sep); - // append AP to current scan-entry - WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts)); - wifi.back().data.entries.push_back(e); - } + for (size_t i = 0; i < s.size(); i += 3) { - } + const std::string mac = s.get(i+0); + const float freq = s.getFloat(i+1); + const float rssi = s.getFloat(i+2); - void parseBeacons(const uint64_t ts, const std::string& data) { + // append AP to current scan-entry + WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts)); + wifi.entries.push_back(e); - const auto pos1 = data.find(';'); - const auto pos2 = data.find(';', pos1+1); - const auto pos3 = data.find(';', pos2+1); + } - const std::string mac = data.substr(0, pos1); - const std::string rssi = data.substr(pos1+1, pos2); - const std::string txp = data.substr(pos2+1, pos3); + // add new wifi reading + this->wifi.push_back(TS(ts, wifi)); + entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1)); - //yes the timestamp is redundant here, but in case of multiusage... - TS e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi))); - beacon.push_back(e); - entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1)); + } - } + void parseBeacons(const uint64_t ts, const std::string& data) { - void parseGroundTruth(const uint64_t ts, const std::string& data) { + const auto pos1 = data.find(';'); + const auto pos2 = data.find(';', pos1+1); + const auto pos3 = data.find(';', pos2+1); - const auto pos1 = data.find(';'); - std::string gtIndex = data.substr(0, pos1); + const std::string mac = data.substr(0, pos1); + const std::string rssi = data.substr(pos1+1, pos2); + const std::string txp = data.substr(pos2+1, pos3); - TS elem(ts, std::stoi(gtIndex)); - groundTruth.push_back(elem); + //yes the timestamp is redundant here, but in case of multiusage... + TS e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi))); + beacon.push_back(e); + entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1)); - } + } - void parseBarometer(const uint64_t ts, const std::string& data) { + void parseGroundTruth(const uint64_t ts, const std::string& data) { - const auto pos1 = data.find(';'); + const auto pos1 = data.find(';'); + std::string gtIndex = data.substr(0, pos1); - const std::string hPa = data.substr(0, pos1); + TS elem(ts, std::stoi(gtIndex)); + groundTruth.push_back(elem); - TS elem(ts, BarometerData(std::stof(hPa))); - barometer.push_back(elem); - entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + } - } + void parseBarometer(const uint64_t ts, const std::string& data) { -public: - const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { + BarometerData baro; + Splitter s(data, sep); - // finde alle positionen der waypoints im gtPath aus map - std::unordered_map waypointsMap; - for(Floorplan::Floor* f : map->floors){ - float h = f->atHeight; - for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){ + baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN); - //wenn die gleiche id 2x vergeben wurde, knallt es - if(waypointsMap.find(gtp->id) == waypointsMap.end()){ - waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)}); - } - else{ - throw std::string("the floorplan's ground truth contains two points with identical id's!"); - } + TS elem(ts, baro); + barometer.push_back(elem); + entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); - } - } + } - // bringe diese in richtige reihenfolge und füge timestamp hinzu - Interpolator interpol; + void parseCompass(const uint64_t ts, const std::string& data) { - int it = 0; - for(int id : gtPath){ - auto itMap = waypointsMap.find(id); - if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");} + CompassData compass; + Splitter s(data, sep); - //the time, when the gt button was clicked on the app - uint64_t tsGT = groundTruth[it++].ts; - interpol.add(tsGT, itMap->second); + compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN); + compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN); - } + TS elem(ts, compass); + this->compass.push_back(elem); + entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1)); - if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){ - throw std::string("No Ground Truth points found within the map.xml file"); - } + } - return interpol; - } + /** parse the given GPS entry */ + void parseGPS(const uint64_t ts, const std::string& data) { -}; + GPSData gps; + Splitter s(data, sep); + + gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN); + gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN); + gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN); + gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN); + gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN); + + TS elem(ts, gps); + this->gps.push_back(elem); + entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1)); + + + } + + + public: + const Interpolator getGroundTruthPath(Floorplan::IndoorMap* map, std::vector gtPath) const { + + // finde alle positionen der waypoints im gtPath aus map + std::unordered_map waypointsMap; + for(Floorplan::Floor* f : map->floors){ + float h = f->atHeight; + for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){ + + //wenn die gleiche id 2x vergeben wurde, knallt es + if(waypointsMap.find(gtp->id) == waypointsMap.end()){ + waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)}); + } + else{ + throw std::string("the floorplan's ground truth contains two points with identical id's!"); + } + + } + } + + // bringe diese in richtige reihenfolge und füge timestamp hinzu + Interpolator interpol; + + int it = 0; + for(int id : gtPath){ + auto itMap = waypointsMap.find(id); + if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");} + + //the time, when the gt button was clicked on the app + uint64_t tsGT = groundTruth[it++].ts; + interpol.add(tsGT, itMap->second); + + } + + if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){ + throw std::string("No Ground Truth points found within the map.xml file"); + } + + return interpol; + } + + }; + +} #endif // FILEREADER_H diff --git a/sensors/offline/FileWriter.h b/sensors/offline/FileWriter.h new file mode 100644 index 0000000..4e2f97e --- /dev/null +++ b/sensors/offline/FileWriter.h @@ -0,0 +1,92 @@ +#ifndef FILEWRITER_H +#define FILEWRITER_H + +#include "../gps/GPSData.h" +#include "../imu/CompassData.h" +#include "../imu/LinearAccelerationData.h" +#include "../imu/GravityData.h" +#include "../pressure/BarometerData.h" +#include "../imu/GyroscopeData.h" +#include "../imu/AccelerometerData.h" +#include "../radio/WiFiMeasurements.h" +#include "Sensors.h" + +#include + +namespace Offline { + + class FileWriter { + + private: + + std::ofstream out; + static constexpr char sep = ';'; + const std::string nl = "\n"; + + public: + + FileWriter() { + ; + } + + ~FileWriter() { + close(); + } + + void open(const std::string& file) { + out.open(file); + if (!out) {throw Exception("error opening file: " + file);} + } + + void close() { + out.flush(); + out.close(); + } + + void flush() { + out.flush(); + } + + void add(const Timestamp ts, const AccelerometerData& data) { + out << ts.ms() << sep << (int) Sensor::ACC << sep << data.x << sep << data.y << sep << data.z << nl; + } + + void add(const Timestamp ts, const LinearAccelerationData& data) { + out << ts.ms() << sep << (int) Sensor::LIN_ACC << sep << data.x << sep << data.y << sep << data.z << nl; + } + + void add(const Timestamp ts, const GravityData& data) { + out << ts.ms() << sep << (int) Sensor::GRAVITY << sep << data.x << sep << data.y << sep << data.z << nl; + } + + void add(const Timestamp ts, const GyroscopeData& data) { + out << ts.ms() << sep << (int) Sensor::GYRO << sep << data.x << sep << data.y << sep << data.z << nl; + } + + void add(const Timestamp ts, const BarometerData& data) { + out << ts.ms() << sep << (int) Sensor::BARO << sep << data.hPa << nl; + } + + void add(const Timestamp ts, const GPSData& data) { + out << ts.ms() << sep << (int) Sensor::GPS << sep << data.lat << sep << data.lon << sep << data.alt << sep << data.accuracy << sep << data.speed << nl; + } + + void add(const Timestamp ts, const CompassData& data) { + out << ts.ms() << sep << (int) Sensor::COMPASS << sep << data.azimuth << sep << data.quality01 << nl; + } + + void add(const Timestamp ts, const WiFiMeasurements& data) { + out << ts.ms() << sep << (int) Sensor::WIFI; + for (const WiFiMeasurement& m : data.entries) { + out << sep << m.getAP().getMAC().asString(); + out << sep << m.getFrequency(); + out << sep << m.getRSSI(); + } + out << "\n"; + } + + }; + +} + +#endif // FILEWRITER_H diff --git a/sensors/offline/Listener.h b/sensors/offline/Listener.h new file mode 100644 index 0000000..d9be0a6 --- /dev/null +++ b/sensors/offline/Listener.h @@ -0,0 +1,33 @@ +#ifndef OFFLINE_LISTENER_H +#define OFFLINE_LISTENER_H + +#include "../gps/GPSData.h" +#include "../imu/CompassData.h" +#include "../imu/GravityData.h" +#include "../pressure/BarometerData.h" +#include "../imu/GyroscopeData.h" +#include "../imu/AccelerometerData.h" +#include "../radio/WiFiMeasurements.h" + +namespace Offline { + + /** + * listen for events/callbacks while parsing offline files + */ + class Listener { + + public: + + virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0; + 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; + virtual void onGPS(const Timestamp ts, const GPSData data) = 0; + virtual void onCompass(const Timestamp ts, const CompassData data) = 0; + + }; + +} + +#endif // OFFLINE_LISTENER_H diff --git a/sensors/offline/OfflineAndroid.h b/sensors/offline/OfflineAndroid.h index 6d8801d..19049b1 100644 --- a/sensors/offline/OfflineAndroid.h +++ b/sensors/offline/OfflineAndroid.h @@ -12,8 +12,14 @@ #include "../radio/WiFiMeasurements.h" #include "../imu/AccelerometerData.h" #include "../imu/GyroscopeData.h" +#include "../imu/CompassData.h" +#include "../gps/GPSData.h" #include "../pressure/BarometerData.h" +#include "Splitter.h" +#include "Listener.h" +#include "Sensors.h" + template struct OfflineEntry { Timestamp ts; @@ -35,20 +41,12 @@ struct WalkedPath { }; -/** listener for event callbacks */ -class OfflineAndroidListener { -public: - virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0; - 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 */ +/** + * read sensor data files that were recorded using + * the old java android app + */ class OfflineAndroid { - private: std::vector> wifi; @@ -57,11 +55,15 @@ private: std::vector> accel; std::vector> gravity; + std::vector> compass; std::vector> barometer; + std::vector> gps; + WalkedPath walkedPath; + static constexpr char sep = ';'; const char* name = "OfflineData"; public: @@ -89,6 +91,12 @@ public: /** get all barometer readings */ const std::vector>& getBarometer() const {return barometer;} + /** get all compass readings */ + const std::vector>& getCompass() const {return compass;} + + /** get all gps readings */ + const std::vector>& getGPS() const {return gps;} + /** get the walked path */ const WalkedPath& getWalkedPath() const {return walkedPath;} @@ -105,7 +113,7 @@ public: public: - void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) { + void parse(const std::string& file, Offline::Listener* listener = nullptr) { Log::add(name, "parsing data file: " + file , false); Log::tick(); @@ -152,47 +160,61 @@ public: private: /** parse the given data */ - void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) { + void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, Offline::Listener* listener) { // how to parse switch(sensorID) { - case 0: { + case (int) Offline::Sensor::ACC: { const AccelerometerData data = parseAccelerometer(sensorData); accel.push_back(OfflineEntry(ts, data)); if (listener) {listener->onAccelerometer(ts, data);} break; } - case 1: { + case (int) Offline::Sensor::GRAVITY: { const AccelerometerData data = parseAccelerometer(sensorData); gravity.push_back(OfflineEntry(ts, data)); if (listener) {listener->onGravity(ts, data);} break; } - case 3: { + case (int) Offline::Sensor::GYRO: { const GyroscopeData data = parseGyroscope(sensorData); gyro.push_back(OfflineEntry(ts, data)); if (listener) {listener->onGyroscope(ts, data);} break; } - case 5: { + case (int) Offline::Sensor::BARO: { const BarometerData data = parseBarometer(sensorData); barometer.push_back(OfflineEntry(ts, data)); if (listener) {listener->onBarometer(ts, data);} break; } - case 8: { + case (int) Offline::Sensor::WIFI: { const WiFiMeasurements data = parseWiFi(ts, sensorData); wifi.push_back(OfflineEntry(ts, data)); if (listener) {listener->onWiFi(ts, data);} break; } - case 99: { + case (int) Offline::Sensor::COMPASS: { + const CompassData data = parseCompass(sensorData); + compass.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onCompass(ts, data);} + break; + } + + case (int) Offline::Sensor::GPS: { + const GPSData data = parseGPS(sensorData); + gps.push_back(OfflineEntry(ts, data)); + if (listener) {listener->onGPS(ts, data);} + break; + } + + case (int) Offline::Sensor::GROUND_TRUTH: { const GroundTruthID data = parseGroundTruthTick(sensorData); groundTruth.push_back(OfflineEntry(ts, data)); // TODO listener @@ -326,6 +348,35 @@ private: } + /** parse the given Compass entry */ + static inline CompassData parseCompass(const std::string& data) { + + CompassData compass; + Splitter s(data, sep); + + compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN); + compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN); + + return compass; + + } + + /** parse the given GPS entry */ + static inline GPSData parseGPS(const std::string& data) { + + GPSData gps; + Splitter s(data, sep); + + gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN); + gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN); + gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN); + gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN); + gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN); + + return gps; + + } + }; #endif // OFFLINEANDROID_H diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h new file mode 100644 index 0000000..47ffec6 --- /dev/null +++ b/sensors/offline/Sensors.h @@ -0,0 +1,38 @@ +#ifndef OFFLINE_SENSORS_H +#define OFFLINE_SENSORS_H + +namespace Offline { + + enum class Sensor { + ACC = 0, + GRAVITY = 1, + LIN_ACC = 2, + GYRO = 3, + BARO = 5, + WIFI = 8, + BEACON = 9, + COMPASS = 15, + GPS = 16, + GROUND_TRUTH = 99, + POS = 1001, // IPIN2016 + }; + + template struct TS { + const uint64_t ts; + T data; + TS(const uint64_t ts) : ts(ts) {;} + TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;} + }; + + /** entry for one sensor */ + struct Entry { + Sensor type; + uint64_t ts; + int idx; + Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;} + }; + + +} + +#endif // OFFLINE_SENSORS_H diff --git a/sensors/offline/Splitter.h b/sensors/offline/Splitter.h new file mode 100644 index 0000000..ae09d43 --- /dev/null +++ b/sensors/offline/Splitter.h @@ -0,0 +1,53 @@ +#ifndef DATA_SPLITTER_H +#define DATA_SPLITTER_H + +#include +#include + +/** + * split an input-file into various tokens + */ +class Splitter { + + std::string str; + char sep = ';'; + std::vector split; + +public: + + /** ctor */ + Splitter(const std::string& str, const char sep = ';') : str(str), sep(sep) { + build(); + } + + bool has(const int idx) const {return split.size() > idx;} + + const std::string& get(const int idx) const {return split.at(idx);} + + const float getFloat(const int idx) const {return std::stof(get(idx));} + + size_t size() const {return split.size();} + +private: + + void build() { + + std::string cur; + + for (char c : str) { + if (c == sep) { + split.push_back(cur); + cur = ""; + } else { + cur += c; + } + } + + split.push_back(cur); + + } + +}; + + +#endif // DATA_SPLITTER_H diff --git a/sensors/pressure/BarometerData.h b/sensors/pressure/BarometerData.h index b9e6e6d..23d17c6 100644 --- a/sensors/pressure/BarometerData.h +++ b/sensors/pressure/BarometerData.h @@ -13,6 +13,19 @@ struct BarometerData { explicit BarometerData(const float hPa) : hPa(hPa) {;} + /** valid data? */ + bool isValid() const { + return hPa == hPa; + } + + bool operator == (const BarometerData& o ) const { + return EQ_OR_NAN(hPa, o.hPa); + } + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + }; #endif // BAROMETERDATA_H diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index d84327d..e8d5d81 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -20,7 +20,7 @@ private: float rssi; /** OPTIONAL. frequence the signal was received */ - float freq; + float freq = NAN; /** OPTIONAL. timestamp the measurement was recorded at */ Timestamp ts; @@ -28,7 +28,7 @@ private: public: /** ctor */ - WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) { + WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi), freq(NAN) { ; } diff --git a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h index 8bd3e42..d093b94 100644 --- a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h +++ b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h @@ -94,6 +94,15 @@ namespace WiFiOptimizer { return ss.str(); } + /** we add some constraints to the parameter range */ + bool outOfRange() const { + return (waf > 0) || + (txp < -50) || + (txp > -30) || + (exp > 4) || + (exp < 1); + } + }; /** add MAC-info to params */ @@ -136,17 +145,17 @@ namespace WiFiOptimizer { return false; }; - const APFilter MIN_8_FPS = [] (const int numFingerprints, const MACAddress& mac) { + const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) { (void) mac; - return numFingerprints < 8; + return numFingerprints < 5; }; private: - Mode mode = Mode::QUALITY; - Floorplan::IndoorMap* map; + Mode mode = Mode::QUALITY; + const char* name = "WiFiOptLDC"; public: @@ -182,6 +191,7 @@ namespace WiFiOptimizer { } const float avgErr = errSum / errCnt; + Log::add(name, "optimized APs: " + std::to_string(errCnt)); Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB"); // done @@ -189,6 +199,7 @@ namespace WiFiOptimizer { } + /** optimize the given AP */ APParams optimize(const MACAddress& mac, Stats& res) const { @@ -210,8 +221,8 @@ namespace WiFiOptimizer { 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, 5), // exp + LeOpt::MinMax(-50, -30), // txp + LeOpt::MinMax(1, 4), // exp LeOpt::MinMax(-15, -0), // waf }; @@ -271,17 +282,10 @@ namespace WiFiOptimizer { float getErrorLogDistCeiling(const MACAddress& mac, const std::vector& entries, const float* data, Stats* stats = nullptr) const { - constexpr float hugeError = 1e10; 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;} + if (params->outOfRange()) {return 1e10;} // current position guess for the AP; const Point3 apPos_m = params->getPos(); @@ -309,7 +313,7 @@ namespace WiFiOptimizer { } // adjust the error - err += diff*diff; + err += std::pow(std::abs(diff), 2.0); ++cnt; // max distance penality diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp index 47bc411..eb76a40 100644 --- a/tests/sensors/imu/TestMotionDetection.cpp +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -38,7 +38,7 @@ TEST(MotionDetection, motionAxis) { //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. //std::string filename = getDataFile("motion/table_flat.csv"); - FileReader fr(filename); + Offline::FileReader fr(filename); K::Gnuplot gp; K::GnuplotPlot plot; @@ -52,14 +52,14 @@ TEST(MotionDetection, motionAxis) { Timestamp lastTs; //calc motion axis - for (const FileReader::Entry& e : fr.getEntries()) { + for (const Offline::Entry& e : fr.getEntries()) { ts = Timestamp::fromMS(e.ts); - if (e.type == FileReader::Sensor::LIN_ACC) { + if (e.type == Offline::Sensor::LIN_ACC) { md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); - } else if (e.type == FileReader::Sensor::GRAVITY) { + } else if (e.type == Offline::Sensor::GRAVITY) { md.addGravity(ts, fr.getGravity()[e.idx].data); curVec = md.getCurrentMotionAxis(); motionAxisAngleRad = md.getMotionChangeInRad(); @@ -126,7 +126,7 @@ TEST(MotionDetection, motionAngle) { //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. //std::string filename = getDataFile("motion/table_flat.csv"); - FileReader fr(filename); + Offline::FileReader fr(filename); Timestamp ts; //save for later plotting @@ -134,23 +134,23 @@ TEST(MotionDetection, motionAngle) { std::vector delta_turnAngles; //calc motion axis - for (const FileReader::Entry& e : fr.getEntries()) { + for (const Offline::Entry& e : fr.getEntries()) { ts = Timestamp::fromMS(e.ts); - if (e.type == FileReader::Sensor::LIN_ACC) { + if (e.type == Offline::Sensor::LIN_ACC) { md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); - } else if (e.type == FileReader::Sensor::GRAVITY) { + } else if (e.type == Offline::Sensor::GRAVITY) { md.addGravity(ts, fr.getGravity()[e.idx].data); delta_motionAngles.push_back(md.getMotionChangeInRad()); - } else if (e.type == FileReader::Sensor::ACC) { - const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; + } else if (e.type == Offline::Sensor::ACC) { + const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; td.addAccelerometer(ts, _acc.data); - } else if (e.type == FileReader::Sensor::GYRO) { - const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); } diff --git a/tests/sensors/offline/TestReadWrite.cpp b/tests/sensors/offline/TestReadWrite.cpp new file mode 100644 index 0000000..c13c6f2 --- /dev/null +++ b/tests/sensors/offline/TestReadWrite.cpp @@ -0,0 +1,89 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/offline/FileReader.h" +#include "../../../sensors/offline/FileWriter.h" + +TEST(Offline, readWrite) { + + std::string fileName = "/tmp/test.dat"; + + Offline::FileWriter out; + out.open(fileName); + + const GPSData gps(Timestamp::fromMS(1), 2, 3, 4); + out.add(Timestamp::fromMS(11), gps); + + const CompassData compass(4, 2); + out.add(Timestamp::fromMS(13), compass); + + const BarometerData baro(3); + out.add(Timestamp::fromMS(15), baro); + + const AccelerometerData acc(3,4,5); + out.add(Timestamp::fromMS(17), acc); + + const GravityData grav(5,9,7); + out.add(Timestamp::fromMS(19), grav); + + const GyroscopeData gyro(8, 5,11); + out.add(Timestamp::fromMS(21), gyro); + + const LinearAccelerationData lina(13, 12, 11); + out.add(Timestamp::fromMS(23), lina); + + WiFiMeasurements w1; + w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:66")), -70)); + w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:67")), -72)); + w1.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:55:68")), -74)); + out.add(Timestamp::fromMS(25), w1); + + WiFiMeasurements w2; + w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:66")), -60)); + w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:67")), -62)); + w2.entries.push_back(WiFiMeasurement(AccessPoint(MACAddress("11:22:33:44:aa:68")), -64)); + out.add(Timestamp::fromMS(27), w2); + + out.close(); + + Offline::FileReader reader; + reader.open(fileName); + + // check number of entries + ASSERT_EQ(1, reader.getGPS().size()); + ASSERT_EQ(1, reader.getCompass().size()); + ASSERT_EQ(1, reader.getBarometer().size()); + ASSERT_EQ(1, reader.getAccelerometer().size()); + ASSERT_EQ(1, reader.getGravity().size()); + ASSERT_EQ(1, reader.getGyroscope().size()); + ASSERT_EQ(1, reader.getLinearAcceleration().size()); + ASSERT_EQ(2, reader.getWiFiGroupedByTime().size()); + + // check timestamps + ASSERT_EQ(11, reader.getGPS().front().ts); + ASSERT_EQ(13, reader.getCompass().front().ts); + ASSERT_EQ(15, reader.getBarometer().front().ts); + ASSERT_EQ(17, reader.getAccelerometer().front().ts); + ASSERT_EQ(19, reader.getGravity().front().ts); + ASSERT_EQ(21, reader.getGyroscope().front().ts); + ASSERT_EQ(23, reader.getLinearAcceleration().front().ts); + ASSERT_EQ(25, reader.getWiFiGroupedByTime().front().ts); + ASSERT_EQ(27, reader.getWiFiGroupedByTime().back().ts); + + // check content + ASSERT_EQ(gps, reader.getGPS().front().data); + ASSERT_EQ(compass, reader.getCompass().front().data); + ASSERT_EQ(baro, reader.getBarometer().front().data); + ASSERT_EQ(acc, reader.getAccelerometer().front().data); + ASSERT_EQ(grav, reader.getGravity().front().data); + ASSERT_EQ(gyro, reader.getGyroscope().front().data); + ASSERT_EQ(lina, reader.getLinearAcceleration().front().data); + + int i = 0; (void) i; + +} + + + +#endif From 991f42060c4256cef7f7e77cf96cd44136779dff Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 21 Mar 2017 17:20:55 +0100 Subject: [PATCH 25/43] added some asserts --- main.cpp | 2 +- math/divergence/KullbackLeibler.h | 6 ++++++ tests/math/divergence/TestKullbackLeibler.cpp | 2 +- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/main.cpp b/main.cpp index 52a00bc..b103b91 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*FloorplanCeilings*"; + ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h index 357b1c3..1ae4433 100644 --- a/math/divergence/KullbackLeibler.h +++ b/math/divergence/KullbackLeibler.h @@ -53,6 +53,10 @@ namespace Divergence { auto log1 = std::log(det1); auto log2 = std::log(det2); + //determinate shouldn't be 0! + Assert::isNot0(det1, "Determinat of the first Gauss is Zero! Check the Cov Matrix."); + Assert::isNot0(det2, "Determinat of the second Gauss is Zero! Check the Cov Matrix."); + //trace Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols()); toTrace = norm2.getSigmaInv() * norm1.getSigma(); @@ -74,6 +78,8 @@ namespace Divergence { if(klb < 0.0){ Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); } + Assert::isNotNaN(klb, "The Kullback Leibler Distance is NaN!"); + return klb; } diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp index 0a14f92..0a899f1 100644 --- a/tests/math/divergence/TestKullbackLeibler.cpp +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -201,7 +201,7 @@ TEST(KullbackLeibler, multivariateGaussGeCov) { double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); - std::cout << kld34 << " >" << kld12 << std::endl; + std::cout << kld34 << " > " << kld12 << std::endl; double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); From b03804d378010fee43a5a8f03bf964fe9ecfc2ab Mon Sep 17 00:00:00 2001 From: FrankE Date: Tue, 21 Mar 2017 21:11:58 +0100 Subject: [PATCH 26/43] added earth-registration load/save --- floorplan/v2/Floorplan.h | 5 ++++- floorplan/v2/FloorplanReader.h | 26 +++++++++++++++++++++++++- floorplan/v2/FloorplanWriter.h | 23 +++++++++++++++++++++++ geo/EarthPos.h | 5 +++++ 4 files changed, 57 insertions(+), 2 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 0a71fb0..1f62b72 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -545,6 +545,9 @@ namespace Floorplan { Point3 posOnMap_m; + /** empty ctor */ + EarthPosMapPos() : posOnEarth(), posOnMap_m() {;} + /** ctor */ EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;} @@ -555,7 +558,7 @@ namespace Floorplan { struct EarthRegistration { /** all available correspondences: earth <-> map */ - std::vector correspondences; + std::vector correspondences; }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 7c2f397..63a6ac7 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -81,12 +81,36 @@ namespace Floorplan { IndoorMap* map = new IndoorMap(); map->width = el->FloatAttribute("width"); map->depth = el->FloatAttribute("depth"); + FOREACH_NODE(n, el) { - if (std::string("floors") == n->Name()) {map->floors = parseFloors(n);} + if (std::string("floors") == n->Name()) {map->floors = parseFloors(n);} + if (std::string("earthReg") == n->Name()) {map->earthReg = parseEarthReg(n);} } return map; } + /** parse the node */ + static EarthRegistration parseEarthReg(const XMLElem* el) { + EarthRegistration reg; + FOREACH_NODE(n, el) { + if (std::string("correspondences") == n->Name()) { + FOREACH_NODE(n2, n) { + if (std::string("point") == n2->Name()) { + Floorplan::EarthPosMapPos* pos = new Floorplan::EarthPosMapPos(); + pos->posOnMap_m.x = n2->FloatAttribute("mx"); + pos->posOnMap_m.y = n2->FloatAttribute("my"); + pos->posOnMap_m.z = n2->FloatAttribute("mz"); + pos->posOnEarth.lat = n2->FloatAttribute("lat"); + pos->posOnEarth.lon = n2->FloatAttribute("lon"); + pos->posOnEarth.height = n2->FloatAttribute("alt"); + reg.correspondences.push_back(pos); + } + } + } + } + return reg; + } + /** parse the node */ static std::vector parseFloors(const XMLElem* el) { std::vector floors; diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 3c09292..4294ff4 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -49,11 +49,34 @@ namespace Floorplan { root->SetAttribute("width", map->width); root->SetAttribute("depth", map->depth); + // add earth registration to the map + addEarthReg(doc, root, map); + // add all floors to the map addFloors(doc, root, map); } + /** add earth registration to the map */ + static void addEarthReg(XMLDoc& doc, XMLElem* root, const IndoorMap* map) { + XMLElem* earthReg = doc.NewElement("earthReg"); { + XMLElem* correspondences = doc.NewElement("correspondences"); + for (const Floorplan::EarthPosMapPos* reg : map->earthReg.correspondences) { + XMLElem* point = doc.NewElement("point"); + point->SetAttribute("lat", reg->posOnEarth.lat); + point->SetAttribute("lon", reg->posOnEarth.lon); + point->SetAttribute("alt", reg->posOnEarth.height); + point->SetAttribute("mx", reg->posOnMap_m.x); + point->SetAttribute("my", reg->posOnMap_m.y); + point->SetAttribute("mz", reg->posOnMap_m.z); + correspondences->InsertEndChild(point); + } + earthReg->InsertEndChild(correspondences); + } root->InsertEndChild(earthReg); + } + + + /** add all floors to the map */ static void addFloors(XMLDoc& doc, XMLElem* root, const IndoorMap* map) { XMLElem* floors = doc.NewElement("floors"); diff --git a/geo/EarthPos.h b/geo/EarthPos.h index d038fc1..c8bac44 100644 --- a/geo/EarthPos.h +++ b/geo/EarthPos.h @@ -11,6 +11,11 @@ struct EarthPos { /** height above sea level */ float height; + /** empty ctor */ + EarthPos() : lat(NAN), lon(NAN), height(NAN) { + ; + } + /** ctor with values */ EarthPos(const double lat, const double lon, const float height) : lat(lat), lon(lon), height(height) { ; From 59502931e55556a8ba2d52c9ffd19c758d194f89 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 23 Mar 2017 19:52:06 +0100 Subject: [PATCH 27/43] added kernel density wrapper added general kld solution fixed minor bugs added tests --- main.cpp | 4 +- math/distribution/KernelDensity.h | 35 ++++++++ math/distribution/NormalN.h | 12 ++- math/divergence/KullbackLeibler.h | 67 ++++++++++++++ sensors/offline/FileReader.h | 2 +- sensors/radio/model/WiFiModelLogDistCeiling.h | 6 +- tests/math/divergence/TestKullbackLeibler.cpp | 89 +++++++++++++++++++ 7 files changed, 207 insertions(+), 8 deletions(-) create mode 100644 math/distribution/KernelDensity.h diff --git a/main.cpp b/main.cpp index de06c02..9e42812 100755 --- a/main.cpp +++ b/main.cpp @@ -28,8 +28,8 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - - ::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; + ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; + //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/distribution/KernelDensity.h b/math/distribution/KernelDensity.h new file mode 100644 index 0000000..a103e3e --- /dev/null +++ b/math/distribution/KernelDensity.h @@ -0,0 +1,35 @@ +#ifndef KERNELDENSITY_H +#define KERNELDENSITY_H + +#include +#include +#include + +#include + +#include "../../Assertions.h" +#include "../Random.h" + + +namespace Distribution { + + template class KernelDensity{ + + private: + const std::function probabilityFunction; + + + public: + KernelDensity(const std::function probabilityFunction) : probabilityFunction(probabilityFunction){ + + } + + T getProbability(Sample sample){ + return probabilityFunction(sample); + } + + }; + +} + +#endif // KERNELDENSITY_H diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index d98c4d9..bb332c4 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -15,8 +15,8 @@ namespace Distribution { private: - const Eigen::VectorXd mu; - const Eigen::MatrixXd sigma; + Eigen::VectorXd mu; + Eigen::MatrixXd sigma; const double _a; const Eigen::MatrixXd _sigmaInv; @@ -61,6 +61,14 @@ namespace Distribution { return this->_sigmaInv; } + void setSigma(Eigen::MatrixXd sigma){ + this->sigma = sigma; + } + + void setMu(Eigen::VectorXd mu){ + this->mu = mu; + } + /** return a NormalN based on given data */ static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) { diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h index 1ae4433..aab08be 100644 --- a/math/divergence/KullbackLeibler.h +++ b/math/divergence/KullbackLeibler.h @@ -9,10 +9,77 @@ namespace Divergence { + enum LOGMODE{ + BASE2, + BASE10, + NATURALIS + }; + template class KullbackLeibler { public: + /** Calculate the Kullback Leibler Distance from a set of sample densities + * Info: https://en.wikipedia.org/wiki/Kullback%E2%80%93Leibler_divergence + * @param P is the vector containing the densities of a set of samples + * @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){ + + // Assure P and Q have the same size and are finite in all values + Assert::equal(P.size(), Q.size(), "The sample vectors P and Q do not have the same size."); + if(!P.allFinite() || !Q.allFinite()){ + Assert::doThrow("The sample vectors P and Q are not finite. Check for NaN or Inf."); + } + + Scalar dist = 0.0; + Scalar PQratio = 0.0; + + //normalize to 1 + 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"); + + //sum up the logarithmic difference between P and Q, also called kullback leibler... + for(int i = 0; i < P.size(); ++i){ + + if((P[i] == 0.0) && (Q[i] == 0.0)){ + dist += 0.0; + } else { + + // calc PQratio + if(Q[i] == 0.0){ + Assert::doThrow("Division by zero is not allowed ;). TODO: What if the densities are to small?"); + //PQratio = P[i] / 0.00001; + } else { + PQratio = P[i] / Q[i]; + } + + //use log for dist + if (mode == NATURALIS){ + dist += P[i] * log(PQratio); + } + + else if (mode == BASE2){ + dist += P[i] * log2(PQratio); + } + + else if (mode == BASE10){ + dist += P[i] * log10(PQratio); + } + + } + } + + return dist; + } + + static inline Scalar getGeneralFromSamplesSymmetric(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){ + return getGeneralFromSamples(P, Q, mode) + getGeneralFromSamples(Q, P, mode); + } + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution * Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/ */ diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 99d0fd4..17786fc 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -193,7 +193,7 @@ namespace Offline { WiFiMeasurements wifi; Splitter s(data, sep); - for (size_t i = 0; i < s.size(); i += 3) { + for (size_t i = 0; i < s.size()-1; i += 3) { const std::string mac = s.get(i+0); const float freq = s.getFloat(i+1); diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index f0a7e05..f0e237f 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -89,9 +89,9 @@ public: void addAP(const MACAddress& accessPoint, const APEntry& params) { // sanity check - Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); - 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::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); + //Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]"); + //Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]"); Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?"); diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp index 0a899f1..6f066cb 100644 --- a/tests/math/divergence/TestKullbackLeibler.cpp +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -211,4 +211,93 @@ TEST(KullbackLeibler, multivariateGaussGeCov) { ASSERT_GE(kld34sym, kld12sym); } +TEST(KullbackLeibler, 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] = 1.0; + mu4[1] = 1.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) = 1.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::KullbackLeibler::getGeneralFromSamples(samples1, samples2, Divergence::LOGMODE::NATURALIS); + double kld34 = Divergence::KullbackLeibler::getGeneralFromSamples(samples3, samples4, Divergence::LOGMODE::NATURALIS); + std::cout << kld34 << " > " << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getGeneralFromSamplesSymmetric(samples1, samples2, Divergence::LOGMODE::NATURALIS); + double kld34sym = Divergence::KullbackLeibler::getGeneralFromSamplesSymmetric(samples3, samples4, Divergence::LOGMODE::NATURALIS); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); + +} + #endif From c0cef979bb875425470e664272b8fe4bec3196c8 Mon Sep 17 00:00:00 2001 From: kazu Date: Fri, 24 Mar 2017 10:07:08 +0100 Subject: [PATCH 28/43] added helper methods for debug printing fixed issue when reading wifi entries within old walk files worked on earth-registration added corresponding test-cases other minor changes --- floorplan/v2/FloorplanHelper.h | 11 ++ geo/EarthMapping.h | 138 ++++++++++++++++-- geo/EarthPos.h | 4 + main.cpp | 5 +- sensors/gps/GPSData.h | 4 + sensors/offline/FileReader.h | 3 +- sensors/offline/Sensors.h | 4 +- sensors/radio/model/WiFiModelLogDistCeiling.h | 10 +- sensors/radio/setup/WiFiOptimizer.h | 7 + tests/geo/TestEarth.cpp | 50 +++++++ 10 files changed, 214 insertions(+), 22 deletions(-) create mode 100644 tests/geo/TestEarth.cpp diff --git a/floorplan/v2/FloorplanHelper.h b/floorplan/v2/FloorplanHelper.h index 473202c..53605cd 100644 --- a/floorplan/v2/FloorplanHelper.h +++ b/floorplan/v2/FloorplanHelper.h @@ -29,6 +29,17 @@ public: return std::make_pair(nullptr, nullptr); } + /** get all APs within the map */ + static std::vector> getAPs(const Floorplan::IndoorMap* map) { + std::vector> res; + for (Floorplan::Floor* f : map->floors) { + for (Floorplan::AccessPoint* ap : f->accesspoints) { + res.push_back(std::make_pair(ap,f)); + } + } + return res; + } + public: /** align all floorplan values to the given grid size. needed for the grid factory */ diff --git a/geo/EarthMapping.h b/geo/EarthMapping.h index 8ff10da..aa1471c 100644 --- a/geo/EarthMapping.h +++ b/geo/EarthMapping.h @@ -3,6 +3,9 @@ #include "Point3.h" #include "EarthPos.h" +#include "../floorplan/v2/Floorplan.h" +#include "../floorplan/v2/FloorplanHelper.h" +#include "../misc/Debug.h" /** * mapping between positions on earth and positions within the floorplan @@ -23,30 +26,119 @@ private: double m_per_deg_lat; double m_per_deg_lon; + static constexpr const char* NAME = "EarthMap"; + public: + /** ctor with parameters */ + EarthMapping(Point3 center_map_m, EarthPos center_earth, float rotation_deg) : + center_map_m(center_map_m), center_earth(center_earth), rotation_deg(rotation_deg) { + + precalc(); + + } + + /** ctor for a given map */ + EarthMapping(Floorplan::IndoorMap* map) { + + // get the map<->earth correspondences from the floorplan + const std::vector cor = map->earthReg.correspondences; + + // sanity check + if (cor.size() < 3) { + throw Exception("for EarthMapping to work, the map needs at least 3 correspondence points"); + } + + Log::add(NAME, "calculating map<->earth correspondence using " + std::to_string(cor.size()) + " reference points"); + + // 1) + // to reduce errors, use the average of all correspondces for earth<->map mapping + Point3 _mapSum(0,0,0); + EarthPos _earthSum(0,0,0); + for (const Floorplan::EarthPosMapPos* epmp : cor) { + _mapSum += epmp->posOnMap_m; + _earthSum.lat += epmp->posOnEarth.lat; + _earthSum.lon += epmp->posOnEarth.lon; + _earthSum.height += epmp->posOnEarth.height; + } + const Point3 _mapAvg = _mapSum / cor.size(); + const EarthPos _earthAvg = EarthPos(_earthSum.lat/cor.size(), _earthSum.lon/cor.size(), _earthSum.height/cor.size()); + + // 2) + // initialize the mapper with those values + // this allows a first mapping, but the map is not facing towards the north! + rotation_deg = 0; // currently unkown + center_map_m = _mapAvg; + center_earth = _earthAvg; + precalc(); + + Log::add(NAME, "avg. reference points: " + _mapAvg.asString() + " <-> " + _earthAvg.asString()); + + // 3) + // now we use this initial setup to determine the rotation angle between map and world + float deltaAngleSum = 0; + for (Floorplan::EarthPosMapPos* epmp : cor) { + + // angle between mapAvg and mapCorrespondencePoint + const float angleMap = std::atan2(_mapAvg.y - epmp->posOnMap_m.y, _mapAvg.x - epmp->posOnMap_m.x); + + // use the current setup to convert from map to world, WITHOUT correct rotation + const Point3 repro = this->worldToMap(epmp->posOnEarth); + + // get the angle between mapAvg and projectedCorrespondencePoint + const float angleEarth = std::atan2(_mapAvg.y - repro.y, _mapAvg.x - repro.x); + + // the difference between angleMap and angleEarth contains the angle needed to let the map face northwards + // we use the average of all those angles determined by each correspondence + const float dx_rad = angleEarth - angleMap; + float dx_deg = (dx_rad * 180 / M_PI); + if (dx_deg < 0) {dx_deg = 360 + dx_deg;} + deltaAngleSum += dx_deg; + + } + const float deltaSumAvg = deltaAngleSum / cor.size(); + + Log::add(NAME, "avg angular difference [north-rotation]: " + std::to_string(deltaSumAvg)); + + // 4) + // the current center is not the rotation center we actually need: + // e.g. when correspondence points were outside of the building, the rotation center is wrong + // as real rotation center, we use the building's bbox center and the correspondence lon/lat on earth + const BBox3 bbox = FloorplanHelper::getBBox(map); + const Point2 _mapCenter2 = ((bbox.getMax() - bbox.getMin()) / 2).xy(); + const Point3 _mapCenter3 = Point3(_mapCenter2.x, _mapCenter2.y, this->center_map_m.z); // keep original z! + this->center_earth = mapToWorld(_mapCenter3); + this->center_map_m = _mapCenter3; + + Log::add(NAME, "setting rotation center from bbox: " + center_map_m.asString() + " <-> " + center_earth.asString()); + + // 5) + // finally, let the mapper know the north-angle + this->rotation_deg = deltaSumAvg; + + } + void build() { // TODO + precalc(); } /** convert from map-coordinates to earth-coordinates */ EarthPos mapToWorld(const Point3 mapPos_m) const { - Point3 pos = mapPos_m; - // move to (0,0,0) - pos -= center_map_m; + Point3 pos = mapPos_m - center_map_m; // undo the rotation - const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI); + const Point2 xy = pos.xy().rotated(degToRad(+rotation_deg)); // convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center - const double lat = cenLat + (xy.y / m_per_deg_lat); - const double lon = cenLon + (xy.x / m_per_deg_lon); - const float height = pos.z; + const double lat = center_earth.lat + (xy.y / m_per_deg_lat); + const double lon = center_earth.lon + (xy.x / m_per_deg_lon); + const float height = center_earth.height + pos.z; // done return EarthPos(lat, lon, height); @@ -56,18 +148,36 @@ public: /** convert from earth-coordinates to map-coordinates */ Point3 worldToMap(const EarthPos earthPos) const { - const double y_m = +(lat-cenLat) * m_per_deg_lat; - const double x_m = +(lon-cenLon) * m_per_deg_lon; + // move to center and scale + const double y_m = +(earthPos.lat - center_earth.lat) * m_per_deg_lat; + const double x_m = +(earthPos.lon - center_earth.lon) * m_per_deg_lon; + const double z_m = (earthPos.height - center_earth.height); // rotate (our map is axis aligned) - Point2 pos(x_m, y_m); - pos = pos.rotated(rotDeg / 180 * M_PI); + Point2 xy(x_m, y_m); + xy = xy.rotated(degToRad(-rotation_deg)); - // apply movement - pos += mapCenter_m; + // append height + Point3 pos3(xy.x, xy.y, z_m); - return pos; + // move from center + pos3 += center_map_m; + return pos3; + + } + +private: + + /** perform some pre-calculations to speed things up */ + void precalc() { + const double refLat = center_earth.lat / 180.0 * M_PI; + m_per_deg_lat = 111132.954 - 559.822 * std::cos( 2.0 * refLat ) + 1.175 * std::cos( 4.0 * refLat); + m_per_deg_lon = 111132.954 * std::cos ( refLat ); + } + + static inline float degToRad(const float deg) { + return deg / 180.0f * (float) M_PI; } }; diff --git a/geo/EarthPos.h b/geo/EarthPos.h index c8bac44..e46c50d 100644 --- a/geo/EarthPos.h +++ b/geo/EarthPos.h @@ -21,6 +21,10 @@ struct EarthPos { ; } + std::string asString() const { + return "(lat: " + std::to_string(lat) + "°, lon: " + std::to_string(lon) + "°, alt: " + std::to_string(height) + ")"; + } + }; #endif // EARTHPOS_H diff --git a/main.cpp b/main.cpp index de06c02..411cec6 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,10 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; + //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; + ::testing::GTEST_FLAG(filter) = "*Earth*"; + + //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/sensors/gps/GPSData.h b/sensors/gps/GPSData.h index e314d85..7d63cac 100644 --- a/sensors/gps/GPSData.h +++ b/sensors/gps/GPSData.h @@ -38,6 +38,10 @@ struct GPSData { EQ_OR_NAN(speed, o.speed); } + std::string asString() const { + return "(lat: " + std::to_string(lat) + ", lon: " + std::to_string(lon) + ", alt: " + std::to_string(alt) + " acur: " + std::to_string(accuracy) + ")"; + } + private: static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 99d0fd4..b5372ef 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -193,7 +193,8 @@ namespace Offline { WiFiMeasurements wifi; Splitter s(data, sep); - for (size_t i = 0; i < s.size(); i += 3) { + // the -1 is due to some old files containing a trailing ";" resulting in one additional stray column + for (size_t i = 0; i < s.size()-1; i += 3) { const std::string mac = s.get(i+0); const float freq = s.getFloat(i+1); diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h index 47ffec6..32168cd 100644 --- a/sensors/offline/Sensors.h +++ b/sensors/offline/Sensors.h @@ -9,12 +9,12 @@ namespace Offline { LIN_ACC = 2, GYRO = 3, BARO = 5, + COMPASS = 6, // also called "orientatioN" WIFI = 8, BEACON = 9, - COMPASS = 15, GPS = 16, GROUND_TRUTH = 99, - POS = 1001, // IPIN2016 + POS = 1001, // IPIN2016 }; template struct TS { diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index f0a7e05..c823f9d 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -86,12 +86,14 @@ public: } /** make the given AP (and its parameters) known to the model */ - void addAP(const MACAddress& accessPoint, const APEntry& params) { + void addAP(const MACAddress& accessPoint, const APEntry& params, const bool assertSafe = true) { // sanity check - Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); - 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]"); + if (assertSafe) { + Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]"); + Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]"); + Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]"); + } Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?"); diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index c9996c9..125fb7c 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -32,6 +32,13 @@ namespace WiFiOptimizer { return res; } + /** get all [VAPGrouped, Averaged] fingerprints for the given mac */ + virtual const std::vector& getFingerprintsFor(const MACAddress& mac) { + const auto& it = apMap.find(mac); + if (it == apMap.end()) {throw Exception("mac not found: " + mac.asString());} + return it->second; + } + /** add a new fingerprint to the optimizer as data-source */ virtual void addFingerprint(const WiFiFingerprint& fp) { diff --git a/tests/geo/TestEarth.cpp b/tests/geo/TestEarth.cpp new file mode 100644 index 0000000..955c6f0 --- /dev/null +++ b/tests/geo/TestEarth.cpp @@ -0,0 +1,50 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/EarthMapping.h" + +TEST(Earth, fordwardBackward) { + + const Point3 mapCenter(20, 30, 10); + const EarthPos earthCenter(50, 10, 100); + + // earth-mapping including rotation + EarthMapping em(mapCenter, earthCenter, 40); + + // as-is + const Point3 p1 = em.worldToMap( earthCenter ); + ASSERT_EQ(mapCenter.x, p1.x); + ASSERT_EQ(mapCenter.y, p1.y); + ASSERT_EQ(mapCenter.z, p1.z); + + // as-is + const EarthPos ep1 = em.mapToWorld( mapCenter ); + ASSERT_EQ(earthCenter.lat, ep1.lat); + ASSERT_EQ(earthCenter.lon, ep1.lon); + ASSERT_EQ(earthCenter.height, ep1.height); + + + // + 20 height + const Point3 p2 = em.worldToMap( EarthPos(50,10,120) ); + ASSERT_EQ(mapCenter.x, p2.x); + ASSERT_EQ(mapCenter.y, p2.y); + ASSERT_EQ(30, p2.z); + + // - 20 height + const EarthPos ep2 = em.mapToWorld( Point3(20, 30, -10) ); + ASSERT_EQ(earthCenter.lat, ep2.lat); + ASSERT_EQ(earthCenter.lon, ep2.lon); + ASSERT_EQ(earthCenter.height-20, ep2.height); + +} + +TEST(Earth, estimateNorth) { + + + +} + + + +#endif From e34e773e31440a4d64752e9bf77d0b3508e8cf99 Mon Sep 17 00:00:00 2001 From: kazu Date: Fri, 24 Mar 2017 11:20:29 +0100 Subject: [PATCH 29/43] added new helper methods to work with ground-truth made some assertions optional [i know what i am doing!] --- floorplan/v2/Floorplan.h | 7 +++--- floorplan/v2/FloorplanCeilings.h | 2 +- floorplan/v2/FloorplanHelper.h | 25 +++++++++++++++++++ sensors/radio/model/WiFiModel.h | 3 +++ sensors/radio/model/WiFiModelLogDistCeiling.h | 25 ++++++++++++++----- 5 files changed, 52 insertions(+), 10 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 1f62b72..f1dcaf4 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -197,7 +197,7 @@ namespace Floorplan { using FloorPOIs = std::vector; using FloorStairs = std::vector; using FloorElevators = std::vector; - using FloorGroundTruthPoints = std::vector; + using FloorGroundTruthPoints = std::vector; /** describes one floor within the map, starting at a given height */ struct Floor { @@ -215,7 +215,7 @@ namespace Floorplan { FloorPOIs pois; // POIs within the floor FloorStairs stairs; // all stairs within one floor FloorElevators elevators; // all elevators within one floor - FloorGroundTruthPoints gtpoints; // all ground truth points within one floor + FloorGroundTruthPoints gtpoints; // all ground truth points within one floor //FloorKeyValue other; // other, free elements Floor() {;} @@ -251,9 +251,10 @@ namespace Floorplan { /** a GroundTruthPoint located somewhere on a floor */ struct GroundTruthPoint { int id; //TODO: this value can be changed and isn't set incremental within the indoormap - Point3 pos; + Point3 pos; // TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height! GroundTruthPoint() : id(), pos() {;} GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;} + const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);} bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} }; diff --git a/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h index 887ed8e..857bfe8 100644 --- a/floorplan/v2/FloorplanCeilings.h +++ b/floorplan/v2/FloorplanCeilings.h @@ -87,7 +87,7 @@ namespace Floorplan { if (diff < 0.1) {++numNear;} else {++numFar;} } if ((numNear + numFar) > 150000) { - Assert::isTrue(numNear < numFar*0.1, + Assert::isTrue(numNear < numFar*0.15, "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! \ diff --git a/floorplan/v2/FloorplanHelper.h b/floorplan/v2/FloorplanHelper.h index 53605cd..5a96f16 100644 --- a/floorplan/v2/FloorplanHelper.h +++ b/floorplan/v2/FloorplanHelper.h @@ -40,6 +40,31 @@ public: return res; } + /** get all ground-truth points within the map as hash-map: id->pos */ + static std::unordered_map getGroundTruthPoints(const Floorplan::IndoorMap* map) { + std::unordered_map res; + for (const Floorplan::Floor* f : map->floors) { + for (const Floorplan::GroundTruthPoint* gtp : f->gtpoints) { + res[gtp->id] = gtp->getPosition(*f); + } + } + return res; + } + + /** get all ground-truth points, identified by the given indices */ + static std::vector getGroundTruth(const Floorplan::IndoorMap* map, const std::vector indices) { + + // get a map id->pos for all ground-truth-points within the map + const std::unordered_map src = getGroundTruthPoints(map); + std::vector res; + for (const int idx : indices) { + const auto& it = src.find(idx); + if (it == src.end()) {throw Exception("map does not contain a ground-truth-point with ID " + std::to_string(idx));} + res.push_back(it->second); + } + return res; + } + public: /** align all floorplan values to the given grid size. needed for the grid factory */ diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index bebfce4..f6603c4 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -14,6 +14,9 @@ class WiFiModel { public: + /** dtor */ + virtual ~WiFiModel() {;} + // /** get the given access-point's RSSI at the provided location */ // virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0; diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index c823f9d..328bf01 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -71,21 +71,28 @@ public: } - /** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */ - void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) { + /** + * load AP information from the floorplan. + * use the given fixed TXP/EXP/WAF for all APs. + * usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false! + */ + void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) { for (const Floorplan::Floor* floor : map->floors) { for (const Floorplan::AccessPoint* ap : floor->accesspoints) { const APEntry ape(ap->getPos(floor), txp, exp, waf); const MACAddress mac = vg.getBaseMAC(MACAddress(ap->mac)); - Log::add("WiModLDC", "AP: " + ap->mac + " -> " + mac.asString()); - addAP(MACAddress(mac), ape); + Log::add("WiModLDC", "AP added! given: " + ap->mac + " -> after VAP: " + mac.asString()); + addAP(MACAddress(mac), ape, assertSafe); } } } - /** make the given AP (and its parameters) known to the model */ + /** + * 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 APEntry& params, const bool assertSafe = true) { // sanity check @@ -126,7 +133,13 @@ public: const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m); // combine - return rssiLOS + wafLoss; + const float res = rssiLOS + wafLoss; + + // sanity check + Assert::isNotNaN(res, "detected NaN within WiFiModelLogDistCeiling::getRSSI()"); + + // ok + return res; } From a25ffb2ba35a71253a2f1c050920e832c8a162b1 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 24 Mar 2017 17:28:57 +0100 Subject: [PATCH 30/43] added jensen shannon, fixe kullback leibler when P=0 --- math/divergence/JensenShannon.h | 28 ++++++++++++++++++++++++++++ math/divergence/KullbackLeibler.h | 11 ++++++++--- 2 files changed, 36 insertions(+), 3 deletions(-) create mode 100644 math/divergence/JensenShannon.h diff --git a/math/divergence/JensenShannon.h b/math/divergence/JensenShannon.h new file mode 100644 index 0000000..a76686d --- /dev/null +++ b/math/divergence/JensenShannon.h @@ -0,0 +1,28 @@ +#ifndef JENSENSHANNON_H +#define JENSENSHANNON_H + +#include "KullbackLeibler.h" + +#include "../../Assertions.h" +#include + + +namespace Divergence { + +template class JensenShannon { + + /** Calculate the Jensen Shannon Divergece from a set of sample densities + * Info: https://en.wikipedia.org/wiki/Jensen–Shannon_divergence + * @param P is the vector containing the densities of a set of samples + * @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){ + Eigen::VectorXd M = 0.5 * (P + Q); + + return (0.5 * KullbackLeibler::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler::getGeneralFromSamples(Q, M, mode)); + } +}; + +} + +#endif // JENSENSHANNON_H diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h index aab08be..630e5d2 100644 --- a/math/divergence/KullbackLeibler.h +++ b/math/divergence/KullbackLeibler.h @@ -45,13 +45,18 @@ namespace Divergence { //sum up the logarithmic difference between P and Q, also called kullback leibler... for(int i = 0; i < P.size(); ++i){ + // if both prob are near zero we assume a 0 distance since lim->0 = 0 if((P[i] == 0.0) && (Q[i] == 0.0)){ - dist += 0.0; - } else { + dist += 0.0; + } + //if prob of P is 0 we also assume a 0 distance since lim->0 0 * log(0) = 0 + else if ((P[i] == 0.0) && (Q[i] > 0.0)){ + dist += 0.0; + } else{ // calc PQratio if(Q[i] == 0.0){ - Assert::doThrow("Division by zero is not allowed ;). TODO: What if the densities are to small?"); + Assert::doThrow("Division by zero is not allowed ;)."); //PQratio = P[i] / 0.00001; } else { PQratio = P[i] / Q[i]; From d9788a71c07e8395d58cb8d28901b0ebc117ffb6 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 24 Mar 2017 17:37:05 +0100 Subject: [PATCH 31/43] fixed bugs in jensenshannon.h --- math/divergence/JensenShannon.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/math/divergence/JensenShannon.h b/math/divergence/JensenShannon.h index a76686d..5c9bbb3 100644 --- a/math/divergence/JensenShannon.h +++ b/math/divergence/JensenShannon.h @@ -11,6 +11,7 @@ namespace Divergence { template class JensenShannon { +public: /** Calculate the Jensen Shannon Divergece from a set of sample densities * Info: https://en.wikipedia.org/wiki/Jensen–Shannon_divergence * @param P is the vector containing the densities of a set of samples @@ -19,7 +20,7 @@ template class JensenShannon { static inline Scalar getGeneralFromSamples(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){ Eigen::VectorXd M = 0.5 * (P + Q); - return (0.5 * KullbackLeibler::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler::getGeneralFromSamples(Q, M, mode)); + return (0.5 * KullbackLeibler::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler::getGeneralFromSamples(Q, M, mode)); } }; From 4439123e5bedf3f0e1d18d65fbc718525bb38641 Mon Sep 17 00:00:00 2001 From: kazu Date: Tue, 28 Mar 2017 21:03:17 +0200 Subject: [PATCH 32/43] fixed some issues some new helper methods added listener-support to offline-reader --- sensors/offline/FileReader.h | 44 ++++++++++++++++--- sensors/offline/Listener.h | 2 +- sensors/radio/WiFiMeasurements.h | 10 +++++ sensors/radio/WiFiProbabilityFree.h | 4 +- sensors/radio/model/WiFiModelLogDistCeiling.h | 4 +- sensors/radio/setup/WiFiFingerprint.h | 9 ++++ sensors/radio/setup/WiFiFingerprints.h | 27 +++++++++--- 7 files changed, 82 insertions(+), 18 deletions(-) diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index b5372ef..60b8e47 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -24,6 +24,7 @@ #include "Splitter.h" #include "Sensors.h" +#include "Listener.h" #warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]" @@ -49,6 +50,8 @@ namespace Offline { static constexpr char sep = ';'; + Listener* listener = nullptr; + public: /** empty ctor. call open() */ @@ -62,7 +65,8 @@ namespace Offline { } /** open the given file */ - void open(const std::string& file) { + void open(const std::string& file, Listener* listener = nullptr) { + this->listener = listener; parse(file); } @@ -142,20 +146,27 @@ namespace Offline { TS elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z))); lin_acc.push_back(elem); entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1)); + } void parseGravity(const uint64_t ts, const std::string& data){ + GravityData gravData; + const auto pos1 = data.find(';'); const auto pos2 = data.find(';', pos1+1); - const std::string x = data.substr(0, pos1); - const std::string y = data.substr(pos1+1, pos2-pos1-1); - const std::string z = data.substr(pos2+1); + gravData.x = std::stof(data.substr(0, pos1)); + gravData.y = std::stof(data.substr(pos1+1, pos2-pos1-1)); + gravData.z = std::stof(data.substr(pos2+1)); - TS elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z))); + TS elem(ts, gravData); gravity.push_back(elem); entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1)); + + // inform listener + if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);} + } void parseAccelerometer(const uint64_t ts, const std::string& data) { @@ -167,10 +178,14 @@ namespace Offline { const std::string y = data.substr(pos1+1, pos2-pos1-1); const std::string z = data.substr(pos2+1); - TS elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z))); + const AccelerometerData accData(std::stof(x), std::stof(y), std::stof(z)); + TS elem(ts, accData); acc.push_back(elem); entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1)); + // inform listener + if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);} + } void parseGyroscope(const uint64_t ts, const std::string& data) { @@ -182,10 +197,14 @@ namespace Offline { const std::string y = data.substr(pos1+1, pos2-pos1-1); const std::string z = data.substr(pos2+1); - TS elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z))); + const GyroscopeData gyroData(std::stof(x), std::stof(y), std::stof(z)); + TS elem(ts, gyroData); gyro.push_back(elem); entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1)); + // inform listener + if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);} + } void parseWiFi(const uint64_t ts, const std::string& data) { @@ -210,6 +229,9 @@ namespace Offline { this->wifi.push_back(TS(ts, wifi)); entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1)); + // inform listener + if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);} + } void parseBeacons(const uint64_t ts, const std::string& data) { @@ -250,6 +272,9 @@ namespace Offline { barometer.push_back(elem); entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + // inform listener + if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);} + } void parseCompass(const uint64_t ts, const std::string& data) { @@ -264,6 +289,9 @@ namespace Offline { this->compass.push_back(elem); entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1)); + // inform listener + if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);} + } /** parse the given GPS entry */ @@ -282,6 +310,8 @@ namespace Offline { this->gps.push_back(elem); entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1)); + // inform listener + if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);} } diff --git a/sensors/offline/Listener.h b/sensors/offline/Listener.h index d9be0a6..83cc28c 100644 --- a/sensors/offline/Listener.h +++ b/sensors/offline/Listener.h @@ -20,7 +20,7 @@ namespace Offline { virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0; virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0; - virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0; + virtual void onGravity(const Timestamp ts, const GravityData data) = 0; virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0; virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0; virtual void onGPS(const Timestamp ts, const GPSData data) = 0; diff --git a/sensors/radio/WiFiMeasurements.h b/sensors/radio/WiFiMeasurements.h index a846251..267075b 100644 --- a/sensors/radio/WiFiMeasurements.h +++ b/sensors/radio/WiFiMeasurements.h @@ -22,6 +22,16 @@ struct WiFiMeasurements { return res; } + /** get the measurements for the given MAC [if available] otherwise null */ + WiFiMeasurement* getForMac(const MACAddress& mac) { + for (WiFiMeasurement& m : entries) { + if (m.getAP().getMAC() == mac) { + return &m; + } + } + return nullptr; + } + }; #endif // WIFIMEASUREMENTS_H diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index e850101..5f3b5ac 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -17,7 +17,7 @@ class WiFiObserverFree : public WiFiProbability { private: - const float sigma = 8.0f; + const float sigma; const float sigmaPerSecond = 3.0f; @@ -58,7 +58,7 @@ public: 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?"); + Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?"); // sigma grows with measurement age const float sigma = this->sigma + this->sigmaPerSecond * age.sec(); diff --git a/sensors/radio/model/WiFiModelLogDistCeiling.h b/sensors/radio/model/WiFiModelLogDistCeiling.h index 328bf01..37d80e4 100644 --- a/sensors/radio/model/WiFiModelLogDistCeiling.h +++ b/sensors/radio/model/WiFiModelLogDistCeiling.h @@ -60,12 +60,12 @@ public: } /** 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) { + void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) { 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); + addAP(MACAddress(ap->mac), ape, assertSafe); } } diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index 940faa4..2938c5d 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -57,6 +57,15 @@ struct WiFiFingerprint { } + /** get all measurements for the given MAC */ + std::vector getAllForMAC(const MACAddress& mac) const { + std::vector res; + for (const WiFiMeasurement& m : measurements.entries) { + if (m.getAP().getMAC() == mac) {res.push_back(m);} + } + return res; + } + /** serialize */ void write(std::ostream& out) const { out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n"; diff --git a/sensors/radio/setup/WiFiFingerprints.h b/sensors/radio/setup/WiFiFingerprints.h index 9dd1b2f..8745b2b 100644 --- a/sensors/radio/setup/WiFiFingerprints.h +++ b/sensors/radio/setup/WiFiFingerprints.h @@ -15,22 +15,37 @@ class WiFiFingerprints { private: - /** the file to save the calibration model to */ - std::string file; +// /** the file to save the calibration model to */ +// std::string file; /** all fingerprints (position -> measurements) within the model */ std::vector fingerprints; public: - WiFiFingerprints(const std::string& file) : file(file) { - load(); + /** empty ctor */ + WiFiFingerprints() { + + } + + /** ctor from file */ + WiFiFingerprints(const std::string& file) { + load(file); } const std::vector& getFingerprints() const { return fingerprints; } + std::vector& getFingerprints() { + return fingerprints; + } + + /** attach the given fingerprint */ + void add(const WiFiFingerprint& fp) { + fingerprints.push_back(fp); + } + /** get all fingerprints that measured exactly the given mac [no VAP grouping] */ const std::vector getFingerprintsFor(const MACAddress& mac) const { @@ -58,7 +73,7 @@ public: } /** deserialize the model */ - void load() { + void load(const std::string& file) { // open and check std::ifstream inp(file.c_str()); @@ -87,7 +102,7 @@ public: /** serialize the model */ - void save() { + void save(const std::string& file) { // open and check std::ofstream out(file.c_str()); From 8930be1e2ca8eff1e0f316097a9a4414a2242b4e Mon Sep 17 00:00:00 2001 From: kazu Date: Fri, 31 Mar 2017 11:47:29 +0200 Subject: [PATCH 33/43] added new sanity-check assertions fixed issue with angles [bad interface] - adjusted other parts accordingly - added corresponding test-cases started working on absolute heading --- geo/Angle.h | 10 ++- geo/Heading.h | 7 +- grid/factory/v2/Importance.h | 2 + grid/walk/v2/GridWalker.h | 12 ++- .../WalkModuleAbsoluteHeadingControl.h | 85 +++++++++++++++++++ .../v2/modules/WalkModuleHeadingControl.h | 3 +- .../v2/modules/WalkModuleHeadingVonMises.h | 3 +- main.cpp | 2 +- sensors/offline/OfflineAndroid.h | 26 +++++- tests/geo/TestAngle.cpp | 37 ++++++-- 10 files changed, 168 insertions(+), 19 deletions(-) create mode 100644 grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h diff --git a/geo/Angle.h b/geo/Angle.h index 5732897..eabe999 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -29,6 +29,13 @@ public: return radToDeg(getRAD_2PI(x1,y1,x2,y2)); } + /** ensure the given radians-value is within [0:2pi] */ + static float makeSafe_2PI(float rad) { + while(rad < 0) {rad += 2*PI;} + while(rad >= 2*PI) {rad -= 2*PI;} + return rad; + } + /** * gets the angular difference between @@ -44,13 +51,14 @@ public: /** * gets the angular difference between + * "angular change from r1 to r2" * - the given radians [0:2PI] * - as a change-in-direction between [-PI:+PI] */ static float getSignedDiffRAD_2PI(const float r1, const float r2) { Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg - float diff = r1-r2; + float diff = r2-r1; if (diff > +PI) {diff = -(2*PI - diff);} else if (diff < -PI) {diff = +(2*PI + diff);} Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg diff --git a/geo/Heading.h b/geo/Heading.h index eb9720e..ca22b6c 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -33,8 +33,11 @@ public: } /** signled angular difference [-PI:+PI] */ - float getSignedDiff(const Heading other) const { - return Angle::getSignedDiffRAD_2PI(rad, other.rad); +// float getSignedDiff(const Heading other) const { +// return Angle::getSignedDiffRAD_2PI(other.rad, rad); +// } + static float getSignedDiff(const Heading from, const Heading to) { + return Angle::getSignedDiffRAD_2PI(from.rad, to.rad); } /** update the angle but ensure we stay within [0:2PI] */ diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 618185d..0906fa2 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -153,6 +153,8 @@ public: // sanity check + Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString()); + Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString()); 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/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 1fe88b7..59f51e9 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -104,9 +104,19 @@ public: //if (cnt != 0) {probability /= cnt;} else {probability = 1.0;} probability = 1.0; //probability = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0); // dist_m might be zero -> no edges -> no maximum - probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) + + // add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...] + const float walkImportance = curNode->getWalkImportance(); + Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?"); + 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"); + // update after updateAfter(currentState, *startNode, *curNode); diff --git a/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h new file mode 100644 index 0000000..d3987f9 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h @@ -0,0 +1,85 @@ +#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H +#define WALKMODULEABSOLUTEHEADINGCONTROL_H + +/** + * compare the state's absolute heading against a given compass [control] + */ + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" + + +/** keep the state's heading */ +template class WalkModuleAbsoluteHeadingControl : public WalkModule { + + const float sigma_rad; + const Control* ctrl; + +public: + + /** ctor. 180 should be OK! */ + WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) : + sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)), + ctrl(ctrl) { + + // ensure the template WalkState inherits from 'WalkStateHeading'! + StaticAssert::AinheritsB(); + + } + + + 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; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) startNode; + + // NOTE: ctrl->turnAngle is cumulative SINCE the last transition! + // reset this one after every transition! + Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds"); + + // 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 (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); + + // compare the heading against the state's heading - the last error + const Heading stateHead = state.heading.direction; + + // 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;} + + } + +}; + +#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 01fe668..a79cdad 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -77,7 +77,8 @@ public: const Heading stateHead = state.heading.direction; // get the error (signed difference) between both - const float angularDiff = stateHead.getSignedDiff(head); +// const float angularDiff = stateHead.getSignedDiff(head); + const float angularDiff = Heading::getSignedDiff(head, stateHead); // adjust the error. // note: the error may get > +/- 2PI but this is not an issue! diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h index 9c01a79..30be8e1 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -81,7 +81,8 @@ public: const Heading stateHead = state.heading.direction; // get the error (signed difference) between both - const float angularDiff = stateHead.getSignedDiff(head); + //const float angularDiff = stateHead.getSignedDiff(head); + const float angularDiff = Heading::getSignedDiff(head, stateHead); // adjust the error. // note: the error may get > +/- 2PI but this is not an issue! diff --git a/main.cpp b/main.cpp index 411cec6..2808d16 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) = "*Earth*"; + ::testing::GTEST_FLAG(filter) = "*Angle*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; diff --git a/sensors/offline/OfflineAndroid.h b/sensors/offline/OfflineAndroid.h index 19049b1..3d32fc0 100644 --- a/sensors/offline/OfflineAndroid.h +++ b/sensors/offline/OfflineAndroid.h @@ -54,7 +54,7 @@ private: std::vector> gyro; std::vector> accel; - std::vector> gravity; + std::vector> gravity; std::vector> compass; std::vector> barometer; @@ -86,7 +86,7 @@ public: const std::vector>& getAccelerometer() const {return accel;} /** get all gravity readings */ - const std::vector>& getGravity() const {return gravity;} + const std::vector>& getGravity() const {return gravity;} /** get all barometer readings */ const std::vector>& getBarometer() const {return barometer;} @@ -173,8 +173,8 @@ private: } case (int) Offline::Sensor::GRAVITY: { - const AccelerometerData data = parseAccelerometer(sensorData); - gravity.push_back(OfflineEntry(ts, data)); + const GravityData data = parseGravity(sensorData); + gravity.push_back(OfflineEntry(ts, data)); if (listener) {listener->onGravity(ts, data);} break; } @@ -301,6 +301,24 @@ private: } + static inline GravityData parseGravity(const std::string& data) { + + const size_t pos1 = data.find(';', 0); + const size_t pos2 = data.find(';', pos1+1); + const size_t pos3 = data.find(';', pos2+1); + + Assert::isTrue(pos1 != std::string::npos, "format error"); + Assert::isTrue(pos2 != std::string::npos, "format error"); + Assert::isTrue(pos3 != std::string::npos, "format error"); + + const std::string sx = data.substr(0, pos1); + const std::string sy = data.substr(pos1+1, pos2-pos1-1); + const std::string sz = data.substr(pos2+1, pos3-pos2-1); + + return GravityData(std::stof(sx), std::stof(sy), std::stof(sz)); + + } + /** parse the given Barometer entry */ static inline BarometerData parseBarometer(const std::string& data) { diff --git a/tests/geo/TestAngle.cpp b/tests/geo/TestAngle.cpp index 713e571..0299627 100755 --- a/tests/geo/TestAngle.cpp +++ b/tests/geo/TestAngle.cpp @@ -14,17 +14,38 @@ TEST(Angle, dir) { } +TEST(Angle, safe) { + + ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(0))))); + ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(360))))); + ASSERT_EQ(85, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(85))))); + ASSERT_EQ(155, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(155))))); + ASSERT_EQ(275, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(275))))); + ASSERT_EQ(355, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(355))))); + + // negative + ASSERT_EQ(330, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-30))))); + ASSERT_EQ(270, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-90))))); + ASSERT_EQ(185, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-175))))); + + // too positive + ASSERT_EQ(30, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(390))))); + ASSERT_EQ(140, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(500))))); + ASSERT_EQ(180, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(900))))); + +} + TEST(Angle, calc) { - ASSERT_EQ(0, Angle::getDEG_360(0,0, +1,0)); // to the right - ASSERT_EQ(90, Angle::getDEG_360(0,0, 0,+1)); // upwards - ASSERT_EQ(180, Angle::getDEG_360(0,0, -1,0)); // to the left - ASSERT_EQ(270, Angle::getDEG_360(0,0, 0,-1)); // downwards + ASSERT_EQ(0, (int)Angle::getDEG_360(0,0, +1,0)); // to the right + ASSERT_EQ(90, (int)Angle::getDEG_360(0,0, 0,+1)); // upwards + ASSERT_EQ(180, (int)Angle::getDEG_360(0,0, -1,0)); // to the left + ASSERT_EQ(270, (int)Angle::getDEG_360(0,0, 0,-1)); // downwards - ASSERT_EQ(45, Angle::getDEG_360(0,0, +1,+1)); // to the upper right - ASSERT_EQ(135, Angle::getDEG_360(0,0, -1,+1)); // to the upper left - ASSERT_EQ(225, Angle::getDEG_360(0,0, -1,-1)); // to the lower left - ASSERT_EQ(315, Angle::getDEG_360(0,0, +1,-1)); // to the lower right + ASSERT_EQ(45, (int)Angle::getDEG_360(0,0, +1,+1)); // to the upper right + ASSERT_EQ(135, (int)Angle::getDEG_360(0,0, -1,+1)); // to the upper left + ASSERT_EQ(225, (int)Angle::getDEG_360(0,0, -1,-1)); // to the lower left + ASSERT_EQ(315, (int)Angle::getDEG_360(0,0, +1,-1)); // to the lower right } From a9cd8d5e681a04cc683c51f40fbdcf0553a6feae Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 4 Apr 2017 16:50:11 +0200 Subject: [PATCH 34/43] 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 f67f95d1ceed98a4b72ed9d600101d5c6816328b Mon Sep 17 00:00:00 2001 From: kazu Date: Mon, 17 Apr 2017 16:47:02 +0200 Subject: [PATCH 35/43] added first draft of a wifi quality analyzer --- sensors/radio/WiFiQualityAnalyzer.h | 106 ++++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 sensors/radio/WiFiQualityAnalyzer.h diff --git a/sensors/radio/WiFiQualityAnalyzer.h b/sensors/radio/WiFiQualityAnalyzer.h new file mode 100644 index 0000000..994b2ec --- /dev/null +++ b/sensors/radio/WiFiQualityAnalyzer.h @@ -0,0 +1,106 @@ +#ifndef WIFIQUALITYANALYZER_H +#define WIFIQUALITYANALYZER_H + +#include "WiFiMeasurements.h" +#include + +class WiFiQualityAnalyzer { + +private: + + int historySize = 3; + std::vector history; + float quality = 0; + +public: + + /** attach the current measurement and infer the quality */ + void add(const WiFiMeasurements& mes) { + + // update the history + history.push_back(mes); + while(history.size() > historySize) { + history.erase(history.begin()); + } + + // recalculate + update(); + + } + + /** get a quality score for the WiFi */ + float getQuality() const { + return quality; + } + +private: + + /** re-calculate the current quality */ + void update() { + + const float qCnt = getQualityByCount(); + const float qAvgdB = getQualityByAvgDB(); + + quality = qAvgdB; + + } + + /** score [0:1] based on the average sig-strength. the higher the better */ + float getQualityByAvgDB() const { + + // stats + float sum = 0; + float sum2 = 0; + float cnt = 0; + + for (const WiFiMeasurements& mes : history) { + for (const WiFiMeasurement& m : mes.entries) { + const float rssi = m.getRSSI(); + sum += rssi; + sum2 += rssi*rssi; + ++cnt; + } + } + + // average sig strength + const float avg = sum/cnt; + const float avg2 = sum2/cnt; + const float stdDev = std::sqrt(avg2 - avg*avg); + + // avg rssi score + const float minRSSI = -90; + const float maxRSSI = -65; + float score1 = (avg-minRSSI) / (maxRSSI-minRSSI); // min = 0; max = 1 + if (score1 > 1) {score1 = 1;} + if (score1 < 0) {score1 = 0;} + //const float score1 = 1.0 - std::exp(-0.07 * (avg-minRSSI)); + + // std dev score + const float score2 = 1.0 - std::exp(-1.0 * stdDev); + + return score1 * score2; + + } + + /** quality score [0:1] according to the number of seen APs */ + float getQualityByCount() const { + + // distinct macs + std::unordered_set macs; + for (const WiFiMeasurements& mes : history) { + for (const WiFiMeasurement& m : mes.entries) { + macs.insert(m.getAP().getMAC()); + } + } + + // number of distinct macs + const int cnt = macs.size(); + + // see function plot. function between [0.0:1.0] has 0.5 around 7 seen APs + return 1.0 - std::exp(-0.1 * cnt); + + } + +}; + +#endif // WIFIQUALITYANALYZER_H From 200aa94ca80d8dbc0895c2e77765b77272b07de0 Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 17 Apr 2017 16:50:56 +0200 Subject: [PATCH 36/43] 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 37/43] 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 38/43] 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 39/43] 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 40/43] 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 From 04d8ae8c74719b43937eca8fdac1c11370867a51 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 24 May 2017 10:03:39 +0200 Subject: [PATCH 41/43] changes from the laptop - some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...? --- data/XMLload.h | 14 ++ data/XMLsave.h | 14 ++ data/XMLserialize.h | 44 +++++ data/xml.h | 33 ++++ debug/PlotWifiMeasurements.h | 76 ++++++++ floorplan/v2/FloorplanCeilings.h | 46 +++-- floorplan/v2/FloorplanLINT.h | 17 +- geo/BBoxes3.h | 45 +++++ 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/MovingStdDevTS.h | 49 ++++++ math/distribution/Rectangular.h | 46 +++++ sensors/activity/Activity.h | 20 +++ sensors/activity/ActivityDetector.h | 126 ++++++++++++++ sensors/gps/GPSData.h | 7 +- sensors/gps/GPSProbability.h | 65 +++++++ sensors/offline/FilePlayer.h | 123 +++++++++++++ sensors/offline/FileReader.h | 74 ++++++-- sensors/pressure/PressureTendence.h | 6 +- sensors/radio/AccessPoint.h | 5 + sensors/radio/WiFiMeasurements.h | 4 +- sensors/radio/WiFiProbabilityFree.h | 75 +++++++- sensors/radio/model/WiFiModel.h | 8 +- sensors/radio/model/WiFiModelFactory.h | 32 ++++ sensors/radio/model/WiFiModelFactoryImpl.h | 42 +++++ sensors/radio/model/WiFiModelLogDist.h | 8 + sensors/radio/model/WiFiModelLogDistCeiling.h | 73 +++++++- sensors/radio/model/WiFiModelPerBBox.h | 164 ++++++++++++++++++ sensors/radio/model/WiFiModelPerFloor.h | 122 +++++++++++++ sensors/radio/model/WiFiModels.h | 8 + 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 +- 42 files changed, 1344 insertions(+), 60 deletions(-) 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/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h index 857bfe8..3e8e2d6 100644 --- a/floorplan/v2/FloorplanCeilings.h +++ b/floorplan/v2/FloorplanCeilings.h @@ -46,22 +46,40 @@ 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; @@ -80,14 +98,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 +123,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/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/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/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..195d087 --- /dev/null +++ b/sensors/activity/ActivityDetector.h @@ -0,0 +1,126 @@ +#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 "../activity/Activity.h" + +#include +#include +#include +#include +#include + +/** + * 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 baroAvgSlow; + MovingAverageTS baroAvgFast; + + +public: + + K::Gnuplot gp; + K::GnuplotPlot gplot; + K::GnuplotPlotElementLines l1; + K::GnuplotPlotElementLines l2; + + /** ctor */ + ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0), + stdDev(Timestamp::fromMS(200), 0), stdDev2(Timestamp::fromMS(2000), 0) { + ; + gplot.add(&l1); + gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000"); + } + + /** add barometer data */ + void add(const Timestamp ts, const BarometerData& baro) { + if (baro.isValid()) { + baroAvgSlow.add(ts, baro.hPa); + baroAvgFast.add(ts, baro.hPa); + } + } + + Activity get() { + + // delta in acceleration + const float delta_acc = std::abs(avgLong.get() - avgShort.get()); + + if (delta_acc < 0.3) { + return Activity::STANDING; + } + + // delta in pressure + const float delta_hPa = baroAvgFast.get() - baroAvgSlow.get(); + + if (std::abs(delta_hPa) < 0.1) { + return Activity::WALKING; + } else if (delta_hPa > 0) { + return Activity::WALKING_DOWN; + } else { + return Activity::WALKING_UP; + } + + } + + /** 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 (x % 10 == 0) { +//// l1.add({x, avgLong.get()}); +//// l2.add({x, avgShort.get()}); +//// gp.draw(gplot); +//// gp.flush(); +//// } + +// 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; + + } + + + + + +}; + +#endif // ACTIVITYDETECTOR_H 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/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/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/WiFiMeasurements.h b/sensors/radio/WiFiMeasurements.h index 267075b..d7d2f04 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; } diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 5f3b5ac..19a89ec 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -27,15 +27,26 @@ 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; + double error = 0; + int numMatchingAPs = 0; // process each measured AP @@ -50,7 +61,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 +70,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/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/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/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..3251f13 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,8 @@ 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); // combine const float res = rssiLOS + wafLoss; @@ -143,6 +161,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/model/WiFiModelPerBBox.h b/sensors/radio/model/WiFiModelPerBBox.h new file mode 100644 index 0000000..8a32899 --- /dev/null +++ b/sensors/radio/model/WiFiModelPerBBox.h @@ -0,0 +1,164 @@ +#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 { + 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()) { + 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..e5aa2b1 --- /dev/null +++ b/sensors/radio/model/WiFiModelPerFloor.h @@ -0,0 +1,122 @@ +#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 { + return models.front().mdl->getAllAPs(); + } + + 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 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 34271b5cb7de5691ff3346acd71563f3cfcb80d2 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 24 May 2017 17:51:29 +0200 Subject: [PATCH 42/43] fixed grid factory issue with stairs added some sanity checks --- grid/GridNode.h | 1 + grid/factory/v2/GridFactory.h | 79 +++++++++++++++++++++++++++++------ grid/factory/v2/Stairs2.h | 19 +++++++-- 3 files changed, 83 insertions(+), 16 deletions(-) diff --git a/grid/GridNode.h b/grid/GridNode.h index 863ee43..6b21e87 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -54,6 +54,7 @@ public: static const uint8_t TYPE_OUTDOOR = 100; + public: /** ctor */ diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 589dd5d..a42ed81 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -42,6 +42,8 @@ private: Elevators elevators; + std::vector withinRemovePolygon; + bool _buildStairs = true; bool _removeIsolated = true; @@ -60,6 +62,14 @@ public: void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;} + /** does the given grid-node have a stair-neighbor? */ + bool hasStairNeighbor(const T& node) const { + for (const T& n : grid.neighbors(node)) { + if (n.getType() == GridNode::TYPE_STAIR) {return true;} + } + return false; + } + /** build using the given map */ void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) { @@ -93,6 +103,27 @@ public: } } + // remove nodes within remove-OutlinePolygons + // this must happen AFTER stairs have been added, otherwise stairs might not be connectable due to removed/missing nodes. + // also, within this loop we prevent the deltion of nodes that are a direct neighbor of a stair! + // [otherwise the same thing would happen again!] + if (true) { + + for (const GridPoint& gp : withinRemovePolygon) { + T* n = (T*) grid.getNodePtrFor(gp); + + // delete if node is present and is no direct neighbor of a stair + if (n && !hasStairNeighbor(*n)) { + grid.remove(*n); + } + + } + + // remove all nodes that were just marked for removal + grid.cleanup(); + + } + // remove isolated nodes if (_removeIsolated) { if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");} @@ -114,31 +145,36 @@ public: return bb; } - enum class PartOfOutline { - NO, - INDOOR, - OUTDOOR, + struct PartOfOutline { + bool contained = false; // contained within at-least one polygon? + bool markedForRemove = false; // part of a "to-be-removed" polygon? + bool outdoor = false; // otherwise: indoor }; /** get the part of outline the given location belongs to. currently: none, indoor, outdoor */ 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; + PartOfOutline res; + res.contained = false; + res.markedForRemove = false; // process every outline polygon for (Floorplan::FloorOutlinePolygon* poly : outline) { HelperPoly pol(*poly); if (pol.contains(Point2(x_cm, y_cm))) { + // mark as "contained" + res.contained = true; + // belongs to a "remove" polygon? -> directly ignore this location! if (poly->method == Floorplan::OutlineMethod::REMOVE) { - return PartOfOutline::NO; + res.markedForRemove = true; } // belongs to a "add" polygon? -> remember until all polygons were checked // [might still belong to a "remove" polygon] - res = poly->outdoor ? PartOfOutline::OUTDOOR : PartOfOutline::INDOOR; + res.outdoor = poly->outdoor; } } @@ -173,7 +209,7 @@ public: // does the outline-polygon contain this position? const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline); - if (part == PartOfOutline::NO) {continue;} + if (!part.contained) {continue;} // check intersection with the floorplan GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); @@ -188,6 +224,11 @@ public: updateType(t, part, bbox, floor); grid.add(t); + // node part of remove-region? + // if so, remove >>AFTER<< stairs have been added + if (part.markedForRemove) { + withinRemovePolygon.push_back(t); + } // debug ++numNodes; @@ -395,7 +436,19 @@ public: Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false); Log::tick(); for (T& n2 : grid) { - if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);} + if (set.find(n2.getIdx()) == set.end()) { + + // sanity check + // wouldn't make sense that a stair-node is removed.. + // maybe something went wrong elsewhere??? + Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!"); + Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!"); + //Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!"); + + // proceed ;) + grid.remove(n2); + + } } Log::tock(); @@ -510,10 +563,10 @@ private: static inline void updateType(T& t, const PartOfOutline part, const GridNodeBBox& bbox, const Floorplan::Floor* floor) { // first, assume the type of the outline polygon - switch (part) { - case PartOfOutline::OUTDOOR: t.setType(GridNode::TYPE_OUTDOOR); break; - case PartOfOutline::INDOOR: t.setType(GridNode::TYPE_FLOOR); break; - default: throw Exception("should not happen"); + if (part.outdoor) { + t.setType(GridNode::TYPE_OUTDOOR); + } else { + //t.setType(GridNode::TYPE_FLOOR); } // hereafter, process each obstacle and mark doors diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index 1e4e6cc..2372d03 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -157,18 +157,24 @@ public: // remember the z-position of the already-existing grid-node we connected the stair to connectedWithHeights.insert(grid[sn.gridIdx].z_cm); + // mark the node as stair-node + //grid[sn.gridIdx].setType(GridNode::TYPE_STAIR); + } else { sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm)); // check if there is a nearby floor-node to delete + // -> remove nodes directly above/below the stair const int deleteDist_cm = 100; const float distToBelow = gp.z_cm - floor->getStartingZ()*100; const float distToAbove = floor->getEndingZ()*100 - gp.z_cm; - if (distToBelow > gs_cm && distToBelow < deleteDist_cm) { + //if (distToBelow > gs_cm && distToBelow < deleteDist_cm) { + if (distToBelow > 0 && distToBelow < deleteDist_cm) { T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100)); if (n) {toDelete.push_back(n);} - } else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) { + //} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) { + } else if (distToAbove > 0 && distToAbove < deleteDist_cm) { T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100)); if (n) {toDelete.push_back(n);} } @@ -181,7 +187,11 @@ public: } // sanity check - Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!"); + // connectedWithHeights should contain 2 entries: + // one for the starting floor + // one for the ending floor + // this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected + Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!"); // now connect all new nodes with their neighbors // do not perform normal grid-connection but examine the nodes within the generated vector @@ -225,6 +235,9 @@ public: void finalize() { + //std::cout << "stairs.finalize() crashes!" << std::endl; + //return; + // delete all pending nodes and perform a cleanup if (!toDelete.empty()) { for (T* n : toDelete) {grid.remove(*n);} From 9ea7da557b85131ad253a39a8d3e65dfce236c29 Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 1 Jun 2017 15:58:58 +0200 Subject: [PATCH 43/43] fixed some grid-factory issues added some new attributes minor changes --- floorplan/v2/Floorplan.h | 7 ++++--- floorplan/v2/FloorplanReader.h | 3 ++- floorplan/v2/FloorplanWriter.h | 1 + geo/Line2.h | 11 +++++++---- grid/factory/v2/Elevators.h | 9 +++++++-- grid/factory/v2/GridFactory.h | 29 ++++++++++++++++++++++++++--- 6 files changed, 47 insertions(+), 13 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index f1dcaf4..8732839 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -253,7 +253,7 @@ namespace Floorplan { int id; //TODO: this value can be changed and isn't set incremental within the indoormap Point3 pos; // TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height! GroundTruthPoint() : id(), pos() {;} - GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;} + GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;} const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);} bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);} }; @@ -319,8 +319,9 @@ namespace Floorplan { ObstacleType type; Point2 from; Point2 to; - FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to) {;} - FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2) {;} + float thickness_m; + FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m) {;} + FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m) {;} }; /** circle obstacle */ diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 63a6ac7..b9cb84e 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -402,7 +402,8 @@ namespace Floorplan { parseObstacleType(el->Attribute("type")), parseMaterial(el->Attribute("material")), el->FloatAttribute("x1"), el->FloatAttribute("y1"), - el->FloatAttribute("x2"), el->FloatAttribute("y2") + el->FloatAttribute("x2"), el->FloatAttribute("y2"), + (el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15) // default wall thickness in m ); } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 4294ff4..e8eff0e 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -330,6 +330,7 @@ namespace Floorplan { obstacle->SetAttribute("y1", line->from.y); obstacle->SetAttribute("x2", line->to.x); obstacle->SetAttribute("y2", line->to.y); + obstacle->SetAttribute("thickness", line->thickness_m); obstacles->InsertEndChild(obstacle); } diff --git a/geo/Line2.h b/geo/Line2.h index b8ee038..f7e1b93 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -14,15 +14,18 @@ public: public: - /** empty ctor */ + /** empty ctor */ Line2() : p1(), p2() {;} - /** value ctor */ - Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;} + /** value ctor */ + Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;} - /** value ctor */ + /** value ctor */ Line2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;} + + Line2 operator * (const float v) const {return Line2(p1*v, p2*v);} + // bool getSegmentIntersection(const Line& other) const { // static K::Point p; // return K::Line::getSegmentIntersection(other, p); diff --git a/grid/factory/v2/Elevators.h b/grid/factory/v2/Elevators.h index ae20b3a..a10f0de 100644 --- a/grid/factory/v2/Elevators.h +++ b/grid/factory/v2/Elevators.h @@ -79,8 +79,13 @@ public: // connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm) for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) { - const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); - const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); + GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + + // above the ending floor? -> snap to ending floor + // note: this one is needed if the floor-heights are not dividable by the grid-size + if (gp.z_cm > floor->getEndingZ()*100) {gp.z_cm = floor->getEndingZ()*100;} + Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node"); Assert::isTrue(grid.hasNodeFor(gp), "missing node"); T& n1 = (T&) grid.getNodeFor(gpBelow); diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index a42ed81..8d576d1 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -487,7 +487,23 @@ private: private: - + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + static std::vector getThickLines(const Floorplan::FloorObstacleLine* line) { + //const Line2 base(line->from*100, line->to*100); + const float thickness_m = line->thickness_m; + const Point2 dir = (line->to - line->from); // obstacle's direction + const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) + const Point2 p1 = line->from + perp * thickness_m/2; // start-up + const Point2 p2 = line->from - perp * thickness_m/2; // start-down + const Point2 p3 = line->to + perp * thickness_m/2; // end-up + const Point2 p4 = line->to - perp * thickness_m/2; // end-down + return { + Line2(p1, p2), + Line2(p3, p4), + Line2(p2, p4), + Line2(p1, p3), + }; + } /** does the bbox intersect with any of the floor's walls? */ static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) { @@ -499,8 +515,15 @@ private: // depends on the type of obstacle if (dynamic_cast(fo)) { const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo; - const Line2 l2(line->from*100, line->to*100); - if (bbox.intersects(l2)) {return true;} + + // old method (does not support thickness) + //const Line2 l2(line->from*100, line->to*100); + //if (bbox.intersects(l2)) {return true;} + const std::vector lines = getThickLines(line); + for (const Line2& l : lines) { + if (bbox.intersects(l*100)) {return true;} + } + } else if (dynamic_cast(fo)) { const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;