From c083aae476f9ee1ce02488c2f0910a62cfc05aaa Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 4 Nov 2016 17:25:49 +0100 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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