168 lines
6.3 KiB
C++
168 lines
6.3 KiB
C++
#ifndef HELPER_H
|
|
#define HELPER_H
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
#include <Indoor/grid/factory/GridFactory.h>
|
|
#include <Indoor/grid/factory/GridImportance.h>
|
|
|
|
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
|
|
|
#include "Settings.h"
|
|
#include "MyGridNode.h"
|
|
#include "OldGroundTruth.h"
|
|
|
|
class Helper {
|
|
|
|
private:
|
|
|
|
|
|
|
|
public:
|
|
|
|
/** convert height (in cm) to floor-numbers */
|
|
static int getFloorNr(float z_cm) {
|
|
// if (z_cm < 360) {return 0;}
|
|
// if (z_cm < 360+340) {return 1;}
|
|
// if (z_cm < 360+340+340) {return 2;}
|
|
// return 3;
|
|
if (z_cm < 380) {return 0;}
|
|
if (z_cm < 380+340) {return 1;}
|
|
if (z_cm < 380+340+340) {return 2;}
|
|
return 3;
|
|
}
|
|
|
|
/** convert height (in cm) to floor-numbers */
|
|
static int getFloorNrFloat(const float z_cm) {
|
|
return z_cm / 350.0f;
|
|
}
|
|
|
|
static int getHeight(const int floorNr) {
|
|
switch(floorNr) {
|
|
case 0: return 0;
|
|
case 1: return 380;
|
|
case 2: return 380+340;
|
|
case 3: return 380+340+340;
|
|
default: throw "error";
|
|
}
|
|
}
|
|
|
|
/** align the given value onto the grid */
|
|
static int align(const int val) {
|
|
return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm;
|
|
}
|
|
|
|
/** all floors within the building */
|
|
struct FHWSFloors {
|
|
|
|
Floor f0, f1, f2, f3;
|
|
|
|
Stairs s01, s12, s23;
|
|
|
|
const LengthF h0 = LengthF::cm(align(getHeight(0)));
|
|
const LengthF h1 = LengthF::cm(align(getHeight(1)));
|
|
const LengthF h2 = LengthF::cm(align(getHeight(2)));
|
|
const LengthF h3 = LengthF::cm(align(getHeight(3)));
|
|
|
|
// all ground-truth points
|
|
std::unordered_map<int, Point3> gtwp;
|
|
|
|
FHWSFloors() {;}
|
|
|
|
};
|
|
|
|
/** load the entire floorplan */
|
|
static FHWSFloors getFloors() {
|
|
|
|
FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
|
|
FHWSFloors f;
|
|
|
|
f.f0 = fpFac.getFloor("floor_0");
|
|
f.f1 = fpFac.getFloor("floor_1");
|
|
f.f2 = fpFac.getFloor("floor_2");
|
|
f.f3 = fpFac.getFloor("floor_3");
|
|
|
|
f.s01 = fpFac.getStairs("staircase_0_1");
|
|
f.s12 = fpFac.getStairs("staircase_1_2");
|
|
f.s23 = fpFac.getStairs("staircase_2_3");
|
|
|
|
OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222);
|
|
OldGroundTruth gtwp05(MiscSettings::floorplan, "ground_truth_0_5", 2.822222);
|
|
OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222);
|
|
OldGroundTruth gtwp15(MiscSettings::floorplan, "ground_truth_1_5", 2.822222);
|
|
OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222);
|
|
OldGroundTruth gtwp25(MiscSettings::floorplan, "ground_truth_2_5", 2.822222);
|
|
OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222);
|
|
|
|
for (auto it : gtwp0.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
|
|
for (auto it : gtwp1.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
|
|
for (auto it : gtwp2.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
|
|
for (auto it : gtwp3.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}
|
|
|
|
for (auto it : gtwp05.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(0)+getHeight(1))/2);}
|
|
for (auto it : gtwp15.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(1)+getHeight(2))/2);}
|
|
for (auto it : gtwp25.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(2)+getHeight(3))/2);}
|
|
|
|
return f;
|
|
|
|
}
|
|
|
|
template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {
|
|
|
|
GridFactory<MyGridNode> gridFac(grid);
|
|
|
|
gridFac.addFloor(floors.f0, floors.h0.cm());
|
|
gridFac.addFloor(floors.f1, floors.h1.cm());
|
|
gridFac.addFloor(floors.f2, floors.h2.cm());
|
|
gridFac.addFloor(floors.f3, floors.h3.cm());
|
|
|
|
gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
|
|
gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
|
|
gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
|
|
|
|
PlatformStair psUpperLeft;
|
|
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
|
|
psUpperLeft.s1 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
|
|
psUpperLeft.s2 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
|
|
gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
|
|
gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
|
|
gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
|
|
// vis.gp << "set xrange [1100:1800]\n";
|
|
// vis.gp << "set yrange [4500:5200]\n";
|
|
|
|
PlatformStair psUpperRight;
|
|
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
|
|
psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
|
|
psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
|
|
gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
|
|
gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
|
|
gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
|
|
// vis.gp << "set xrange [6100:6900]\n";
|
|
// vis.gp << "set yrange [4500:5200]\n";
|
|
|
|
PlatformStair psLowerLeft;
|
|
psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
|
|
psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
|
|
psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
|
|
gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
|
|
gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
|
|
gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
|
|
// vis.gp << "set xrange [1300:2100]\n";
|
|
// vis.gp << "set yrange [400:1400]\n";
|
|
|
|
// remove all isolated nodes not attached to 300,300,floor0
|
|
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );
|
|
|
|
// stamp importance information onto the grid-nodes
|
|
GridImportance gridImp;
|
|
gridImp.addImportance(grid, floors.h0.cm());
|
|
gridImp.addImportance(grid, floors.h1.cm());
|
|
gridImp.addImportance(grid, floors.h2.cm());
|
|
gridImp.addImportance(grid, floors.h3.cm());
|
|
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // HELPER_H
|