From 89a1542dae312ed1edf0d706bd6fb082f5c10496 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 7 Aug 2019 17:59:38 +0200 Subject: [PATCH] init push --- CMakeLists.txt | 108 +++++++ Plotty.h | 687 ++++++++++++++++++++++++++++++++++++++++++ Settings.h | 191 ++++++++++++ main.cpp | 430 ++++++++++++++++++++++++++ main.h | 6 + navMesh/filter.h | 318 +++++++++++++++++++ navMesh/mesh.h | 31 ++ navMesh/meshPlotter.h | 242 +++++++++++++++ 8 files changed, 2013 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 Plotty.h create mode 100644 Settings.h create mode 100644 main.cpp create mode 100644 main.h create mode 100644 navMesh/filter.h create mode 100644 navMesh/mesh.h create mode 100644 navMesh/meshPlotter.h diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bd8a64d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,108 @@ +# Usage: +# Create build folder, like RC-build next to RobotControl and WifiScan folder +# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' +# make + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) + +# select build type +SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) + +PROJECT(IMMPF_Localization) + +IF(NOT CMAKE_BUILD_TYPE) +MESSAGE(STATUS "No build type selected. Default to Debug") +SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../ +) + + +FILE(GLOB HEADERS + ./notes.txt + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h +) + + +FILE(GLOB SOURCES + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ../Indoor/lib/tinyxml/tinyxml2.cpp + ../Indoor/lib/Recast/*.cpp +) + + +if ("${CMAKE_GENERATOR}" MATCHES "Visual Studio*") + add_definitions( + -D_USE_MATH_DEFINES + -DUNICODE + -D_UNICODE + -DNOGDI + ) + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++17") +else() + # system specific compiler flags + ADD_DEFINITIONS( + + #-std=gnu++14 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + -g3 + #-O2 + -march=native + + ) +endif() + +add_definitions( +# -DWITH_TESTS +-DWITH_ASSERTIONS +# -DWITH_DEBUG_LOG +# -DWITH_DEBUG_PLOT +# -D_GLIBCXX_DEBUG +) + +# allow OMP +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} + stdc++fs +# gtest +# pthread +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/Plotty.h b/Plotty.h new file mode 100644 index 0000000..8b12ec6 --- /dev/null +++ b/Plotty.h @@ -0,0 +1,687 @@ +#ifndef PLOTTY_H +#define PLOTTY_H + +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +struct Color { + + uint8_t r; + uint8_t g; + uint8_t b; + + Color() : r(0), g(0), b(0) { + ; + } + + static Color fromRGB(const uint8_t r, const uint8_t g, const uint8_t b) { + Color c; c.setRGB(r,g,b); + return c; + } + + static Color fromHSV(const uint8_t h, const uint8_t s, const uint8_t v) { + Color c; c.setHSV(h,s,v); + return c; + } + + void setRGB(const uint8_t r, const uint8_t g, const uint8_t b) { + this->r = r; + this->g = g; + this->b = b; + } + + void setHSV(const uint8_t h, const uint8_t s, const uint8_t v) { + + uint8_t region, remainder, p, q, t; + + region = h / 43; + remainder = (h - (region * 43)) * 6; + + p = (v * (255 - s)) >> 8; + q = (v * (255 - ((s * remainder) >> 8))) >> 8; + t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8; + + switch (region) { + case 0: + r = v; g = t; b = p; + break; + case 1: + r = q; g = v; b = p; + break; + case 2: + r = p; g = v; b = t; + break; + case 3: + r = p; g = q; b = v; + break; + case 4: + r = t; g = p; b = v; + break; + default: + r = v; g = p; b = q; + break; + } + + } + + std::string toHEX() const { + char buf[8]; + sprintf(buf, "#%02x%02x%02x", r, g, b); + std::string color(buf); + return color; + } + +}; + + +class Plotty { + +public: + + const Floorplan::IndoorMap* map; + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementPoints points; + K::GnuplotSplotElementColorPoints cpoints; + + K::GnuplotSplotElementLines pathReal; + K::GnuplotSplotElementLines pathEst; + K::GnuplotSplotElementLines shortestPath; + + K::GnuplotSplotElementColorPoints particles; + + K::GnuplotSplotElementLines mapOutlineGlass; + K::GnuplotSplotElementLines mapOutlineDrywall; + K::GnuplotSplotElementLines mapOutlineConcrete; + K::GnuplotSplotElementLines mapBBoxes; + + K::GnuplotSplotElementEmpty emptyElem; + + + K::GnuplotSplotElementPM3D pm3doutline; + + std::string codeFile; + + struct Settings { + std::vector floors = {}; + bool stairs = true; + bool obstacles = true; + bool outline = true; + bool outlineColorCustom = false; + bool skipI1 = false; + K::GnuplotColor outlineColor = K::GnuplotColor::fromRGB(128,128,128); + float minZ = -9999; + float maxZ = +9999; + } settings; + +public: + + Plotty(const Floorplan::IndoorMap* map) : map(map) { + + //gp << "set view equal xy\n"; + + //gp << "set palette model RGB\n"; + //gp << "r(x) = (x < 0) ? 0 : (x/2)\n"; + //gp << "g(x) = 0\n"; + //gp << "b(x) = (x > 0) ? 0 : (-x/2)\n"; + //gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; + gp << "set ticslevel 0\n"; + + + // how to draw the floorplan + mapOutlineConcrete.getStroke().getColor().setHexStr("#888888"); mapOutlineConcrete.getStroke().setWidth(2); + mapOutlineDrywall.getStroke().getColor().setHexStr("#888888"); + mapOutlineGlass.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().setType(K::GnuplotDashtype::DASHED); + mapBBoxes.getStroke().setWidth(2); + + splot.add(&emptyElem); + + splot.add(&mapOutlineConcrete); + splot.add(&mapOutlineDrywall); + splot.add(&mapOutlineGlass); + splot.add(&mapBBoxes); + + splot.add(&particles); particles.setPointSize(0.20f); //particles.setColorHex("#777777"); + + splot.add(&pathReal); pathReal.getStroke().setWidth(2); pathReal.getStroke().getColor().setHexStr("#000000"); + splot.add(&pathEst); pathEst.getStroke().setWidth(2); pathEst.getStroke().getColor().setHexStr("#0000ff"); + splot.add(&shortestPath); shortestPath.getStroke().setWidth(3); shortestPath.getStroke().getColor().setHexStr("#ff00ff"); + + splot.add(&pm3doutline); + + splot.add(&points); + points.setPointType(7); + points.setPointSize(0.5); + + splot.add(&cpoints); + cpoints.setPointSize(2); + cpoints.setPointType(7); + + splot.getView().setEnabled(false); + + } + + void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { + for (BBox3 bb : boxes.get()) { + //&&addBBoxPoly(bb, c); + //bb.shrink(0.98); + addBBoxPoly2(bb, c); + } + } + + void addBBoxPoly2(const BBox3& bb, const K::GnuplotColor& color) { + + + K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::SOLID, color); + K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 1, color); + + K::GnuplotObjectPolygon* gpol1 = new K::GnuplotObjectPolygon(filler, stroke); + gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol1->close(); + gpol1->setZIndex(bb.getMin().z - 0.1f); + splot.getObjects().add(gpol1); + + K::GnuplotColor color2 = K::GnuplotColor::fromRGB(128,128,128); + K::GnuplotStroke stroke2(K::GnuplotDashtype::NONE, 1, color2); + K::GnuplotFill noFiller = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color2); + + K::GnuplotObjectPolygon* gpol2 = new K::GnuplotObjectPolygon(noFiller, stroke2); + gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol2->close(); + gpol2->setZIndex(bb.getMin().z + 3.1f); + splot.getObjects().add(gpol2); + + K::GnuplotObjectPolygon* gpol3a = new K::GnuplotObjectPolygon(noFiller, stroke2); + gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol3a->setZIndex(bb.getMin().z + 1.1f); + splot.getObjects().add(gpol3a); + K::GnuplotObjectPolygon* gpol3b = new K::GnuplotObjectPolygon(noFiller, stroke2); + gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol3b->setZIndex(bb.getMin().z + 1.1f); + splot.getObjects().add(gpol3b); + K::GnuplotObjectPolygon* gpol3c = new K::GnuplotObjectPolygon(noFiller, stroke2); + gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol3c->setZIndex(bb.getMin().z + 1.1f); + splot.getObjects().add(gpol3c); + K::GnuplotObjectPolygon* gpol3d = new K::GnuplotObjectPolygon(noFiller, stroke2); + gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST)); + gpol3d->setZIndex(bb.getMin().z + 1.1f); + splot.getObjects().add(gpol3d); + + + } + + void addBBoxPoly(const BBox3& bb, K::GnuplotColor c) { + + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST)); + poly->close(); + poly->setStroke(K::GnuplotStroke::NONE()); + poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, c)); + splot.getObjects().add(poly); + + } + + void setGroundTruth(const Point3 pos_m) { + gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n"; + } + void setCurEst(const Point3 pos_m) { + gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n"; + } + + + void setPaletteRedBlue() { + + float max = -9999; + float min = +9999; + for (const auto& e : cpoints.get()) { + if (e.color > max) {max = e.color;} + if (e.color < min) {min = e.color;} + } + setPaletteRedBlue(min, max); + + } + + void setPaletteRedBlue(const float blueVal, const float redVal) { + + // we need to map the range from [blueVal:redVal] to [0:1] + const float min = blueVal; + const float max = redVal; + const float range = (max - min); + const float center01 = (0-min)/range; + + // values above 0 dB = red + // values below 0 dB = blue + gp << "set palette model RGB\n"; + gp << "cen01 = " << center01 << "\n"; + gp << "r(x) = (x < cen01) ? 0 : ((x-cen01) / (1-cen01))\n"; + gp << "g(x) = 0\n"; + gp << "b(x) = (x > cen01) ? 0 : (1 - (x/cen01))\n"; + gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n"; + } + + void addLabel(const std::string& txt, const Point3 pos) { + //gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n"; + splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n"; + } + + void setActivity(const int act) { + + std::string activity = "Unkown"; + if(act == 0){ + activity = "Walking"; + } else if(act == 1) { + activity = "Standing"; + } else if(act == 2) { + activity = "Up"; + } else if(act == 3) { + activity = "Down"; + } + + gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\n"; + } + + void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) { + std::vector points = { + Point3(p1.x, p1.y, p1.z), + Point3(p2.x, p1.y, p1.z), + Point3(p2.x, p2.y, p1.z), + Point3(p1.x, p2.y, p1.z), + Point3(p1.x, p1.y, p1.z), + }; + addPolygon(points, c.toHEX(), front, fill); + } + + void addRectangleW(const Point3 p1, const Point3 p2, const K::GnuplotColor c, const float w, bool front = false) { + std::vector points = { + Point3(p1.x, p1.y, p1.z), + Point3(p2.x, p1.y, p1.z), + Point3(p2.x, p2.y, p1.z), + Point3(p1.x, p2.y, p1.z), + Point3(p1.x, p1.y, p1.z), + }; + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->getStroke().setWidth(w); + poly->getStroke().setColor(c); + poly->setFront(front); + for (const Point3 &p : points) { + poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST)); + } + splot.getObjects().add(poly); + } + + K::GnuplotObjectPolygon* addStartIndicator(const Point3 pt, const std::string& color, const float s = 2) { + +// for (const Point3 p : points) { +// if (p.z < settings.minZ) {return nullptr;} +// if (p.z > settings.maxZ) {return nullptr;} +// } + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color))); + poly->setStroke(K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromRGB(0,0,0))); + //poly->setStroke(K::GnuplotStroke::NONE()); + poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST)); + poly->close(); + poly->setFront(true); + splot.getObjects().add(poly); + + return poly; + + } + + K::GnuplotObjectPolygon* addPolygon(const std::vector& points, const std::string& color, bool front = false, bool fill = true, const float alpha = 1) { + + for (const Point3 &p : points) { + if (p.z < settings.minZ) {return nullptr;} + if (p.z > settings.maxZ) {return nullptr;} + } + + const K::GnuplotFill pfill = (fill) ? (K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotFill::NONE()); + const K::GnuplotStroke pstroke = (!fill) ? (K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1.0, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotStroke::NONE()); + + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(pfill, pstroke); + for (const Point3 &p : points) { + poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST)); + poly->setZIndex(p.z); // manual depth ordering + poly->getFill().setAlpha(alpha); + } + poly->setFront(front); + + splot.getObjects().add(poly); + + +// gp << "set object polygon from "; +// for (size_t i = 0; i < points.size(); ++i) { +// const Point3 p = points[i]; +// if (i > 0) {gp << " to ";} +// gp << p.x << "," << p.y << "," << p.z << " "; +// } +// gp << (front ? "front" : ""); +// if (fill) {gp << " fs solid ";} else {gp << " fs transparent ";} +// gp << " fc rgb " << "'" << color << "'"; +// gp << "\n"; + + return poly; + + } + + void setZRange(const float min, const float max) { + gp << "set zrange [" << min << ":" << max << "]\n"; + } + + + K::GnuplotObjectPolygon* addFloorRect(const Point3 pos_m, const float size, Color c, float ratio = 1.0) { + + const Point3 p1 = pos_m + Point3(-size, -size/ratio, 0); + const Point3 p2 = pos_m + Point3(+size, -size/ratio, 0); + const Point3 p3 = pos_m + Point3(+size, +size/ratio, 0); + const Point3 p4 = pos_m + Point3(-size, +size/ratio, 0); + + std::vector points = {p1,p2,p3,p4,p1}; + + return addPolygon(points, c.toHEX(), false, true); + +// gp << "set object polygon from "; +// for (size_t i = 0; i < points.size(); ++i) { +// const Point3 p = points[i]; +// if (i > 0) {gp << " to ";} +// gp << p.x << "," << p.y << "," << p.z << " "; +// } +// gp << "front fs solid fc rgb " << "'" << c.toHEX() << "'"; +// gp << "\n"; + + } + + template void showParticles(const std::vector& particles) { + this->particles.clear(); + float min = +999; + float max = -999; + for (const T& p : particles) { + const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z); + const double prob = std::pow(p.weight, 0.25); + this->particles.add(p3, prob); + if (prob > max) {max = prob;} + if (prob < min) {min = prob;} + } + splot.getAxisCB().setRange(min, max + 0.000001f); + } + + // estimated path + void addEstimationNode(const Point3 pos){ + K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10); + pathEst.add(est); + } + + + void setTitle(const std::string& title) { + gp << "set title '" << title << "'\n"; + } + + void setGroundTruth(const std::vector indices) { + const std::vector path = FloorplanHelper::getGroundTruth(map, indices); + pathReal.clear(); + for (const Point3& p : path) { + pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z)); + } + } + + template void setPathToDestination(std::vector>& path) { + shortestPath.clear(); + for (auto& e : path) { + K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z); + shortestPath.add(gp); + } + } + + void equalXY() { + gp << "set view equal xy\n"; + } + + void setView(const float degX, const float degY) { + //gp << "set view " << degX << "," << degY << "\n"; + splot.getView().setCamera(degX, degY); + } + + void setScale(const float x, const float y, const float ox = 0, const float oy = 0) { + gp << "set multiplot layout 1,1 scale " << x << "," << y << " offset " << ox << "," << oy << "\n"; + } + + void writeCodeTo(const std::string& file) { + this->codeFile = file; + } + + void noFrame() { + gp << "unset border\n"; +// gp << "unset xtics\n"; +// gp << "unset ytics\n"; +// gp << "unset ztics\n"; + splot.getAxisX().setTicsVisible(false); + splot.getAxisY().setTicsVisible(false); + splot.getAxisZ().setTicsVisible(false); + } + + void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5f, 5.1f)) { + gp.setTerminal("epslatex", size); + gp.setOutput(file); + } + + void plot() { + + this->mapOutlineConcrete.getStroke().setColor(settings.outlineColor); + this->mapOutlineDrywall.getStroke().setColor(settings.outlineColor); + this->mapOutlineGlass.getStroke().setColor(settings.outlineColor); + + gp.draw(splot); + gp << "unset multiplot\n"; // scaling + if (codeFile != "") { + std::ofstream out(codeFile); + out << gp.getBuffer(); + out.close(); + } + gp.flush(); + } + + void closeStream(){ + gp.close(); + } + + void saveToFile(std::ofstream& stream){ + gp.draw(splot); + stream << "set terminal x11 size 2000,1500\n"; + stream << gp.getBuffer(); + stream << "pause -1\n"; + gp.flush(); + } + + void printOverview(const std::string& path) { + gp << "set terminal png size 2000,1500\n"; + gp << "set output '" << path << "_overview" << ".png'\n"; + gp << "set view 75,60\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + } + + void buildFloorplan() { + + std::vector floors; + + BBox3 bbox = FloorplanHelper::getBBox(map); + + // only some floors?? + if (settings.floors.empty()) { + floors = map->floors; + } else { + for (int i : settings.floors) { + floors.push_back(map->floors[static_cast(i)]); + } + } + +// mapOutlineDrywall.addSegment( +// K::GnuplotPoint3(bbox.getMin().x, bbox.getMin().y, bbox.getMin().z), +// K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z) +// ); + + splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x)); + splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y)); + splot.getAxisZ().setRange(K::GnuplotAxis::Range(0, 11)); + + // process each selected floor + for (Floorplan::Floor* floor : floors) { + + const float vo = floor->atHeight * 4.5f; + + // plot the floor's outline + if (settings.outline) { + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + + if (floor->atHeight < settings.minZ) {continue;} + if (floor->atHeight > settings.maxZ) {continue;} + + // for toni + if (settings.skipI1) { + if (floor->atHeight == 4.0f) { + //if (poly->poly.points[2].y < 0) { + if (poly->poly.points[0].x > 70 && poly->poly.points[0].y < 50) { + continue; + } + } + } + + const float v = 180 + vo; + + K::GnuplotColor color = K::GnuplotColor::fromRGB(static_cast(v),static_cast(v),static_cast(v)); + if (poly->outdoor) {color = K::GnuplotColor::fromRGB(180, 240, 180);} + if (poly->method == Floorplan::OutlineMethod::REMOVE) {color = K::GnuplotColor::fromRGB(245,245,245);} + K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color); + K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, K::GnuplotStroke::NONE()); + for (Point2 pt : poly->poly.points) { + K::GnuplotCoordinate3 coord(pt.x, pt.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST); + gpol->add(coord); + } + gpol->close(); + gpol->setZIndex(floor->atHeight - 0.1f); // below the lines + //gpol->setFront(true); + splot.getObjects().add(gpol); + + } + } + + // plot obstacles? + if (settings.obstacles) { + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line) { + + if (floor->atHeight < settings.minZ) {continue;} + if (floor->atHeight > settings.maxZ) {continue;} + +// const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight); +// const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight); +// switch(line->material) { +// case Floorplan::Material::CONCRETE: mapOutlineConcrete.addSegment(p1, p2); break; +// case Floorplan::Material::GLASS: mapOutlineGlass.addSegment(p1, p2); break; +// case Floorplan::Material::UNKNOWN: +// case Floorplan::Material::DRYWALL: mapOutlineDrywall.addSegment(p1, p2); break; +// } + +// K::GnuplotObjectArrow* arrow = new K::GnuplotObjectArrow( +// K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST), +// K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST) +// ); +// arrow->setHead(K::GnuplotObjectArrow::Head::NONE); +// splot.getObjects().add(arrow); + + const float v = 140 + vo; + + // drawing outlines as polygon is a hack for correct depth-order in gnuplot + K::GnuplotColor color = (settings.outlineColorCustom) ? (settings.outlineColor) : (K::GnuplotColor::fromRGB(static_cast(v),static_cast(v),static_cast(v))); + K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color); + K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 6, color); + //K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), stroke); + K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke); + //K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), K::GnuplotStroke::NONE()); + + gpol->add(K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST)); + gpol->add(K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST)); + gpol->close(); + gpol->setZIndex(floor->atHeight); // above the ground polygon + //gpol->setFront(true); + splot.getObjects().add(gpol); + + } + } + } + + // plot the stairs as polygon + if (settings.stairs) { + for (Floorplan::Stair* s : floor->stairs) { + std::vector quads = Floorplan::getQuads(s->getParts(), floor); + for (const Floorplan::Quad3& q : quads) { +// K::GnuplotObjectPolygon* poly = addPolygon({q.p1, q.p2, q.p3, q.p4, q.p1}, "#c0c0c0"); +// if (poly) { +// poly->setZIndex(floor->atHeight+1.5); // above the floor +// } + + const float v1 = 180 + q.p1.z * 4.5f; + const float v2 = 140 + q.p1.z * 4.5f; + + const float z = (q.p1.z + q.p2.z + q.p3.z + q.p4.z) / 4.0f; + + if (z < settings.minZ) {continue;} + if (z > settings.maxZ) {continue;} + + K::GnuplotColor color = K::GnuplotColor::fromRGB(static_cast(v1),static_cast(v1),static_cast(v1)); + K::GnuplotColor color2 = K::GnuplotColor::fromRGB(static_cast(v2),static_cast(v2),static_cast(v2)); + K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color); + K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, color2); + K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke); + gpol->add(K::GnuplotCoordinate3(q.p1.x, q.p1.y, q.p1.z, K::GnuplotCoordinateSystem::FIRST)); + gpol->add(K::GnuplotCoordinate3(q.p2.x, q.p2.y, q.p2.z, K::GnuplotCoordinateSystem::FIRST)); + gpol->add(K::GnuplotCoordinate3(q.p3.x, q.p3.y, q.p3.z, K::GnuplotCoordinateSystem::FIRST)); + gpol->add(K::GnuplotCoordinate3(q.p4.x, q.p4.y, q.p4.z, K::GnuplotCoordinateSystem::FIRST)); + gpol->close(); + gpol->setZIndex(z); // above the ground + splot.getObjects().add(gpol); + + } + } + } + + } + + } + +}; + +#endif // PLOTTY_H diff --git a/Settings.h b/Settings.h new file mode 100644 index 0000000..f6ffbe1 --- /dev/null +++ b/Settings.h @@ -0,0 +1,191 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include +#include + +namespace Settings { + + const int numParticles = 2000; + const int numBSParticles = 50; + + namespace IMU { + const float turnSigma = 2.5f; // 3.5 + const float stepLength = 1.00f; + const float stepSigma = 0.15f; //toni changed + } + + const float smartphoneAboveGround = 1.3f; + + const float offlineSensorSpeedup = 2.0f; + + namespace Grid { + constexpr int gridSize_cm = 20; + } + + namespace Smoothing { + const bool activated = true; + const double stepLength = 0.7; + const double stepSigma = 0.2; + const double headingSigma = 25.0; + const double zChange = 0.0; // mu change in height between two time steps + const double zSigma = 0.1; + const int lag = 5; + + } + + namespace KDE { + const Point2 bandwidth(1,1); + const float gridSize = 0.2f; + } + + namespace KDE3D { + const Point3 bandwidth(1.5, 1.5, 0.2f); + const Point3 gridSize(0.2f, 0.2f, 0.2f); // in meter + } + + //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination + const GridPoint destination = GridPoint(0,0,0); // do not use destination + + namespace SensorDebug { + const Timestamp updateEvery = Timestamp::fromMS(200); + } + + namespace WiFiModel { + constexpr float sigma = 2.0; + /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ + constexpr float TXP = -45; + constexpr float EXP = 2.3f; + constexpr float WAF = -11.0; + + const bool optimize = true; + const bool useRegionalOpt = false; + + // how to perform VAP grouping. see + // - calibration in Controller.cpp + // - eval in Filter.h + // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! + const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM + } + + namespace BeaconModel { + constexpr float sigma = 8.0; + constexpr float TXP = -71; + constexpr float EXP = 1.5; + constexpr float WAF = -20.0; //-5 //20?? + } + + namespace MapView3D { + const int maxColorPoints = 1000; + constexpr int fps = 15; + const Timestamp msPerFrame = Timestamp::fromMS(1000/fps); + } + + namespace Filter { + const Timestamp updateEvery = Timestamp::fromMS(500); + constexpr bool useMainThread = false; // perform filtering in the main thread + } + + const std::string mapDir = "/mnt/Daten/Dokumente/programme/localization/maps/museum/"; + const std::string dataDir = "/mnt/Daten/Dokumente/programme/localization/measurements/rothenburg/2018/"; + const std::string errorDir = dataDir + "results/"; + + /** describes one dataset (map, training, parameter-estimation, ...) */ + struct DataSetup { + std::string map; + std::vector training; + std::string fingerprints; + std::string wifiModel; + int numGTPoints; + }; + + /** all configured datasets */ + static struct Data { + + DataSetup Path0 = { + + mapDir + "map42_ap_path0.xml", + + { + dataDir + "nexus/Path0_4113.csv", + dataDir + "nexus/Path0_6563.csv", + dataDir + "pixel/Path0_1498.csv", + dataDir + "pixel/Path0_2529.csv", + //dataDir + "pixel/Path0_7380.csv", + //dataDir + "samsung/Path0_4208.csv", + //dataDir + "samsung/Path0_6812.csv", + }, + + dataDir + "nexus/fingerprints/wifi_fp.dat", + dataDir + "wifimodel.dat", + 15 + }; + + DataSetup Path1 = { + + mapDir + "map42_ap_path1_hofzu.xml", + + { + dataDir + "nexus/Path1_1548.csv", + dataDir + "nexus/Path1_9477.csv", + dataDir + "pixel/Path1_2468.csv", + dataDir + "pixel/Path1_5497.csv", + dataDir + "pixel/Path1_8787.csv", + dataDir + "samsung/Path1_5745.csv", + dataDir + "samsung/Path1_9130.csv" + }, + + dataDir + "nexus/fingerprints/wifi_fp.dat", + dataDir + "wifimodel.dat", + 32 + }; + + DataSetup Path2 = { + + mapDir + "map42_ap_path2.xml", + + { + dataDir + "nexus/Path2_0374.csv", + dataDir + "nexus/Path2_4718.csv", + dataDir + "nexus/Path2_8766.csv", + dataDir + "pixel/Path2_4336.csv", + //dataDir + "museum/Pixel/Path2_4341.csv", + dataDir + "pixel/Path2_9038.csv", + dataDir + "samsung/Path2_3326.csv", + dataDir + "samsung/Path2_5814.csv" + }, + + dataDir + "nexus/fingerprints/wifi_fp.dat", + dataDir + "wifimodel.dat", + 44 + }; + + DataSetup Path3 = { + + mapDir + "map42_ap_path3.xml", + + { + //dataDir + "nexus/Path3_4519.csv", + //dataDir + "nexus/Path3_5929.csv", + dataDir + "pixel/Path3_6307.csv", + //dataDir + "pixel/Path3_5451.csv", //geht nur bis gt-punkt 31 + //dataDir + "pixel/Path3_9243.csv", + //dataDir + "samsung/Path3_7610.csv", + //dataDir + "samsung/Path3_7819.csv" + }, + + dataDir + "nexus/fingerprints/wifi_fp.dat", + dataDir + "wifimodel_new.dat", + 46 + }; + + } data; + +} + + + + +#endif // SETTINGS_H diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..15b9069 --- /dev/null +++ b/main.cpp @@ -0,0 +1,430 @@ +//#include "main.h" + +#include "Settings.h" +#include "Plotty.h" + +// navMesh +#include "navMesh/mesh.h" +#include "navMesh/filter.h" +#include "navMesh/meshPlotter.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include +#include +#include + +#include + +Stats::Statistics run(Settings::DataSetup setup, unsigned long numFile, std::string folder) { + + // reading file + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); + Offline::FileReader fr(setup.training[numFile]); + WiFiFingerprints fingerprints(setup.fingerprints); + std::ifstream inp(setup.wifiModel, std::ifstream::binary); + + // ground truth + std::vector gtPath; + for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);} + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); + Stats::Statistics errorStats; + + //calculate distance of path + std::vector::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); + float distance = 0; + for(unsigned long i = 1; i < gtEntries.size(); ++i){ + distance += gtEntries[i].value.getDistance(gtEntries[i-1].value); + } + + std::cout << "Distance of Path: " << distance << std::endl; + + // error file + const long int t = static_cast(time(nullptr)); + auto evalDir = std::experimental::filesystem::path(Settings::errorDir); + evalDir.append(folder); + + if (!std::experimental::filesystem::exists(evalDir)) { + std::experimental::filesystem::create_directory(evalDir); + } + + std::ofstream errorFile; + errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + std::ofstream activityFile; + activityFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_activity.csv"); + + + // wifi +// WiFiModelLogDistCeiling WiFiModel(map); +// WiFiModelPerFloor WiFiModelPerFloor(map); +// WiFiModelPerBBox WiFiModelPerBBox(map); + + WiFiModel* wifiModel = nullptr; + + // with optimization + if(Settings::WiFiModel::optimize){ + + if (!inp.good() || (inp.peek()) || inp.eof()) { + + Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!"); + + if (Settings::WiFiModel::useRegionalOpt) { + + // use a regional optimization scheme (one per floor) + + WiFiOptimizer::PerFloor opt(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY); + WiFiOptimizer::LogDistCeiling ldc(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY); + + // add all fingerprints to the optimizer (optimizer will add them to the correct floor/model) + for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { + opt.addFingerprint(fp); + } + + wifiModel = dynamic_cast(opt.optimizeAll(ldc.MIN_2_FPS)); + wifiModel->saveXML(setup.wifiModel); + + } else { + + /** NOTE: Funktioniert fürs Museum VIEL VIEL Besser */ + // use one model per AP for the whole map + + wifiModel = dynamic_cast(new WiFiModelLogDistCeiling(map)); + WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); + for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { + opt.addFingerprint(fp); + } + const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); + + for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { + const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); + (dynamic_cast(wifiModel))->addAP(ap.mac, entry); + } + + wifiModel->saveXML(setup.wifiModel); + + } + + } else { + + // load WiFiModel from file. The factory will create the correct instance + //WiFiModel->loadXML(setup.wifiModel); + WiFiModelFactory fac(map); + wifiModel = fac.loadXML(setup.wifiModel); + + } + + } else { + // without optimization + wifiModel = dynamic_cast(new WiFiModelLogDistCeiling(map)); + (dynamic_cast(wifiModel))->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + Assert::isFalse(wifiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + } + + + + // mesh + NM::NavMeshSettings set; + MyNavMesh mesh; + MyNavMeshFactory fac(&mesh, set); + fac.build(map); + + //add destination + NM::NavMeshDijkstra::stamp(mesh, gtEntries.back().value); + + const Point3 srcPath0(26, 43, 7.5f); + const Point3 srcPath1(62, 38, 1.7f); + const Point3 srcPath2(62, 38, 1.8f); + const Point3 srcPath3(62, 38, 1.8f); + + // add shortest-path to destination + //const Point3 dst(51, 45, 1.7); + //const Point3 dst(25, 45, 0); + //NM::NavMeshDijkstra::stamp(mesh, dst); + + // debug show + //MeshPlotter dbg; + //dbg.addFloors(map); + //dbg.addOutline(map); + //dbg.addMesh(mesh); + //dbg.addDijkstra(mesh); + //dbg.draw(); + + Plotty plot(map); + plot.buildFloorplan(); + plot.setGroundTruth(gtPath); + + // particle-filter + const int numParticles = Settings::numParticles; + //auto init = std::make_unique(&mesh, srcPath0); // known position + auto init = std::make_unique(&mesh); // uniform distribution + auto eval = std::make_unique(*wifiModel); + auto trans = std::make_unique(mesh, *wifiModel); + + auto resample = std::make_unique>(); + //auto resample = std::make_unique>(&mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth); + //auto resample = std::make_unique>(&mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth, 0.75); + //auto resample = std::make_unique>(); + //auto resample = std::make_unique>(); + + //auto estimate = std::make_unique>(map, 0.2, Point2(1,1)); + auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); + + // setup + MyFilter pf(numParticles, std::move(init)); + pf.setEvaluation(std::move(eval)); + pf.setTransition(std::move(trans)); + pf.setResampling(std::move(resample)); + pf.setEstimation(std::move(estimate)); + pf.setNEffThreshold(0.5); + + // sensors + MyControl ctrl; + MyObservation obs; + + StepDetection sd; + PoseDetection pd; + TurnDetection td(dynamic_cast(&pd)); + RelativePressure relBaro; + ActivityDetector act; + relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + Timestamp lastTimestamp = Timestamp::fromMS(0); + int gtActivity = 0; + + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) { + + const Timestamp ts = Timestamp::fromMS(static_cast(e.ts)); + + if (e.type == Offline::Sensor::WIFI) { + obs.wifi = fr.getWiFiGroupedByTime()[static_cast(e.idx)].data; + ctrl.wifi = fr.getWiFiGroupedByTime()[static_cast(e.idx)].data; + + } else if (e.type == Offline::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[static_cast(e.idx)].data)) { + ++ctrl.numStepsSinceLastEval; + } + const Offline::TS& _acc = fr.getAccelerometer()[static_cast(e.idx)]; + pd.addAccelerometer(ts, _acc.data); + + //simpleActivity walking / standing + act.add(ts, fr.getAccelerometer()[static_cast(e.idx)].data); + + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[static_cast(e.idx)]; + const float delta_gyro = td.addGyroscope(ts, _gyr.data); + + ctrl.headingChangeSinceLastEval += delta_gyro; + + } else if (e.type == Offline::Sensor::BARO) { + relBaro.add(ts, fr.getBarometer()[static_cast(e.idx)].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + + //simpleActivity stairs up / down + act.add(ts, fr.getBarometer()[static_cast(e.idx)].data); + + activityFile << ts.ms() << " " << gtActivity << " " << static_cast(act.get()) << "\n"; + + } else if (e.type == Offline::Sensor::ACTIVITY) { + + //get the activity recorded by the user by pressing a button + gtActivity = fr.getActivity()[static_cast(e.idx)].data; + } + + if (ctrl.numStepsSinceLastEval > 0) { + + obs.currentTime = ts; + ctrl.currentTime = ts; + +// if(ctrl.numStepsSinceLastEval > 0){ +// pf.updateTransitionOnly(&ctrl); +// } + MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs); + ctrl.afterEval(); + Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1f); + lastTimestamp = ts; + + ctrl.lastEstimate = est.pos.pos; + + //plot + //dbg.showParticles(pf.getParticles()); + //dbg.setCurPos(est.pos.pos); + //dbg.setGT(gtPos); + //dbg.addEstimationNode(est.pos.pos); + //dbg.addGroundTruthNode(gtPos); + //dbg.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); + //dbg.draw(); + + //get shortest path to destination and plot + est.pos = mesh.getLocationNearestTo(est.pos.pos); + std::vector> path = est.pos.tria->getPathToDestination(est.pos.pos); + plot.setPathToDestination(path); + + plot.showParticles(pf.getParticles()); + plot.setCurEst(est.pos.pos); + plot.setGroundTruth(gtPos); + plot.addEstimationNode(est.pos.pos); + plot.setActivity(static_cast(act.get())); + + plot.plot(); + + //error calc with penalty for wrong floor + float errorFactor = 3.0f; + Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z); + Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z); + float err_m = gtPosError.getDistance(estError); + errorStats.add(err_m); + errorFile << ts.ms() << " " << err_m << "\n"; + + + //stuff for drawing +// plot.gp << "set terminal png size 1280,720\n"; +// plot.gp.setOutput("/tmp/videoSparkasse/" + std::to_string(i++) + ".png"); + +// int degree = ((30 - i) % 360); +// if (degree < 0){degree += 360;} +// plot.gp << "set view 63,"<< degree << "\n"; +// plot.gp << "set autoscale xy\n"; +// plot.gp << "set autoscale z\n"; +// plot.plot(); + } + } + + + + // get someting on console + std::cout << "Statistical Analysis Filtering: " << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; + + // save the statistical data in file + errorFile << "========================================================== \n"; + errorFile << "Average of all statistical data: \n"; + errorFile << "Median: " << errorStats.getMedian() << "\n"; + errorFile << "Average: " << errorStats.getAvg() << "\n"; + errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n"; + errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n"; + errorFile.close(); + + //activityFile << "===================================================== \n"; + //activityFile << "Falsche Zustandswechsel: " << numFalseActivityChange; + activityFile.close(); + + /* plot in gp file */ + std::ofstream plotFile; + plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); + + //save also a png image, just for a better overview + plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.plot(); + + plot.closeStream(); + + return errorStats; +} + +int main() { + + Stats::Statistics statsAVG; + Stats::Statistics statsMedian; + Stats::Statistics statsSTD; + Stats::Statistics statsQuantil; + Stats::Statistics tmp; + + std::string evaluationName = "Test_ParticleFilter_Standard"; + + for(int i = 0; i < 1; ++i){ + + + for(unsigned long j = 0; j < Settings::data.Path0.training.size(); ++j){ + tmp = run(Settings::data.Path0, j, evaluationName); + statsMedian.add(tmp.getMedian()); + statsAVG.add(tmp.getAvg()); + statsSTD.add(tmp.getStdDev()); + statsQuantil.add(tmp.getQuantile(0.75)); + } + +// for(int j = 0; j < Settings::data.Path1.training.size(); ++j){ +// tmp = run(Settings::data.Path1, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + +// for(int j = 0; j < Settings::data.Path2.training.size(); ++j){ +// tmp = run(Settings::data.Path2, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + +// for(int j = 0; j < Settings::data.Path3.training.size(); ++j){ +// tmp = run(Settings::data.Path3, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + +// for(int j = 0; j < 1; ++j){ +// tmp = run(Settings::data.Stairs, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + + std::cout << "Iteration " << i << " completed" << std::endl; + } + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data: " << std::endl; + std::cout << "Median: " << statsMedian.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + std::ofstream finalStatisticFile; + finalStatisticFile.open (Settings::errorDir + evaluationName + ".csv", std::ios_base::app); + + finalStatisticFile << "========================================================== \n"; + finalStatisticFile << "Average of all statistical data: \n"; + finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n"; + finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n"; + finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n"; + finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n"; + finalStatisticFile << "========================================================== \n"; + + finalStatisticFile.close(); + + //std::this_thread::sleep_for(std::chrono::seconds(60)); + +} diff --git a/main.h b/main.h new file mode 100644 index 0000000..e62a9c5 --- /dev/null +++ b/main.h @@ -0,0 +1,6 @@ +#ifndef NAV_MESH_MAIN_H +#define NAV_MESH_MAIN_H + + + +#endif diff --git a/navMesh/filter.h b/navMesh/filter.h new file mode 100644 index 0000000..65797db --- /dev/null +++ b/navMesh/filter.h @@ -0,0 +1,318 @@ +#ifndef NAV_MESH_FILTER_H +#define NAV_MESH_FILTER_H + +#include "mesh.h" +#include "../Settings.h" +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +struct MyState { + + /** the state's position (within the mesh) */ + MyNavMeshLocation pos; + + /** the state's heading */ + Heading heading; + + MyState() : pos(), heading(0) {} + + MyState(Point3 p) : pos(p, nullptr), heading(0){} + + MyState& operator += (const MyState& o) { + pos.tria = nullptr; // impossible + pos.pos += o.pos.pos; + return *this; + } + + MyState& operator /= (const float val) { + pos.tria = nullptr; // impossible + pos.pos /= val; + return *this; + } + + MyState operator * (const float val) const { + MyState res; + res.pos.pos = pos.pos * val; + return res; + } + + float getX(){ + return pos.pos.x; + } + + float getY() { + return pos.pos.y; + } + + float getZ() { + return pos.pos.z; + } + + + float getBinValue(const int dim) const { + switch (dim) { + case 0: return this->pos.pos.x; + case 1: return this->pos.pos.y; + case 2: return this->pos.pos.z; + case 3: return this->heading.getRAD(); + } + throw "cant find this value within the bin"; + } + +}; + +struct MyControl { + + int numStepsSinceLastEval = 0; + float headingChangeSinceLastEval = 0; + + void afterEval() { + numStepsSinceLastEval = 0; + headingChangeSinceLastEval = 0; + } + + //wifi + WiFiMeasurements wifi; + + //time + Timestamp currentTime; + + //last estimation + Point3 lastEstimate = Point3(26, 43, 7.5); + +}; + +struct MyObservation { + + // pressure + float sigmaPressure = 0.10f; + float relativePressure = 0; + + //wifi + WiFiMeasurements wifi; + + //time + Timestamp currentTime; + + //activity + Activity activity; + +}; + +class MyPFInitUniform : public SMC::ParticleFilterInitializer { + + const MyNavMesh* mesh; + +public: + + MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) { + } + + virtual ~MyPFInitUniform(){} + + virtual void initialize(std::vector>& particles) override { + + /** random position and heading within the mesh */ + Distribution::Uniform dHead(0, 2*M_PI); + MyNavMeshRandom rnd = mesh->getRandom(); + for (SMC::Particle& p : particles) { + p.state.pos = rnd.draw(); + p.state.heading = static_cast(dHead.draw()); + p.weight = 1.0 / particles.size(); + } + } + +}; + +class MyPFInitFixed : public SMC::ParticleFilterInitializer { + + const MyNavMesh* mesh; + const Point3 pos; + +public: + + MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) { + } + + virtual ~MyPFInitFixed(){} + + virtual void initialize(std::vector>& particles) override { + + /** random position and heading within the mesh */ + Distribution::Uniform dHead(0, 2*M_PI); + for (SMC::Particle& p : particles) { + p.state.pos = mesh->getLocation(pos); + p.state.heading = static_cast(dHead.draw()); + p.weight = 1.0 / particles.size(); + } + } + +}; + + +class MyPFTrans : public SMC::ParticleFilterTransition { + + //using MyNavMeshWalk = NM::NavMeshWalkSimple; + //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; + //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; + using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim; + + MyNavMeshWalk walker; + + WiFiQualityAnalyzer analyzer; + WiFiObserverFree wifiProbability; + + SMC::ParticleFilterEstimationBoxKDE estimator; + +public: + + //std::vector listRadiusSub; + + MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) : + walker(mesh), + wifiProbability(Settings::WiFiModel::sigma, wifiModel){ + + // how to evaluate drawn points + walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); + walker.addEvaluator(new NM::WalkEvalDistance(0.1)); + //walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target + } + + void transition(std::vector>& particles, const MyControl* control) override { + + // walking and heading random + Distribution::Normal dStepSizeFloor(0.70f, 0.1f); + Distribution::Normal dStepSizeStair(0.35f, 0.1f); + Distribution::Normal dHeading(0.0, 0.1f); + +// #pragma omp parallel for num_threads(3) + for (unsigned long i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; + + // how to walk + MyNavMeshWalkParams params; + params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw(); + params.numSteps = control->numStepsSinceLastEval; + params.start = p.state.pos; + + params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); + params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); + + if(params.stepSizes.stepSizeFloor_m < 0.1f || params.stepSizes.stepSizeStair_m < 0.1f){ + params.stepSizes.stepSizeFloor_m = 0.1f; + params.stepSizes.stepSizeStair_m = 0.1f; + } + + // walk + MyNavMeshWalk::ResultEntry res = walker.getOne(params); + + // assign back to particle's state + p.weight *= res.probability; + p.state.pos = res.location; + p.state.heading = res.heading; + + } + + // reset the control (0 steps, 0 delta-heading) + //control->afterEval(); + + } + + +}; + +class MyPFEval : public SMC::ParticleFilterEvaluation { + + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; + + //TODO: add this to transition probability + float getStairProb(const SMC::Particle& p, const Activity act) { + + const float kappa = 0.85f; + + switch (act) { + + case Activity::WALKING: + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::FLOOR_INDOOR)) {return kappa;} + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::DOOR)) {return kappa;} + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::STAIR_LEVELED)) {return kappa;} + {return 1-kappa;} + + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::STAIR_SKEWED)) {return kappa;} + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::STAIR_LEVELED)) {return kappa;} + if (p.state.pos.tria->getType() == static_cast(NM::NavMeshType::ELEVATOR)) {return kappa;} + {return 1-kappa;} + } + return 1.0; + } + +public: + + //MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} + //MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){} + MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){} + + virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { + + double sum = 0; + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + + //#pragma omp parallel for num_threads(3) + for (size_t i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; + + double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); + double pBluetooth = + double pStair = static_cast(getStairProb(p, observation.activity)); + + double prob = pWifi * pStair; + + p.weight *= prob; + + #pragma omp atomic + sum += prob; + } + + return sum; + + } + +}; + + +using MyFilter = SMC::ParticleFilter; + + +#endif diff --git a/navMesh/mesh.h b/navMesh/mesh.h new file mode 100644 index 0000000..1652081 --- /dev/null +++ b/navMesh/mesh.h @@ -0,0 +1,31 @@ +#ifndef NAV_MESH_MESH_H +#define NAV_MESH_MESH_H + + +#include +#include +#include +#include + +#include +#include + +/** the triangle to use with the nav-mesh */ +class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra { + + // add own parameters here + +public: + + MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) { + } + +}; + +using MyNavMeshFactory = NM::NavMeshFactory; +using MyNavMesh = NM::NavMesh; +using MyNavMeshLocation = NM::NavMeshLocation; +using MyNavMeshRandom = NM::NavMeshRandom; +using MyNavMeshWalkParams = NM::NavMeshWalkParams; + +#endif diff --git a/navMesh/meshPlotter.h b/navMesh/meshPlotter.h new file mode 100644 index 0000000..2b37838 --- /dev/null +++ b/navMesh/meshPlotter.h @@ -0,0 +1,242 @@ +#ifndef MESHPLOTTER_H +#define MESHPLOTTER_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class NavMeshTriangleDijkstra; + +/** + * debug plot NavMeshes + */ +class MeshPlotter { + +public: + + K::Gnuplot gp; + K::GnuplotSplot plot; + K::GnuplotSplotElementLines pFloor; + K::GnuplotSplotElementLines pOutline; + K::GnuplotSplotElementLines lines; + K::GnuplotSplotElementPoints border; + K::GnuplotSplotElementColorPoints particles; + K::GnuplotSplotElementColorPoints distances; + K::GnuplotSplotElementLines pathEstimated; + K::GnuplotSplotElementLines shortestPath; + K::GnuplotSplotElementLines groundtruthPath; + +private: + + K::GnuplotFill gFill[6] = { + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed + }; + +public: + + MeshPlotter() { + gp << "set view equal xy\n"; + plot.add(&lines); lines.setShowPoints(true); + plot.add(&border); + plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2); + plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00"); + plot.add(&groundtruthPath); groundtruthPath.getStroke().setWidth(2); groundtruthPath.getStroke().getColor().setHexStr("#000000"); + plot.add(&distances); distances.setPointSize(0.75); distances.setPointType(7); + plot.add(&shortestPath); shortestPath.getStroke().setWidth(3); + plot.add(&pFloor); + plot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999"); + } + + void draw() { + gp.draw(plot); + gp.flush(); + } + + template void showParticles(const std::vector& particles) { + this->particles.clear(); + double min = +999; + double max = -999; + for (const T& p : particles) { + const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z); + const double prob = std::pow(p.weight, 0.25); + this->particles.add(p3, prob); + if (prob > max) {max = prob;} + if (prob < min) {min = prob;} + } + plot.getAxisCB().setRange(min, max + 0.000001); + } + + + template void addMesh(NM::NavMesh& nm) { + + K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); + + const BBox3 bbox = nm.getBBox(); + + border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z)); + border.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z)); +// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0)); +// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0)); +// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z)); + + //stairs in eigene group? vlt gehen dann auch die dellen weg? + + for (const Tria* tria : nm) { + const uint8_t type = tria->getType(); + if (type < 0 || type > 5) { + throw std::runtime_error("out of type-bounds"); + } + K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke); + pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST)); + pol->close(); + pol->setZIndex(tria->getP3().z); + plot.getObjects().add(pol); + + //for (int i = 0; i < nm.getNumNeighbors(tria); ++i) { + // const Tria* o = nm.getNeighbor(tria, i); + // const Point3 p1 = tria->getCenter(); + // const Point3 p2 = o.getCenter(); + // //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1)); + //} + + for (const NM::NavMeshTriangle* o : *tria) { + const Point3 p1 = tria->getCenter(); + const Point3 p2 = o->getCenter(); + // lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1)); + } + + } + + plot.getObjects().reOrderByZIndex(); + + } + + template void addDijkstra(NM::NavMesh& mesh) { + + distances.clear(); + + // ensure Tria extends NavMeshTriangleDijkstra + StaticAssert::AinheritsB(); + + NM::NavMeshRandom rnd = mesh.getRandom(); + + for (int i = 0; i < 5000; ++i) { + NM::NavMeshLocation loc = rnd.draw(); + float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance); + distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v); + } + + +// Distribution::Uniform dist (-0.5, +0.5); +// for (const Tria* t : mesh) { +// const Point3 posC = t->getCenter(); +// distances.add(K::GnuplotPoint3(posC.x+dist.draw(), posC.y+dist.draw(), posC.z), t->distAtCenter); +// const Point3 pos1 = t->getP1(); +// distances.add(K::GnuplotPoint3(pos1.x+dist.draw(), pos1.y+dist.draw(), pos1.z), t->distAtP1); +// const Point3 pos2 = t->getP2(); +// distances.add(K::GnuplotPoint3(pos2.x+dist.draw(), pos2.y+dist.draw(), pos2.z), t->distAtP2); +// const Point3 pos3 = t->getP3(); +// distances.add(K::GnuplotPoint3(pos3.x+dist.draw(), pos3.y+dist.draw(), pos3.z), t->distAtP3); +// } + + } + + template void addDijkstra(std::vector>& path) { + shortestPath.clear(); + for (auto& e : path) { + K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z); + shortestPath.add(gp); + } + } + + void addGroundTruthNode(const Point3 pos) { + K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10); + groundtruthPath.add(gp); + } + + void addEstimationNode(const Point3 pos){ + K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10); + pathEstimated.add(est); + } + + + void setTimeInMinute(const int minutes, const int seconds) { + gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n"; + } + + void setGT(const Point3 pt) { + gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n"; + } + + void setCurPos(const Point3 pt) { + gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n"; + } + + void saveToFile(std::ofstream& stream){ + gp.draw(plot); + stream << "set terminal x11 size 2000,1500\n"; + stream << gp.getBuffer(); + stream << "pause -1\n"; + gp.flush(); + } + + void printOverview(const std::string& path) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_overview" << ".png'\n"; + gp << "set view 75,60\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + draw(); + } + + + //meshless drawing + void addFloors(Floorplan::IndoorMap* map) { + + for (Floorplan::Floor* f : map->floors) { + for (Floorplan::FloorObstacle* obs : f->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line) { + K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight); + K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight); + pFloor.addSegment(p1, p2); + } + } + } + + } + + void addOutline(Floorplan::IndoorMap* map) { + + for (Floorplan::Floor* f : map->floors) { + for (Floorplan::FloorOutlinePolygon* poly : f->outline) { + const int cnt = poly->poly.points.size(); + for (int i = 0; i < cnt; ++i) { + Point2 p1 = poly->poly.points[(i+0)]; + Point2 p2 = poly->poly.points[(i+1)%cnt]; + K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight); + K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight); + pOutline.addSegment(gp1, gp2); + } + } + } + + } + +}; + +#endif // MESHPLOTTER_H