This repository has been archived on 2020-04-08. You can view files and clone it, but cannot push or open issues or pull requests.
Files
Indoor/floorplan/v2/Floorplan.h
k-a-z-u 19d9ce3961 added floorplan support for object-sizing
added floorplan support for different wall-heights
minor fixes/changes
2018-07-04 20:28:15 +02:00

636 lines
19 KiB
C++

#ifndef FLOORPLAN2_H
#define FLOORPLAN2_H
#include <algorithm>
#include <string>
#include <vector>
#include <initializer_list>
#include <unordered_map>
#include "../../geo/Point3.h"
#include "../../geo/Point2.h"
#include "../../geo/EarthPos.h"
namespace Floorplan {
/** convert string to upper case */
inline std::string toUpperCase(std::string str) {
std::transform(str.begin(), str.end(), str.begin(), ::toupper);
return str;
}
/** 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;
// 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 {
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;
}
float getFloat(const std::string& key) const { return std::stof(getVal(key)); }
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) { (*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;
}
};
/** 2D polygon */
struct Polygon2 {
std::vector<Point2> points;
Polygon2() : points() {;}
Polygon2(const std::initializer_list<Point2> lst) : points(lst) {;}
bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());}
};
/** 3D quad */
struct Quad3 {
Point3 p1;
Point3 p2;
Point3 p3;
Point3 p4;
Quad3(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) : p1(p1), p2(p2), p3(p3), p4(p4) {;}
Quad3 operator * (const float v) const {return Quad3(p1*v, p2*v, p3*v, p4*v);}
const Point3& operator [] (const int idx) const {
switch (idx) {
case 0: return p1;
case 1: return p2;
case 2: return p3;
case 3: return p4;
default: throw Exception("out of bounds");
}
}
/** same z-value for all points? */
bool isLeveled() const {
return (p1.z == p2.z) && (p2.z == p3.z) && (p3.z == p4.z);
}
};
/** additional type-info for obstacles */
enum class ObstacleType {
UNKNOWN,
WALL,
WINDOW,
HANDRAIL,
PILLAR,
_END,
};
/** available door types */
enum class DoorType {
UNKNOWN,
SWING, // normal
DOUBLE_SWING,
SLIDE, // schiebetuer
DOUBLE_SLIDE,
REVOLVING, // drehtuer
_END,
};
/** all supported material types */
enum class Material {
UNKNOWN,
CONCRETE,
WOOD,
DRYWALL,
GLASS,
METAL,
METALLIZED_GLAS,
_END,
};
enum class POIType {
ROOM,
};
/** types of outlines. either add or remove the selected region */
enum class OutlineMethod {
ADD,
REMOVE,
_END,
};
struct Floor;
struct FloorOutlinePolygon;
struct FloorObstacle;
struct AccessPoint;
struct Beacon;
struct FingerprintLocation;
struct FloorRegion;
struct UnderlayImage;
struct POI;
struct Stair;
struct Elevator;
struct GroundTruthPoint;
struct FloorOutline : public std::vector<FloorOutlinePolygon*> {
bool enabled = true;
};
struct FloorObstacles : public std::vector<FloorObstacle*> {
bool enabled = true;
};
struct FloorAccessPoints : public std::vector<AccessPoint*> {
bool enabled = true;
};
struct FloorBeacons : public std::vector<Beacon*> {
bool enabled = true;
};
struct FloorFingerprintLocations : public std::vector<FingerprintLocation*> {
bool enabled = true;
};
struct FloorRegions : public std::vector<FloorRegion*> {
bool enabled = true;
};
struct FloorUnderlays : public std::vector<UnderlayImage*> {
bool enabled = true;
};
struct FloorPOIs : public std::vector<POI*> {
bool enabled = true;
};
struct FloorStairs : public std::vector<Stair*> {
bool enabled = true;
};
struct FloorElevators : public std::vector<Elevator*> {
bool enabled = true;
};
struct FloorGroundTruthPoints : public std::vector<GroundTruthPoint*> {
bool enabled = true;
};
/** describes one floor within the map, starting at a given height */
struct Floor {
bool enabled = true;
float atHeight; // the floor's starting height
float height; // the floor's total height (from start)
std::string name; // the floor's name
FloorOutline outline; // the floor's outline (ground)
FloorObstacles obstacles; // all obstacles (wall, door, window, ..) within the floor
FloorRegions regions; // all regions within the floor (rooms, ...)
FloorAccessPoints accesspoints;
FloorBeacons beacons;
FloorFingerprintLocations fpLocations; // potential fingerprint locations
FloorUnderlays underlays; // underlay images (used for map-building)
FloorPOIs pois; // POIs within the floor
FloorStairs stairs; // all stairs within one floor
FloorElevators elevators; // all elevators within one floor
FloorGroundTruthPoints gtpoints; // all ground truth points within one floor
//FloorKeyValue other; // other, free elements
Floor() {;}
Floor(const Floor& o) = delete;
void operator = (const Floor& o) = delete;
float getStartingZ() const {return atHeight;}
float getEndingZ() const {return atHeight + height;}
};
/** location for fingerprint measurements */
struct FingerprintLocation : public HasMeta {
std::string name;
Point2 posOnFloor;
float heightAboveFloor = 0;
FingerprintLocation() {;}
FingerprintLocation(const std::string& name, const Point2 posOnFloor, const float heightAboveFloor) : name(name), posOnFloor(posOnFloor), heightAboveFloor(heightAboveFloor) {;}
Point3 getPosition(const Floor& floor) const {return Point3(posOnFloor.x, posOnFloor.y, floor.atHeight + heightAboveFloor);}
};
/** a POI located somewhere on a floor */
struct POI {
POIType type;
std::string name;
Point2 pos;
POI() : type(), name(), pos() {;}
POI(const POIType type, const std::string& name, const Point2& pos) : type(type), name(name), pos(pos) {;}
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
Point3 pos; // TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
GroundTruthPoint() : id(), pos() {;}
GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;}
const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
};
/** an AccessPoint located somewhere on a floor */
struct AccessPoint : public HasMeta {
std::string name;
std::string mac;
Point3 pos; // z is relative to the floor's height
struct Model {
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) {;}
bool operator == (const AccessPoint& 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
};
/** a beacon located somewhere on a floor */
struct Beacon {
std::string name;
std::string mac;
std::string major;
std::string minor;
std::string uuid;
Point3 pos; // z is relative to the floor's height
struct Model {
float txp;
float exp;
float waf;
} model;
Beacon() : name(), mac(), pos() {;}
Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground
};
/** a polygon denoting a floor's outline. is either added or removed */
struct FloorOutlinePolygon {
OutlineMethod method;
std::string name;
Polygon2 poly;
bool outdoor; // special marker
FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly(), outdoor(false) {;}
FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly, bool outdoor) : method(method), name(name), poly(poly), outdoor(outdoor) {;}
bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly) && (o.outdoor == outdoor);}
};
/** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */
struct FloorObstacle {
Material material;
FloorObstacle() : material() {;}
FloorObstacle(const Material material) : material(material) {;}
virtual ~FloorObstacle() {;}
};
/** line obstacle */
struct FloorObstacleLine : public FloorObstacle {
ObstacleType type;
Point2 from;
Point2 to;
float thickness_m;
float height_m = 0; // 0 = floor's height
FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_m) {;}
FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;}
};
/** circle obstacle */
struct FloorObstacleCircle : public FloorObstacle {
Point2 center;
float radius;
float height = 0; // 0 = floor's height
FloorObstacleCircle(const Material material, const Point2 center, const float radius, const float height=0) : FloorObstacle(material), center(center), radius(radius), height(height) {;}
FloorObstacleCircle(const Material material, const float cx, const float cy, const float radius, const float height=0) : FloorObstacle(material), center(cx,cy), radius(radius), height(height) {;}
//float getHeight(const Floor* f) const {return (height > 0) ? (height) : (f->height);}
};
/** door obstacle */
struct FloorObstacleDoor : public FloorObstacle {
DoorType type;
Point2 from;
Point2 to;
float height;
bool swap;
FloorObstacleDoor(const DoorType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to), height(2.1), swap(false) {;}
FloorObstacleDoor(const DoorType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float height, const bool swap) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), height(height), swap(swap) {;}
float getSize() const {return (to-from).length();}
};
/** 3D obstacle */
struct FloorObstacleObject : public FloorObstacle {
std::string file;
Point3 pos;
Point3 rot;
Point3 scale = Point3(1,1,1);
FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot, const Point3 scale) : file(file), pos(pos), rot(rot), scale(scale) {;}
};
/** one region (e.g. a room) within the floor, described using a polygon */
struct FloorRegion {
std::string name;
Polygon2 poly;
FloorRegion() : name(), poly() {;}
FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;}
};
static Point3 xy0(const Point2 p) {
return Point3(p.x, p.y, 0);
}
/** describes a plane as starting point, ending point and fixed width */
struct StairPart {
/** z is relative to the floor's height */
Point3 start;
/** z is relative to the floor's height */
Point3 end;
/** the width for this element */
float width;
/** whether to connect this element with the previous one */
bool connectWithPrev;
StairPart() {;}
StairPart(const Point3 start, const Point3 end, const float width) : start(start), end(end), width(width), connectWithPrev(false) {;}
StairPart(Point3 center, float deg, float length, float width, float height) {
this->start = Point3(-length/2, 0, -height/2);
this->end = Point3(+length/2, 0, +height/2);
this->width = width;
rotate(deg/180.0*M_PI);
move(center);
}
/** convenience method for array access [0:1] to start and end */
Point3& operator [] (const int idx) {
switch (idx) {
case 0: return start;
case 1: return end;
default: throw "error";
}
}
void rotate(const float rad) {
const float ca = std::cos(rad);
const float sa = std::sin(rad);
start = Point3(ca*start.x - sa*start.y, sa*start.x + ca * start.y, start.z);
end = Point3(ca*end.x - sa*end.y, sa*end.x + ca * end.y, end.z);;
}
void move(const Point3 p) {
start += p;
end += p;
}
};
/** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */
static void snapQuads(std::vector<Floorplan::Quad3>& quads, const float maxDist) {
for (Floorplan::Quad3& quad1 : quads) {
for (int i1 = 0; i1 < 4; ++i1) {
Point3& p1 = (Point3&) quad1[i1];
for (const Floorplan::Quad3& quad2 : quads) {
if (&quad1 == &quad2) {continue;}
for (int i1 = 0; i1 < 4; ++i1) {
Point3& p2 = (Point3&) quad2[i1];
if (p1.getDistance(p2) < maxDist) {
const Point3 p3 = (p1+p2) / 2;
p1 = p3;
p2 = p3;
break;
}
}
}
}
}
}
/** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */
static std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor, const float s = 1.0f) {
std::vector<Quad3> vec;
for (const StairPart& part : parts) {
const float width = part.width;
const Point3 start = part.start;
const Point3 end = part.end;
const Point3 dir = end - start; // direction vector
const Point2 dir2(dir.x, dir.y); // direction without height (just 2D)
const Point2 perp = dir2.perpendicular(); // perendicular vector
const Point2 perpN = perp / perp.length(); // normalized perpendicular vector
const Point3 p1 = start + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p2 = start - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p3 = end - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p4 = end + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Quad3 q(p1,p2,p3,p4);
vec.push_back(q);
// connect
if (part.connectWithPrev) {
if (vec.size() >= 2) {
Quad3& a = vec[vec.size()-2];
Quad3& b = vec[vec.size()-1];
Point3 pa = (a.p3 + b.p2)/2; a.p3 = pa; b.p2 = pa;
Point3 pb = (a.p4 + b.p1)/2; a.p4 = pb; b.p1 = pb;
}
}
}
snapQuads(vec, 0.05);
return vec;
}
/** base-class for stairs */
struct Stair : public HasMeta {
virtual std::vector<StairPart> getParts() const = 0;
};
/** 3-part stair, up[left]->platform->up[right] */
struct StairFreeform : public Stair {
std::vector<StairPart> parts;
StairFreeform() {;}
std::vector<StairPart> getParts() const {return parts;}
};
/**
* describes an elevator between two floors
* so: the height depends on the height of the floor!
*/
struct Elevator {
/** the elevator's center */
Point2 center;
/** the elevator's width (in meter) */
float width;
/** the elevator's depth (in meter) */
float depth;
/** the elevator's rotation (in radians) */
float rotation;
/** the elevator's height (from its starting position) */
float height_m;
/** get the 4 corner points for the elevator */
Polygon2 getPoints() const {
const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center;
const Point2 p2 = Point2(-width/2, +depth/2).rotated(rotation) + center;
const Point2 p3 = Point2(-width/2, -depth/2).rotated(rotation) + center;
const Point2 p4 = Point2(+width/2, -depth/2).rotated(rotation) + center;
Polygon2 poly;
poly.points = {p1, p2, p3, p4};
return poly;
}
/** empty ctor */
Elevator() : center(), width(2), depth(2), rotation(0) {;}
/** ctor */
Elevator(const Point2 center) : center(center), width(2), depth(2), rotation(0) {;}
};
/** an image file that can be added to the map */
struct UnderlayImage {
std::string name;
std::string filename;
Point2 anchor;
float scaleX;
float scaleY;
};
/** one correspondence point: earth <-> map */
struct EarthPosMapPos {
EarthPos posOnEarth;
Point3 posOnMap_m;
/** empty ctor */
EarthPosMapPos() : posOnEarth(), 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 {
bool enabled = true;
/** 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;
};
}
#endif // FLOORPLAN2_H