FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

started adding earth-mapping

Browse Source 
some new helper methods
added support for floorplan metadata load/save
This commit is contained in:
FrankE
2017-03-10 13:47:12 +01:00
parent 2255904385
commit cad299cd7c
6 changed files with 252 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
floorplan/v2/Floorplan.h
View File

@@ -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<std::string, std::string> params;
// std::unordered_map<std::string, std::string> 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<KeyVal> 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<StairPart> 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<EarthPosMapPos> correspondences;
};
/** describes the whole indoor map */
struct IndoorMap {
float width;
float depth;
std::vector<Floor*> floors;
/** mapping: floorplan <-> earth */
EarthRegistration earthReg;
IndoorMap() {;}
/** no copy */
IndoorMap(const IndoorMap& o) = delete;
/** no copy assign */
void operator = (const IndoorMap& o) = delete;
};

43
floorplan/v2/FloorplanReader.h
View File

@@ -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 <meta> 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) {
// <entry key="123">abc</entry>
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");

14
floorplan/v2/FloorplanWriter.h
View File

@@ -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<const StairFreeform*>(_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) {
// <entry key="123">abc</entry>
XMLElem* subElem = doc.NewElement("entry");
subElem->SetAttribute("key", kv.key.c_str());
subElem->SetText(kv.val.c_str());
elem->InsertEndChild(subElem);
}
other->InsertEndChild(elem);
}

75
geo/EarthMapping.h Normal file
View File

@@ -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

21
geo/EarthPos.h Normal file
View File

@@ -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

7
geo/Point2.h
View File

@@ -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