From cad299cd7cf2e467e6f964d14f4cb7d3fd2b62ec Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 10 Mar 2017 13:47:12 +0100 Subject: [PATCH] started adding earth-mapping some new helper methods added support for floorplan metadata load/save --- floorplan/v2/Floorplan.h | 120 +++++++++++++++++++++++++++++---- floorplan/v2/FloorplanReader.h | 43 ++++++++---- floorplan/v2/FloorplanWriter.h | 14 +++- geo/EarthMapping.h | 75 +++++++++++++++++++++ geo/EarthPos.h | 21 ++++++ geo/Point2.h | 7 ++ 6 files changed, 252 insertions(+), 28 deletions(-) create mode 100644 geo/EarthMapping.h create mode 100644 geo/EarthPos.h diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 1d624e3..ea88b50 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -9,6 +9,7 @@ #include "../../geo/Point3.h" #include "../../geo/Point2.h" +#include "../../geo/EarthPos.h" namespace Floorplan { @@ -21,20 +22,86 @@ namespace Floorplan { /** a free key-value meta element */ struct Meta { + struct KeyVal { + std::string key; + std::string val; + KeyVal() {;} + KeyVal(const std::string& key, const std::string& val) : key(key), val(val) {;} + }; + const std::string EMPTY = ""; - std::unordered_map params; + +// std::unordered_map params; +// const std::string& getVal(const std::string& key) const { +// const auto it = params.find(key); +// return (it == params.end()) ? (EMPTY) : (it->second); +// } + + std::vector params; + + KeyVal* getKV(const std::string& key) { + for (KeyVal& kv : params) { + if (kv.key == key) {return &kv;} + } + return nullptr; + } + + const KeyVal* getKV(const std::string& key) const { + for (const KeyVal& kv : params) { + if (kv.key == key) {return &kv;} + } + return nullptr; + } const std::string& getVal(const std::string& key) const { - const auto it = params.find(key); - return (it == params.end()) ? (EMPTY) : (it->second); + static const std::string EMPTY = ""; + const KeyVal* kv = getKV(key); + return (kv) ? (kv->val) : (EMPTY); + } + void setVal(const std::string& key, const std::string& val) {(*this)[key] = val;} + + void add(const std::string& key, const std::string& val) {params.push_back(KeyVal(key,val));} + + std::string& operator [] (const std::string& key) { + KeyVal* kv = getKV(key); + if (!kv) {params.push_back(KeyVal(key, ""));} + return getKV(key)->val; } - void setVal(const std::string& key, const std::string& val) {params[key] = val;} float getFloat(const std::string& key) const { return std::stof(getVal(key)); } - void setFloat(const std::string& key, const float val) { params[key] = std::to_string(val); } + void setFloat(const std::string& key, const float val) { (*this)[key] = std::to_string(val); } int getInt(const std::string& key) const { return std::stoi(getVal(key)); } - void setInt(const std::string& key, const int val) { params[key] = std::to_string(val); } + void setInt(const std::string& key, const int val) { (*this)[key] = std::to_string(val); } + + + + size_t size() const {return params.size();} + + const std::string& getKey(const int idx) const {return params[idx].key;} + const std::string& getVal(const int idx) const {return params[idx].val;} + + void set(const int idx, const KeyVal& kv) {params[idx] = kv;} + void setKey(const int idx, const std::string& key) {params[idx].key = key;} + void setVal(const int idx, const std::string& val) {params[idx].val = val;} + + /** delete the idx-th entry */ + void deleteEntry(const int idx) {params.erase(params.begin()+idx);} + + }; + + class HasMeta { + + Meta* meta = nullptr; + + public: + + Meta* getMeta() const {return meta;} + + void setMeta(Meta* meta) { + if (this->meta) {delete this->meta;} + this->meta = meta; + } }; @@ -168,14 +235,14 @@ namespace Floorplan { }; /** an AccessPoint located somewhere on a floor */ - struct AccessPoint { + struct AccessPoint : public HasMeta { std::string name; std::string mac; Point3 pos; // z is relative to the floor's height struct Model { - float txp; - float exp; - float waf; + float txp = 0; + float exp = 0; + float waf = 0; } model; AccessPoint() : name(), mac(), pos() {;} AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;} @@ -388,8 +455,7 @@ namespace Floorplan { /** base-class for stairs */ - struct Stair { - Meta* meta = nullptr; + struct Stair : public HasMeta { virtual std::vector getParts() const = 0; }; @@ -440,7 +506,7 @@ namespace Floorplan { /** an image file that can be added to the map */ - struct UnderlayImage { + struct UnderlayImage { std::string name; std::string filename; Point2 anchor; @@ -450,16 +516,44 @@ namespace Floorplan { + /** one correspondence point: earth <-> map */ + struct EarthPosMapPos { + + EarthPos posOnEarth; + + Point3 posOnMap_m; + + /** ctor */ + EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;} + + }; + + + /** describe the floorplan's location on earth */ + struct EarthRegistration { + + /** all available correspondences: earth <-> map */ + std::vector correspondences; + + }; + /** describes the whole indoor map */ struct IndoorMap { float width; float depth; + std::vector floors; + /** mapping: floorplan <-> earth */ + EarthRegistration earthReg; + IndoorMap() {;} + /** no copy */ IndoorMap(const IndoorMap& o) = delete; + + /** no copy assign */ void operator = (const IndoorMap& o) = delete; }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 2af6df0..8522091 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -13,6 +13,7 @@ namespace Floorplan { using XMLAttr = tinyxml2::XMLAttribute; using XMLElem = tinyxml2::XMLElement; + using XMLNode = tinyxml2::XMLNode; /** * read an IndoorMaps from XML-data @@ -31,7 +32,13 @@ namespace Floorplan { setlocale(LC_NUMERIC, "C"); tinyxml2::XMLDocument doc; const tinyxml2::XMLError res = doc.LoadFile(file.c_str()); - if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);} + if (res != tinyxml2::XMLError::XML_SUCCESS) { + throw Exception( + std::string() + "error while loading XML " + file + "\n" + + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ); + } return parse(doc); } @@ -41,7 +48,13 @@ namespace Floorplan { setlocale(LC_NUMERIC, "C"); tinyxml2::XMLDocument doc; const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length()); - if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");} + if (res != tinyxml2::XMLError::XML_SUCCESS) { + throw Exception( + std::string() + "error while parsing XML\n" + + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ); + } return parse(doc); } @@ -157,9 +170,7 @@ namespace Floorplan { // stair meta information? const XMLElem* meta = el->FirstChildElement("meta"); - if (meta) { - stair->meta = parseMetaElement(meta); - } + if (meta) {stair->setMeta(parseMetaElement(meta));} return stair; @@ -227,10 +238,16 @@ namespace Floorplan { /** parse one element */ static Meta* parseMetaElement(const XMLElem* n) { Meta* elem = new Meta(); - const XMLAttr* attr = n->FirstAttribute(); - while (attr) { - elem->params[attr->Name()] = attr->Value(); - attr = attr->Next(); +// const XMLAttr* attr = n->FirstAttribute(); +// while (attr) { +// elem->setVal(attr->Name(), attr->Value()); +// attr = attr->Next(); +// } + const XMLElem* sub = n->FirstChildElement(); + while(sub) { + // abc + elem->add(sub->Attribute("key"), sub->FirstChild()->Value()); + sub = sub->NextSiblingElement(); } return elem; } @@ -244,6 +261,8 @@ namespace Floorplan { ap->model.txp = n->FloatAttribute("mdl_txp"); ap->model.exp = n->FloatAttribute("mdl_exp"); ap->model.waf = n->FloatAttribute("mdl_waf"); + const XMLElem* meta = n->FirstChildElement("meta"); + if (meta) {ap->setMeta(parseMetaElement(meta));} return ap; } @@ -262,9 +281,9 @@ namespace Floorplan { Beacon* b = new Beacon(); b->mac = n->Attribute("mac"); b->name = n->Attribute("name"); - b->major = n->Attribute("major"); - b->minor = n->Attribute("minor"); - b->uuid = n->Attribute("uuid"); + b->major = n->Attribute("major") ? n->Attribute("major") : ""; + b->minor = n->Attribute("minor") ? n->Attribute("minor") : ""; + b->uuid = n->Attribute("uuid") ? n->Attribute("uuid") : ""; b->model.txp = n->FloatAttribute("mdl_txp"); b->model.exp = n->FloatAttribute("mdl_exp"); b->model.waf = n->FloatAttribute("mdl_waf"); diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 3a927a0..00c672c 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -105,7 +105,7 @@ namespace Floorplan { XMLElem* stairs = doc.NewElement("stairs"); for (const Stair* _stair : mf->stairs) { XMLElem* elem = doc.NewElement("stair"); - addMetaElement(doc, elem, _stair->meta); + addMetaElement(doc, elem, _stair->getMeta()); if (dynamic_cast(_stair)) { const StairFreeform* stair = (StairFreeform*) _stair; elem->SetAttribute("type", 0); // TODO: other types? @@ -156,6 +156,7 @@ namespace Floorplan { accesspoint->SetAttribute("mdl_txp", ap->model.txp); accesspoint->SetAttribute("mdl_exp", ap->model.exp); accesspoint->SetAttribute("mdl_waf", ap->model.waf); + addMetaElement(doc, accesspoint, ap->getMeta()); accesspoints->InsertEndChild(accesspoint); } floor->InsertEndChild(accesspoints); @@ -219,8 +220,15 @@ namespace Floorplan { static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) { if (meta == nullptr) {return;} // nothing to add XMLElem* elem = doc.NewElement("meta"); - for (auto it : meta->params) { - elem->Attribute(it.first.c_str(), it.second.c_str()); +// for (auto it : meta->params) { +// elem->Attribute(it.first.c_str(), it.second.c_str()); +// } + for (const Meta::KeyVal& kv : meta->params) { + // abc + XMLElem* subElem = doc.NewElement("entry"); + subElem->SetAttribute("key", kv.key.c_str()); + subElem->SetText(kv.val.c_str()); + elem->InsertEndChild(subElem); } other->InsertEndChild(elem); } diff --git a/geo/EarthMapping.h b/geo/EarthMapping.h new file mode 100644 index 0000000..8ff10da --- /dev/null +++ b/geo/EarthMapping.h @@ -0,0 +1,75 @@ +#ifndef EARTHMAPPING_H +#define EARTHMAPPING_H + +#include "Point3.h" +#include "EarthPos.h" + +/** + * mapping between positions on earth and positions within the floorplan + */ +class EarthMapping { + +private: + + /** one 3D position within the floorplan */ + Point3 center_map_m; + + /** corresponding 3D position on earth */ + EarthPos center_earth; + + /** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */ + float rotation_deg; + + double m_per_deg_lat; + double m_per_deg_lon; + + +public: + + void build() { + + // TODO + + } + + /** convert from map-coordinates to earth-coordinates */ + EarthPos mapToWorld(const Point3 mapPos_m) const { + + Point3 pos = mapPos_m; + + // move to (0,0,0) + pos -= center_map_m; + + // undo the rotation + const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI); + + // convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center + const double lat = cenLat + (xy.y / m_per_deg_lat); + const double lon = cenLon + (xy.x / m_per_deg_lon); + const float height = pos.z; + + // done + return EarthPos(lat, lon, height); + + } + + /** convert from earth-coordinates to map-coordinates */ + Point3 worldToMap(const EarthPos earthPos) const { + + const double y_m = +(lat-cenLat) * m_per_deg_lat; + const double x_m = +(lon-cenLon) * m_per_deg_lon; + + // rotate (our map is axis aligned) + Point2 pos(x_m, y_m); + pos = pos.rotated(rotDeg / 180 * M_PI); + + // apply movement + pos += mapCenter_m; + + return pos; + + } + +}; + +#endif // EARTHMAPPING_H diff --git a/geo/EarthPos.h b/geo/EarthPos.h new file mode 100644 index 0000000..bb6ccc0 --- /dev/null +++ b/geo/EarthPos.h @@ -0,0 +1,21 @@ +#ifndef EARTHPOS_H +#define EARTHPOS_H + +/** describes the location on the earth's surface */ +struct EarthPos { + + double lat; + + double lon; + + /** height above sea level */ + float height; + + /** ctor with values */ + EarthPos(const double lat, const double lon, const float height) : lon(lon), lat(lat), height(height) { + ; + } + +}; + +#endif // EARTHPOS_H diff --git a/geo/Point2.h b/geo/Point2.h index d226d5c..2d58178 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -17,6 +17,8 @@ struct Point2 { /** ctor */ Point2(const float x, const float y) : x(x), y(y) {;} + Point2 operator - () const {return Point2(-x, -y);} + Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);} @@ -60,4 +62,9 @@ struct Point2 { }; +inline void swap(Point2& p1, Point2& p2) { + std::swap(p1.x, p2.x); + std::swap(p1.y, p2.y); +} + #endif // POINT2_H