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

merged

Browse Source
This commit is contained in:
toni
2018-02-12 13:48:11 +01:00
parent 0fef66ac62 2a923dfabc
commit 632b1ca7a7
22 changed files with 1803 additions and 1055 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -99,8 +99,8 @@ ADD_EXECUTABLE(
${SOURCES}
)
SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
INCLUDE_DIRECTORIES(/usr/include/libnl3/)
#SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
#INCLUDE_DIRECTORIES(/usr/include/libnl3/)
#SET(EXTRA_LIBS ${EXTRA_LIBS} iw)
# needed external libraries

46
floorplan/v2/Floorplan.h
View File

@@ -191,17 +191,39 @@ namespace Floorplan {
struct Elevator;
struct GroundTruthPoint;
using FloorOutline = std::vector<FloorOutlinePolygon*>;
using FloorObstacles = std::vector<FloorObstacle*>;
using FloorAccessPoints = std::vector<AccessPoint*>;
using FloorBeacons = std::vector<Beacon*>;
using FloorFingerprintLocations = std::vector<FingerprintLocation*>;
using FloorRegions = std::vector<FloorRegion*>;
using FloorUnderlays = std::vector<UnderlayImage*>;
using FloorPOIs = std::vector<POI*>;
using FloorStairs = std::vector<Stair*>;
using FloorElevators = std::vector<Elevator*>;
using FloorGroundTruthPoints = std::vector<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 {
@@ -567,6 +589,8 @@ namespace Floorplan {
/** describe the floorplan's location on earth */
struct EarthRegistration {
bool enabled = true;
/** all available correspondences: earth <-> map */
std::vector<EarthPosMapPos*> correspondences;

37
floorplan/v2/FloorplanReader.h
View File

@@ -144,8 +144,8 @@ namespace Floorplan {
/** parse the <elevators> tag */
static std::vector<Elevator*> parseFloorElevators(const XMLElem* el) {
std::vector<Elevator*> vec;
static FloorElevators parseFloorElevators(const XMLElem* el) {
FloorElevators vec;
FOREACH_NODE(n, el) {
if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); }
}
@@ -153,8 +153,8 @@ namespace Floorplan {
}
/** parse the <stairs> tag */
static std::vector<Stair*> parseFloorStairs(const XMLElem* el) {
std::vector<Stair*> vec;
static FloorStairs parseFloorStairs(const XMLElem* el) {
FloorStairs vec;
FOREACH_NODE(n, el) {
if (std::string("stair") == n->Name()) { vec.push_back(parseFloorStair(n)); }
}
@@ -207,8 +207,8 @@ namespace Floorplan {
/** parse the <pois> tag */
static std::vector<POI*> parseFloorPOIs(const XMLElem* el) {
std::vector<POI*> vec;
static FloorPOIs parseFloorPOIs(const XMLElem* el) {
FloorPOIs vec;
FOREACH_NODE(n, el) {
if (std::string("poi") == n->Name()) { vec.push_back(parseFloorPOI(n)); }
}
@@ -226,8 +226,8 @@ namespace Floorplan {
/** parse the <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
static FloorGroundTruthPoints parseFloorGroundTruthPoints(const XMLElem* el) {
FloorGroundTruthPoints vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
@@ -242,10 +242,9 @@ namespace Floorplan {
return gtp;
}
/** parse the <accesspoints> tag */
static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) {
std::vector<AccessPoint*> vec;
static FloorAccessPoints parseFloorAccessPoints(const XMLElem* el) {
FloorAccessPoints vec;
FOREACH_NODE(n, el) {
if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); }
}
@@ -317,8 +316,8 @@ namespace Floorplan {
/** parse the <beacons> tag */
static std::vector<Beacon*> parseFloorBeacons(const XMLElem* el) {
std::vector<Beacon*> vec;
static FloorBeacons parseFloorBeacons(const XMLElem* el) {
FloorBeacons vec;
FOREACH_NODE(n, el) {
if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); }
}
@@ -341,9 +340,9 @@ namespace Floorplan {
}
/** parse <fingerprints> <location>s */
static std::vector<FingerprintLocation*> parseFingerprintLocations(const XMLElem* el) {
static FloorFingerprintLocations parseFingerprintLocations(const XMLElem* el) {
assertNode("fingerprints", el);
std::vector<FingerprintLocation*> vec;
FloorFingerprintLocations vec;
FOREACH_NODE(n, el) {
if (std::string("location") == n->Name()) { vec.push_back(parseFingerprintLocation(n)); }
}
@@ -363,8 +362,8 @@ namespace Floorplan {
return fpl;
}
static std::vector<FloorRegion*> parseFloorRegions(const XMLElem* el) {
std::vector<FloorRegion*> vec;
static FloorRegions parseFloorRegions(const XMLElem* el) {
FloorRegions vec;
FOREACH_NODE(n, el) {
if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); }
}
@@ -380,9 +379,9 @@ namespace Floorplan {
}
/** parse the <obstacles> tag */
static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElem* el) {
static FloorObstacles parseFloorObstacles(const XMLElem* el) {
assertNode("obstacles", el);
std::vector<FloorObstacle*> obstacles;
FloorObstacles obstacles;
FOREACH_NODE(n, el) {
// if (std::string("wall") == n->Name()) {obstacles.push_back(parseFloorObstacleWall(n));}
// if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));}

3
grid/factory/v2/Importance.h
View File

@@ -11,7 +11,8 @@
#include "../../../misc/KNNArray.h"
#include "../../../math/MiniMat2.h"
#include "../../../math/Distributions.h"
#include "../../../math/distribution/Normal.h"
#include "../../../math/distribution/Triangle.h"

4
main.cpp
View File

@@ -99,7 +99,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
@@ -109,7 +109,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Matrix4*";
//::testing::GTEST_FLAG(filter) = "*Sphere3*";
//::testing::GTEST_FLAG(filter) = "NavMeshD*";
::testing::GTEST_FLAG(filter) = "Ray.ModelFac*";
//::testing::GTEST_FLAG(filter) = "Timestamp*";
//::testing::GTEST_FLAG(filter) = "*RayTrace3*";

94
math/stats/Histogram.h Normal file
View File

@@ -0,0 +1,94 @@
#ifndef STATS_HISTOGRAM_H
#define STATS_HISTOGRAM_H
#define WITH_DEBUG_PLOT
#ifdef WITH_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#endif
#include <vector>
namespace Stats {
template <typename Scalar> class Histogram {
#ifdef WITH_DEBUG_PLOT
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lines;
#endif
Scalar min;
Scalar max;
int bins;
int cnt = 0;
std::vector<Scalar> counts;
public:
/** ctor */
Histogram(Scalar min, Scalar max, int bins) : min(min), max(max), bins(bins) {
clear();
#ifdef WITH_DEBUG_PLOT
plot.add(&lines);
#endif
}
int count() const {
return cnt;
}
void add(const Scalar x) {
const int idx = binIdx(x);
counts.at(idx) += 1;
++cnt;
if (cnt % 200 == 0) {showPlot();}
}
void clear() {
counts.clear();
counts.resize(bins);
cnt = 0;
}
#ifdef WITH_DEBUG_PLOT
void showPlot() {
lines.clear();
lines.add(K::GnuplotPoint2(-1,0));
for (size_t idx = 0; idx < counts.size(); ++idx) {
const Scalar val = binValue(idx);
const Scalar sum = counts[idx];
const K::GnuplotPoint2 gp2(val, sum);
lines.add(gp2);
}
gp.draw(plot);
gp.flush();
}
#endif
private:
int binIdx(const Scalar val) const {
return (val - min) / (max-min) * bins;
}
Scalar binValue(const int idx) {
return idx * (max-min) + min;
}
};
}
#endif // STATS_HISTOGRAM_H

32
navMesh/NavMeshRandom.h
View File

@@ -6,6 +6,7 @@
#include "../math/DrawList.h"
#include "../geo/Point3.h"
#include "../misc/PerfCheck.h"
#include "../math/stats/Histogram.h"
#include "NavMeshLocation.h"
@@ -20,27 +21,22 @@ namespace NM {
template <typename Tria> class NavMeshRandom {
DrawList<size_t> lst;
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2);
std::vector<const Tria*> triangles;
uint32_t nextSeed() {
static uint32_t seed = 0;
return ++seed;
return seed += 13;
}
public:
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()) {
Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty");
// 1st = almost always the same number?!
gen(); gen();
// construct a DrawList (probability = size[area] of the triangle
// bigger triangles must be choosen more often
for (size_t idx = 0; idx < srcTriangles.size(); ++idx) {
@@ -53,6 +49,9 @@ namespace NM {
/** draw a random point */
NavMeshLocation<Tria> draw() {
// re-use to provide stable random numbers!
static std::mt19937 gen;
PERF_REGION(3, "NavMeshRandom::draw()");
// pick a random triangle to draw from
@@ -63,6 +62,15 @@ namespace NM {
float u = dOnTriangle(gen);
float v = dOnTriangle(gen);
#ifdef WITH_DEBUG_PLOT_2
static Stats::Histogram<float> histU(0, 1, 200);
static Stats::Histogram<float> histV(0, 1, 200);
if (histU.count() > 200) {histU.showPlot(); histU.clear();}
if (histV.count() > 200) {histV.showPlot(); histV.clear();}
histU.add(u);
histV.add(v);
#endif
// if the (u,v) is outside of the triangle, mirror it so its inside the triangle again
if ((u+v) > 1) {
u = 1.0f - u;
@@ -78,15 +86,25 @@ namespace NM {
/** draw a random location within the given radius */
NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) {
// re-use to provide stable random numbers!
static std::mt19937 gen;
std::uniform_real_distribution<float> dDistance(0.001, radius);
while(true) {
const float head = dHeading(gen);
const float dist = dDistance(gen);
const float ox = std::cos(head) * dist;
const float oy = std::sin(head) * dist;
#ifdef WITH_DEBUG_PLOT_2
static Stats::Histogram<float> hist(0, 10, 200);
if (hist.count() > 1000) {hist.showPlot(); hist.clear();}
hist.add(dist);
#endif
// 2D destination (ignore z)
const Point2 dst(center.x + ox, center.y + oy);

1
navMesh/walk/NavMeshWalkParams.h
View File

@@ -20,6 +20,7 @@ namespace NM {
template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
Assert::isTrue(isValid(), "invalid step-sizes given");
Assert::isNotNull(start.tria, "no starting triangle given");
if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
return stepSizeStair_m * steps;

5
tests/geo/TestBVH3.cpp
View File

@@ -10,6 +10,8 @@
#include "../../floorplan/v2/FloorplanReader.h"
#include "../../wifi/estimate/ray3/ModelFactory.h"
using namespace Ray3D;
struct Wrapper {
static std::vector<Point3> getVertices(const BBox3& bbox) {
@@ -146,7 +148,8 @@ TEST(BVH, treeMap) {
ModelFactory fac(map);
fac.setExportCeilings(false);
fac.setFloors({map->floors[3]});
std::vector<Obstacle3D> obs = fac.triangulize();
FloorplanMesh mesh = fac.getMesh();
std::vector<Obstacle3D> obs = mesh.elements;
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;

1
tests/ray/TestDataMap3.cpp
View File

@@ -2,6 +2,7 @@
#include "../Tests.h"
#include "../../wifi/estimate/ray3/DataMap3.h"
using namespace Ray3D;
TEST(DataMap3, test) {

13
tests/ray/TestModelFac.cpp
View File

@@ -4,17 +4,22 @@
#include "../../wifi/estimate/ray3/ModelFactory.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include <fstream>
using namespace Ray3D;
TEST(Ray, ModelFac) {
std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml";
//std::string file = "/apps/paper/diss/data/maps/SHL42_nm.xml";
std::string file = "/mnt/vm/paper/diss/data/maps/SHL42_nm.xml";
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);
ModelFactory fac(map);
fac.triangulize();
//fac.triangulize();
std::ofstream out("/mnt/vm/fhws.obj");
out << fac.toOBJ() << std::endl;
FloorplanMesh mesh = fac.getMesh();
std::ofstream out("/tmp/fhws.ply");
out << mesh.toPLY() << std::endl;
out.close();
}

3
tests/ray/TestRayTrace3.cpp
View File

@@ -4,6 +4,7 @@
#include "../../wifi/estimate/ray3/WifiRayTrace3D.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include <fstream>
using namespace Ray3D;
TEST(RayTrace3, test) {
@@ -19,7 +20,7 @@ TEST(RayTrace3, test) {
ModelFactory fac(map);
std::ofstream outOBJ("/tmp/vm/map.obj");
outOBJ << fac.toOBJ();
outOBJ << fac.getMesh().toOBJ();
outOBJ.close();
const int gs_cm = 50;

55
wifi/estimate/ray3/Cube.h
View File

@@ -1,15 +1,13 @@
#ifndef QUBE_H
#define QUBE_H
#include <vector>
#include "../../../geo/Triangle3.h"
#include "../../../math/Matrix4.h"
#include "Mesh.h"
class Cube {
namespace Ray3D {
private:
std::vector<Triangle3> trias;
class Cube : public Mesh {
public:
@@ -19,39 +17,11 @@ public:
}
/** ctor with position, size and rotation */
Cube(Point3 pos, Point3 size, Point3 rot_deg, const bool topAndBottom = true) {
Cube(const Point3 pos, const Point3 size, const Point3 rot_deg, const bool topAndBottom = true) {
unitCube(topAndBottom);
const Matrix4 mRot = Matrix4::getRotationDeg(rot_deg.x, rot_deg.y, rot_deg.z);
const Matrix4 mSize = Matrix4::getScale(size.x, size.y, size.z);
const Matrix4 mPos = Matrix4::getTranslation(pos.x, pos.y, pos.z);
const Matrix4 mat = mPos * mRot * mSize;
transform(mat);
transform(pos, size, rot_deg);
}
/** get the cube's triangles */
const std::vector<Triangle3> getTriangles() const {
return trias;
}
void transform(const Matrix4& mat) {
for (Triangle3& tria : trias) {
Vector4 v1(tria.p1.x, tria.p1.y, tria.p1.z, 1);
Vector4 v2(tria.p2.x, tria.p2.y, tria.p2.z, 1);
Vector4 v3(tria.p3.x, tria.p3.y, tria.p3.z, 1);
v1 = mat*v1;
v2 = mat*v2;
v3 = mat*v3;
tria.p1 = Point3(v1.x, v1.y, v1.z);
tria.p2 = Point3(v2.x, v2.y, v2.z);
tria.p3 = Point3(v3.x, v3.y, v3.z);
}
}
/** get a transformed version */
Cube transformed(const Matrix4& mat) const {
@@ -60,6 +30,13 @@ public:
return res;
}
/** get a transformed version */
Cube transformed(const Point3 pos, const Point3 size, const Point3 rot_deg) const {
Cube res = *this;
res.transform(pos, size, rot_deg);
return res;
}
private:
/** build unit-cube faces */
@@ -131,11 +108,9 @@ private:
}
void addQuad(Point3 p1, Point3 p2, Point3 p3, Point3 p4) {
trias.push_back( Triangle3(p1,p2,p3) );
trias.push_back( Triangle3(p1,p3,p4) );
}
};
}
#endif // QUBE_H

4
wifi/estimate/ray3/DataMap3.h
View File

@@ -7,6 +7,8 @@
#include <functional>
#include <mutex>
namespace Ray3D {
template <typename T> class DataMap3 {
private:
@@ -232,4 +234,6 @@ public:
};
}
#endif // DATAMAP3_H

156
wifi/estimate/ray3/FloorplanMesh.h Normal file
View File

@@ -0,0 +1,156 @@
#ifndef FLOORPLANMESH_H
#define FLOORPLANMESH_H
#include "Obstacle3.h"
namespace Ray3D {
/**
* meshed version of the floorplan
*/
struct FloorplanMesh {
std::vector<Obstacle3D> elements;
/** DEBUG: convert to .obj file code for exporting */
std::string toOBJ() {
int nVerts = 1;
int nObjs = 0;
std::string res;
// write each obstacle
for (const Obstacle3D& o : elements) {
// write the vertices
for (const Triangle3& t : o.triangles) {
res += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n";
res += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n";
res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
}
// create a new group
res += "g elem_" + std::to_string(++nObjs) + "\n";
// write the group's faces
for (size_t i = 0; i < o.triangles.size(); ++i) {
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
nVerts += 3;
}
}
// done
return res;
}
/** convert to .ply file format */
std::string toPLY() const {
std::stringstream res;
res << "ply\n";
res << "format ascii 1.0\n";
int faces = 0;
int vertices = 0;
for (const Obstacle3D& obs : elements) {
vertices += obs.triangles.size() * 3;
faces += obs.triangles.size();
}
// material
std::vector<Material> mats = {
Material(0,128,0,255), // ground outdoor
Material(64,64,64,255), // ground outdoor
Material(255,96,96,255), // stair
Material(128,128,128,255), // concrete
Material(64,128,255,64), // glass
Material(200,200,200,255), // default
};
res << "element material " << mats.size() << "\n";
res << "property uchar red\n";
res << "property uchar green\n";
res << "property uchar blue\n";
res << "property uchar alpha\n";
res << "element vertex " << vertices << "\n";
res << "property float x\n";
res << "property float y\n";
res << "property float z\n";
res << "property float nx\n";
res << "property float ny\n";
res << "property float nz\n";
res << "property int material_index\n";
res << "property uchar red\n";
res << "property uchar green\n";
res << "property uchar blue\n";
res << "property uchar alpha\n";
res << "element face " << faces << "\n";
res << "property list uchar int vertex_indices\n";
res << "end_header\n";
for (const Material& mat : mats) {
res << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << "\n";
}
for (const Obstacle3D& obs : elements) {
const int matIdx = getMaterial(obs);
const Material& mat = mats[matIdx];
for (const Triangle3& tria : obs.triangles) {
const Point3 n = cross(tria.p2-tria.p1, tria.p3-tria.p1).normalized();
res << tria.p1.x << " " << tria.p1.y << " " << tria.p1.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << "\n";
res << tria.p2.x << " " << tria.p2.y << " " << tria.p2.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
res << tria.p3.x << " " << tria.p3.y << " " << tria.p3.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
}
}
int vidx = 0;
for (const Obstacle3D& obs : elements) {
for (const Triangle3& tria : obs.triangles) {
(void) tria;
res << "3 " << vidx++ << " " << vidx++ << " " << vidx++ << "\n";
}
}
// done
return res.str();
}
struct Material {
int r, g, b, a;
Material(int r, int g, int b, int a) : r(r), g(g), b(b), a(a) {;}
};
int getMaterial(const Obstacle3D& o) const {
if (o.type == Obstacle3D::Type::GROUND_OUTDOOR) {return 0;}
if (o.type == Obstacle3D::Type::GROUND_INDOOR) {return 1;}
if (o.type == Obstacle3D::Type::STAIR) {return 2;}
if (o.mat == Floorplan::Material::CONCRETE) {return 3;}
if (o.mat == Floorplan::Material::GLASS) {return 4;}
return 5;
}
// Color getColor(const Obstacle3D& o) const {
// if (o.type == Obstacle3D::Type::GROUND_OUTDOOR) {return Color(0,128,0,255);}
// if (o.type == Obstacle3D::Type::GROUND_INDOOR) {return Color(64,64,64,255);}
// if (o.mat == Floorplan::Material::CONCRETE) {return Color(128,128,128,255);}
// if (o.mat == Floorplan::Material::GLASS) {return Color(128,128,255,64);}
// return Color(200,200,200,255);
// }
};
}
#endif // FLOORPLANMESH_H

4
wifi/estimate/ray3/MaterialOptions.h
View File

@@ -5,6 +5,8 @@
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../Assertions.h"
namespace Ray3D {
/** raytracing attributes for one material */
struct MaterialAttributes {
@@ -61,4 +63,6 @@ private:
};
}
#endif // MATERIALOPTIONS_H

60
wifi/estimate/ray3/Mesh.h Normal file
View File

@@ -0,0 +1,60 @@
#ifndef RAY3D_MESH_H
#define RAY3D_MESH_H
#include <vector>
#include "../../../geo/Triangle3.h"
#include "../../../math/Matrix4.h"
namespace Ray3D {
class Mesh {
protected:
std::vector<Triangle3> trias;
public:
/** get the mesh's triangles */
const std::vector<Triangle3>& getTriangles() const {
return trias;
}
void transform(const Point3 pos, Point3 size, Point3 rot_deg) {
const Matrix4 mRot = Matrix4::getRotationDeg(rot_deg.x, rot_deg.y, rot_deg.z);
const Matrix4 mSize = Matrix4::getScale(size.x, size.y, size.z);
const Matrix4 mPos = Matrix4::getTranslation(pos.x, pos.y, pos.z);
const Matrix4 mat = mPos * mRot * mSize;
transform(mat);
}
void transform(const Matrix4& mat) {
for (Triangle3& tria : trias) {
Vector4 v1(tria.p1.x, tria.p1.y, tria.p1.z, 1);
Vector4 v2(tria.p2.x, tria.p2.y, tria.p2.z, 1);
Vector4 v3(tria.p3.x, tria.p3.y, tria.p3.z, 1);
v1 = mat*v1;
v2 = mat*v2;
v3 = mat*v3;
tria.p1 = Point3(v1.x, v1.y, v1.z);
tria.p2 = Point3(v2.x, v2.y, v2.z);
tria.p3 = Point3(v3.x, v3.y, v3.z);
}
}
void addQuad(Point3 p1, Point3 p2, Point3 p3, Point3 p4) {
trias.push_back( Triangle3(p1,p2,p3) );
trias.push_back( Triangle3(p1,p3,p4) );
}
};
}
#endif // RAY3D_MESH_H

411
wifi/estimate/ray3/ModelFactory.h
View File

@@ -6,6 +6,10 @@
#include "ModelFactoryHelper.h"
#include "Obstacle3.h"
#include "Cube.h"
#include "Tube.h"
#include "FloorplanMesh.h"
namespace Ray3D {
/**
* convert an indoor map into a 3D model based on triangles
@@ -16,9 +20,14 @@ private:
bool exportCeilings = true;
bool exportObstacles = true;
bool exportStairs = true;
bool exportHandrails = true;
bool exportDoors = true;
bool doorsOpen = true;
bool exportWallTops = false;
std::vector<Floorplan::Floor*> exportFloors;
/** the to-be-exported map */
const Floorplan::IndoorMap* map;
public:
@@ -29,6 +38,7 @@ public:
}
/** whether or not to export ceilings */
void setExportCeilings(bool exp) {
this->exportCeilings = exp;
}
@@ -38,6 +48,18 @@ public:
this->exportFloors = floors;
}
/** convert floorplan to mesh */
FloorplanMesh getMesh() {
FloorplanMesh mesh;
mesh.elements = triangulize();
return mesh;
}
private:
/** get all triangles grouped by obstacle */
std::vector<Obstacle3D> triangulize() {
@@ -49,19 +71,29 @@ public:
// process each floor
for (const Floorplan::Floor* f : floors) {
if (!f->enabled) {continue;}
// triangulize the floor itself (floor/ceiling)
if (exportCeilings) {res.push_back(getTriangles(f));}
// process each obstacle within the floor
for (const Floorplan::FloorObstacle* fo : f->obstacles) {
// handle line obstacles
const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(fo);
if (fol) {
if (fol->type == Floorplan::ObstacleType::HANDRAIL) {continue;}
if (exportObstacles) {res.push_back(getTriangles(f, fol));}
if (exportCeilings) {
std::vector<Obstacle3D> tmp = getFloor(f);
res.insert(res.end(), tmp.begin(), tmp.end());
}
// process each obstacle within the floor
if (f->obstacles.enabled) {
for (const Floorplan::FloorObstacle* fo : f->obstacles) {
std::vector<Obstacle3D> tmp = getObstacle(f, fo);
res.insert(res.end(), tmp.begin(), tmp.end());
}
}
// stairs
if (f->stairs.enabled) {
for (const Floorplan::Stair* stair : f->stairs) {
if (exportStairs) {res.push_back(getStairs(f, stair));}
}
}
// TODO: remove
@@ -74,66 +106,57 @@ public:
}
/** DEBUG: convert to .obj file code for exporting */
std::string toOBJ() {
const std::vector<Obstacle3D> obs = triangulize();
int nVerts = 1;
int nObjs = 0;
std::string res;
// write each obstacle
for (const Obstacle3D& o : obs) {
// write the vertices
for (const Triangle3& t : o.triangles) {
res += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n";
res += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n";
res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
}
// create a new group
res += "g elem_" + std::to_string(++nObjs) + "\n";
// write the group's faces
for (size_t i = 0; i < o.triangles.size(); ++i) {
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
nVerts += 3;
}
}
// done
return res;
}
private:
/** convert a floor (floor/ceiling) into triangles */
Obstacle3D getTriangles(const Floorplan::Floor* f) {
std::vector<Obstacle3D> getFloor(const Floorplan::Floor* f) {
std::vector<Obstacle3D> res;
if (!f->enabled) {return res;}
if (!f->outline.enabled) {return res;}
// floor uses an outline based on "add" and "remove" areas
// we need to create the apropriate triangles to model the polygon
// including all holes (remove-areas)
// TODO: variable type?
Obstacle3D res(Floorplan::Material::CONCRETE);
Polygon poly;
// append all "add" and "remove" areas
// process all "add" regions by type
// [this allows for overlaps of the same type]
std::unordered_map<std::string, Polygon> types;
for (Floorplan::FloorOutlinePolygon* fop : f->outline) {
switch (fop->method) {
case Floorplan::OutlineMethod::ADD: poly.add(fop->poly); break;
case Floorplan::OutlineMethod::REMOVE: poly.remove(fop->poly); break;
default: throw 1;
if (fop->method == Floorplan::OutlineMethod::ADD) {
if (fop->outdoor) {
types["outdoor"].add(fop->poly);
} else {
types["indoor"].add(fop->poly);
}
}
}
// remove the "remove" regions from EVERY "add" region added within the previous step
for (Floorplan::FloorOutlinePolygon* fop : f->outline) {
if (fop->method == Floorplan::OutlineMethod::REMOVE) {
for (auto& it : types) {
it.second.remove(fop->poly);
}
}
// allow for overlapping outdoor/indoor regions -> outdoor wins [remove outdoor part from indoor parts]
if (fop->outdoor) {
types["indoor"].remove(fop->poly);
}
}
// create an obstacle for each type (indoor, outdoor)
for (auto& it : types) {
// TODO: variable type?
Obstacle3D::Type type = (it.first == "indoor") ? (Obstacle3D::Type::GROUND_INDOOR) : (Obstacle3D::Type::GROUND_OUTDOOR);
Obstacle3D obs(type, Floorplan::Material::CONCRETE);
// convert them into polygons
std::vector<std::vector<Point3>> polys = poly.get(f->getStartingZ());
std::vector<std::vector<Point3>> polys = it.second.get(f->getStartingZ());
// convert polygons (GL_TRIANGLE_STRIP) to triangles
for (const std::vector<Point3>& pts : polys) {
@@ -149,50 +172,96 @@ private:
if (tria2.getNormal().z < 0) {tria2 = tria2 - Point3(0,0,0.02);}
// add both
res.triangles.push_back(tria1);
res.triangles.push_back(tria2);
obs.triangles.push_back(tria1);
obs.triangles.push_back(tria2);
}
}
res.push_back(obs);
}
return res;
}
/**
* @brief build the given obstacle
* @param f the floor
* @param fo the obstacle
* @param aboveDoor whether to place this obstacle ABOVE the given door (overwrite)
* @return
*/
std::vector<Obstacle3D> getObstacle(const Floorplan::Floor* f, const Floorplan::FloorObstacle* fo, const Floorplan::FloorObstacleDoor* aboveDoor = nullptr) const {
std::vector<Obstacle3D> res;
// handle line obstacles
const Floorplan::FloorObstacleLine* fol = dynamic_cast<const Floorplan::FloorObstacleLine*>(fo);
if (fol) {
if (exportObstacles) {
if (fol->type != Floorplan::ObstacleType::HANDRAIL || exportHandrails) {
res.push_back(getObstacleLine(f, fol, aboveDoor));
}
}
}
const Floorplan::FloorObstacleDoor* door = dynamic_cast<const Floorplan::FloorObstacleDoor*>(fo);
if (door) {
if (exportObstacles) {
if (exportDoors) {
res.push_back(getDoor(f, door));
}
//std::vector<Obstacle3D> tmp = getDoorAbove(f, door);
//res.insert(res.end(), tmp.begin(), tmp.end());
res.push_back(getDoorAbove(f, door));
}
}
return res;
}
/** convert a line obstacle to 3D triangles */
Obstacle3D getTriangles(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) {
Obstacle3D getObstacleLine(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor = nullptr) const {
/*
Obstacle3D res(fol->material);
switch (fol->type) {
case Floorplan::ObstacleType::HANDRAIL:
return getHandrail(f, fol);
case Floorplan::ObstacleType::WINDOW:
return getWindow(f, fol, aboveDoor);
case Floorplan::ObstacleType::WALL:
return getWall(f, fol, aboveDoor);
default:
throw Exception("invalid obstacle type");
}
Point3 p1(fol->from.x, fol->from.y, f->getStartingZ());
Point3 p2(fol->to.x, fol->to.y, f->getStartingZ());
Point3 p3(fol->to.x, fol->to.y, f->getEndingZ());
Point3 p4(fol->from.x, fol->from.y, f->getEndingZ());
}
Triangle3 t1(p1,p2,p3);
Triangle3 t2(p1,p3,p4);
Obstacle3D getWindow(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
return getWall(f, fol, aboveDoor);
}
res.triangles.push_back(t1);
res.triangles.push_back(t2);
*/
Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
const float thickness_m = fol->thickness_m;
const Point2 from = fol->from;
const Point2 to = fol->to;
const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from);
const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to);
const Point2 cen2 = (from+to)/2;
const float rad = std::atan2(to.y - from.y, to.x - from.x);
const float deg = rad * 180 / M_PI;
// cube's destination center
const Point3 pos(cen2.x, cen2.y, f->atHeight + f->height/2);
const float cenZ = (!aboveDoor) ? (f->atHeight + f->height/2) : (f->getEndingZ() - (f->height - aboveDoor->height) / 2);
const float height = (!aboveDoor) ? (f->height) : (f->height - aboveDoor->height);
const Point3 pos(cen2.x, cen2.y, cenZ);
// div by 2.01 to prevent overlapps and z-fi
const float sx = from.getDistance(to) / 2.01f;
const float sy = thickness_m / 2.01f;
const float sz = f->height / 2.01f; // prevent overlaps
// div by 2.01 to prevent overlapps and z-fighting
const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = height / 2.01f; // prevent overlaps
const Point3 size(sx, sy, sz);
const Point3 rot(0,0,deg);
@@ -200,14 +269,200 @@ private:
Cube cube(pos, size, rot);
// done
Obstacle3D res(fol->material);
Obstacle3D res(getType(fol), fol->material);
res.triangles = cube.getTriangles();
return res;
}
Obstacle3D getDoor(const Floorplan::Floor* f, const Floorplan::FloorObstacleDoor* door) const {
const float thickness_m = 0.10; // TODO??
const Point2 from = door->from;
const Point2 to = door->to;
const float rad = std::atan2(to.y - from.y, to.x - from.x);
float deg = rad * 180 / M_PI;
// div by 2.01 to prevent overlapps and z-fighting
const Point3 rot(0,0,deg);
Point3 pos;
Matrix4 mat = Matrix4::identity();
Obstacle3D res(Obstacle3D::Type::DOOR, door->material);
// normal door? (non-spinner)
if (Floorplan::DoorType::SWING == door->type) {
if (doorsOpen) {deg += (door->swap) ? (-90) : (+90);}
mat = Matrix4::getTranslation(1,0,0); // cube's edge located at 0,0,0
pos = Point3(from.x, from.y, f->atHeight + door->height/2);
const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = door->height / 2.01f; // prevent overlaps
const Point3 size(sx, sy, sz);
Cube cube;
cube.transform(mat);
cube.transform(pos, size, rot);
res.triangles = cube.getTriangles();
} else if (Floorplan::DoorType::REVOLVING == door->type) {
const Point2 cen2 = (from+to)/2;
const Point3 pos(cen2.x, cen2.y, f->atHeight + door->height/2);
// outer and inner radius
const float rOuter = from.getDistance(to) / 2;
const float rInner = rOuter - 0.1;
const float sz = door->height / 2.01f; // prevent overlaps
const Point3 size(1, 1, sz);
// around the doors
Tube tube;
tube.addSegment(0+40-90, 180-40-90, rInner, rOuter, 1, true, true);
tube.addSegment(180+40-90, 360-40-90, rInner, rOuter, 1, true, true);
tube.transform(pos, Point3(1,1,1), rot);
res.triangles = tube.getTriangles();
// the doors
const int numDoors = 3;
Cube cube;
cube.transform(Matrix4::getTranslation(1,0,0));
for (int i = 0; i < numDoors; ++i) {
const int deg = 45 + (360*i / numDoors);
Cube c1 = cube
.transformed(Matrix4::getScale(rInner/2-0.05, thickness_m/2, sz))
.transformed(Matrix4::getTranslation(0.04, 0, 0))
.transformed(Matrix4::getRotationDeg(0,0,deg))
.transformed(Matrix4::getTranslation(pos.x, pos.y, pos.z));
//pos, Point3(rInner/2-0.05, thickness_m/2, sz), Point3(0,0,deg));
std::vector<Triangle3> t1 = c1.getTriangles();
res.triangles.insert(res.triangles.end(), t1.begin(), t1.end());
}
} else {
throw "unsupported door type";
}
return res;
}
/** get the missing part/gap, above the given door */
Obstacle3D getDoorAbove(const Floorplan::Floor* f, const Floorplan::FloorObstacleDoor* door) const {
// find the element above the door (= a connected element)
auto comp = [door] (const Floorplan::FloorObstacle* obs) {
if (obs == door) {return false;}
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
if (!line) {return false;}
return (line->from == door->from || line->to == door->from || line->from == door->to || line->to == door->to);
};
auto it = std::find_if(f->obstacles.begin(), f->obstacles.end(), comp);
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*> (*it);
if (!line) {
throw Exception("did not find a matching element to place above the door");
}
// get the obstacle to place above the door
return getObstacleLine(f, line, door);
}
Obstacle3D getHandrail(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) const {
// target
Obstacle3D res(getType(fol), fol->material);
if (!exportHandrails) {return res;}
const float thickness_m = 0.05;
const Point2 from = fol->from;
const Point2 to = fol->to;
const Point2 cen2 = (from+to)/2;
// edges
const float z1 = f->atHeight;
const float z2 = f->atHeight + 1.0;
Point3 p1 = Point3(from.x, from.y, z1);
Point3 p2 = Point3(to.x, to.y, z1);
Point3 p3 = Point3(from.x, from.y, z2);
Point3 p4 = Point3(to.x, to.y, z2);
const float rad = std::atan2(to.y - from.y, to.x - from.x);
const float deg = rad * 180 / M_PI;
// cube's destination center
const Point3 pUp(cen2.x, cen2.y, z2);
const float sx = from.getDistance(to) / 2;
const float sy = thickness_m / 2;
const float sz = thickness_m / 2;
const Point3 size(sx, sy, sz);
const Point3 rot(0,0,deg);
// upper bar
const Cube cubeUpper(pUp, size, rot);
const std::vector<Triangle3> tmp = cubeUpper.getTriangles();
res.triangles.insert(res.triangles.end(), tmp.begin(), tmp.end());
const Point3 d1 = p2-p1;
const Point3 d2 = p4-p3;
const int numBars = d2.length() / 0.75f;
for (int i = 1; i < numBars; ++i) {
const Point3 s = p1 + d1 * i / numBars;
const Point3 e = p3 + d2 * i / numBars;
const Point3 c = (s+e)/2;
const Point3 size(thickness_m/2, thickness_m/2, s.getDistance(e)/2 - thickness_m);
const Cube cube(c, size, rot);
const std::vector<Triangle3> tmp = cube.getTriangles();
res.triangles.insert(res.triangles.end(), tmp.begin(), tmp.end());
}
// done
return res;
}
/** convert a line obstacle to 3D triangles */
Obstacle3D getStairs(const Floorplan::Floor* f, const Floorplan::Stair* s) {
Obstacle3D res(Obstacle3D::Type::STAIR, Floorplan::Material::CONCRETE);
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), f);
for (const Floorplan::Quad3& quad : quads) {
const Triangle3 t1(quad.p1, quad.p2, quad.p3);
const Triangle3 t2(quad.p3, quad.p4, quad.p1);
res.triangles.push_back(t1);
res.triangles.push_back(t2);
}
return res;
}
static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) {
switch (l->type) {
case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL;
case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW;
case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL;
default: return Obstacle3D::Type::UNKNOWN;
}
}
};
}
#endif // MODELFACTORY_H

4
wifi/estimate/ray3/ModelFactoryHelper.h
View File

@@ -4,6 +4,8 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include "../../../lib/gpc/gpc.h"
namespace Ray3D {
class Polygon {
struct GPCPolygon : gpc_polygon {
@@ -102,4 +104,6 @@ private:
};
}
#endif // MODELFACTORYHELPER_H

24
wifi/estimate/ray3/Obstacle3.h
View File

@@ -7,18 +7,38 @@
#include "../../../floorplan/v2/Floorplan.h"
namespace Ray3D {
/**
* 3D obstacle
* based on multiple triangles
* has a material and a type
*/
struct Obstacle3D {
enum class Type {
UNKNOWN,
GROUND_INDOOR,
GROUND_OUTDOOR,
STAIR,
HANDRAIL,
DOOR,
WALL,
WINDOW,
};
Type type;
Floorplan::Material mat;
std::vector<Triangle3> triangles;
/** empty ctor */
Obstacle3D() : mat() {;}
Obstacle3D() : type(Type::UNKNOWN), mat() {;}
/** ctor */
Obstacle3D(Floorplan::Material mat) : mat(mat) {;}
Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
};
}
#endif // OBSTACLE3_H

122
wifi/estimate/ray3/Tube.h Normal file
View File

@@ -0,0 +1,122 @@
#ifndef TUBE_H
#define TUBE_H
#include "../../../math/Matrix4.h"
#include "Mesh.h"
namespace Ray3D {
/** walled cylinder */
class Tube : public Mesh {
public:
/** ctor */
Tube() {
;
}
/** get a transformed version */
Tube transformed(const Matrix4& mat) const {
Tube res = *this;
res.transform(mat);
return res;
}
/** build */
void addSegment(const float from_deg, const float to_deg, const float rInner, const float rOuter, const float h, bool closeSides, bool topAndBottom) {
const int tiles = 32;
const float deg_per_tile = 360.0f / tiles;
const float rad_per_tile = deg_per_tile / 180.0f * M_PI;
const int startTile = std::round(from_deg / deg_per_tile);
const int endTile = std::round(to_deg / deg_per_tile);
for (int i = startTile; i < endTile; ++i) {
const float startRad = (i+0) * rad_per_tile;
const float endRad = (i+1) * rad_per_tile;
const float xo0 = std::cos(startRad) * rOuter;
const float yo0 = std::sin(startRad) * rOuter;
const float xo1 = std::cos(endRad) * rOuter;
const float yo1 = std::sin(endRad) * rOuter;
const float xi0 = std::cos(startRad) * rInner;
const float yi0 = std::sin(startRad) * rInner;
const float xi1 = std::cos(endRad) * rInner;
const float yi1 = std::sin(endRad) * rInner;
if (closeSides) {
// close start of segment
if (i == startTile) {
addQuad(
Point3(xi0, yi0, -h),
Point3(xo0, yo0, -h),
Point3(xo0, yo0, +h),
Point3(xi0, yi0, +h)
);
}
// close end of segment
if (i == endTile-1) {
addQuad(
Point3(xi1, yi1, +h),
Point3(xo1, yo1, +h),
Point3(xo1, yo1, -h),
Point3(xi1, yi1, -h)
);
}
}
// outer
addQuad(
Point3(xo0, yo0, -h),
Point3(xo1, yo1, -h),
Point3(xo1, yo1, +h),
Point3(xo0, yo0, +h)
);
// innser
addQuad(
Point3(xi0, yi0, +h),
Point3(xi1, yi1, +h),
Point3(xi1, yi1, -h),
Point3(xi0, yi0, -h)
);
if (topAndBottom) {
// top
addQuad(
Point3(xi0, yi0, h),
Point3(xo0, yo0, h),
Point3(xo1, yo1, h),
Point3(xi1, yi1, h)
);
// bottom
addQuad(
Point3(xi1, yi1, -h),
Point3(xo1, yo1, -h),
Point3(xo0, yo0, -h),
Point3(xi0, yi0, -h)
);
}
}
}
};
}
#endif // TUBE_H

9
wifi/estimate/ray3/WifiRayTrace3D.h
View File

@@ -34,6 +34,8 @@
// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
namespace Ray3D {
struct Intersection {
Point3 pos;
const Obstacle3D* obs;
@@ -198,7 +200,7 @@ public:
dm.resize(bbox, gs);
ModelFactory fac(map);
std::vector<Obstacle3D> obstacles = fac.triangulize();
std::vector<Obstacle3D> obstacles = fac.getMesh().elements;
// build bounding volumes
for (Obstacle3D& obs : obstacles) {
@@ -503,9 +505,8 @@ private:
}
*/
};
}
#endif // WIFIRAYTRACE3D_H