From e10ba2cfd9405da2c2452f66174f36bc4f7bb54b Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 1 Dec 2016 19:48:27 +0100 Subject: [PATCH] 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;