diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt new file mode 100755 index 0000000..90091d4 --- /dev/null +++ b/code/CMakeLists.txt @@ -0,0 +1,97 @@ +# 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(Museum) + +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 +) + + +# system specific compiler flags +ADD_DEFINITIONS( + + #-std=gnu++14 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + + -g3 + #-O2 + -march=native + + -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} + gtest + pthread +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user new file mode 100644 index 0000000..ef94e20 --- /dev/null +++ b/code/CMakeLists.txt.user @@ -0,0 +1,375 @@ + + + + + + EnvironmentId + {1808440d-0151-4aa2-b355-41886486d819} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop Qt 5.7.1 GCC 64bit + Desktop Qt 5.7.1 GCC 64bit + qt.57.gcc_64_kit + 0 + 0 + 0 + + + CMAKE_CXX_COMPILER:STRING=%{Compiler:Executable} + QT_QMAKE_EXECUTABLE:STRING=/opt/Qt/5.7/gcc_64/bin/qmake + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Default + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Debug + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Debug + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Debug + Debug + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=Release + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Release + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release + Release + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=RelWithDebInfo + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Release with Debug Information + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Release with Debug Information + Release with Debug Information + CMakeProjectManager.CMakeBuildConfiguration + + + + CMAKE_BUILD_TYPE:STRING=MinSizeRel + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Minimum Size Release + + + + + all + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + + + clean + + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Minimum Size Release + Minimum Size Release + CMakeProjectManager.CMakeBuildConfiguration + + 5 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy locally + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + Museum + + + /home/toni/Documents/programme/localization/museum/build-code-Desktop_Qt_5_7_0_GCC_64bit-Default + 2 + + Museum + + CMakeProjectManager.CMakeRunConfiguration.Museum + 3768 + false + true + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/code/Plotti.h b/code/Plotti.h new file mode 100644 index 0000000..d40d874 --- /dev/null +++ b/code/Plotti.h @@ -0,0 +1,351 @@ +#ifndef PLOTTI_H +#define PLOTTI_H + +#include "Settings.h" + +#include + +#include +#include + +#include + +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include + +struct Plotti { + + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementPoints pGrid; + K::GnuplotSplotElementLines pFloor; + K::GnuplotSplotElementLines pOutline; + K::GnuplotSplotElementLines pStairs; + K::GnuplotSplotElementPoints pAPs; + K::GnuplotSplotElementPoints pInterest; + K::GnuplotSplotElementPoints pParticles; + K::GnuplotSplotElementPoints pNormal1; + K::GnuplotSplotElementPoints pNormal2; + K::GnuplotSplotElementColorPoints pDistributation1; + K::GnuplotSplotElementColorPoints pDistributation2; + K::GnuplotSplotElementColorPoints pColorPoints; + K::GnuplotSplotElementLines gtPath; + K::GnuplotSplotElementLines estPath; + K::GnuplotSplotElementLines estPathSmoothed; + + Plotti() { + gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n"; + splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.getColor().setHexStr("#888888"); + splot.add(&pAPs); pAPs.setPointSize(0.7); + splot.add(&pColorPoints); pColorPoints.setPointSize(0.6); + splot.add(&pDistributation1); pDistributation1.setPointSize(0.6); + splot.add(&pDistributation2); pDistributation2.setPointSize(0.6); + splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f); + splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f); + splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f); + splot.add(&pFloor); + splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999"); + splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000"); + splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000"); + splot.add(>Path); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000"); + splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00"); + splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#0000ff"); + } + + void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) { + gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n"; + } + + void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) { + gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n"; + } + + + void showAngle(const int idx, const float rad, const Point2 cen, const std::string& str) { + + Point2 rot(0, 1); + Point2 pos = cen + rot.rotated(rad) * 0.05; + + gp << "set label "<> samples){ + + float min = +9999; + float max = -9999; + + pDistributation1.clear(); + for (int i = 0; i < samples.size(); ++i) { + //if (i % 10 != 0) {continue;} + + double prob = samples[i].weight; + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + + K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f); + pDistributation1.add(pos, prob); + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + } + + void debugDistribution2(std::vector> samples){ + + float min = +9999; + float max = -9999; + + pDistributation2.clear(); + for (int i = 0; i < samples.size(); ++i) { + if (i % 25 != 0) {continue;} + + double prob = samples[i].weight; + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + + K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f); + pDistributation2.add(pos, prob); + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + } + + void drawNormalN1(Distribution::NormalDistributionN normParticle){ + + pNormal1.clear(); + for (int i = 0; i < 100000; ++i) { + if (++i % 25 != 0) {continue;} + Eigen::VectorXd vec = normParticle.draw(); + K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z()); + pNormal1.add(pos); + } + + } + + void drawNormalN2(Distribution::NormalDistributionN normParticle){ + + pNormal2.clear(); + for (int i = 0; i < 100000; ++i) { + if (++i % 25 != 0) {continue;} + Eigen::VectorXd vec = normParticle.draw(); + K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z()); + pNormal2.add(pos); + } + } + + void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) { + + WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model); + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan); + + float min = +9999; + float max = -9999; + + const float step = 2.0f; + for (float x = 0; x < 80; x += step) { + for (float y = 0; y < 55; y += step) { + Point3 pt(x,y,z); + double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs); + + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); + } + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + + } + + template void debugProb(Grid& grid, std::function func, const MyObs& obs) { + + pColorPoints.clear(); + +// const float step = 2.0; +// float z = 0; +// for (float x = -20; x < 90; x += step) { +// for (float y = -10; y < 60; y += step) { +// const Point3 pos_m(x,y,z); +// const double prob = func(obs, pos_m); +// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); +// } +// } + + std::minstd_rand gen; + std::uniform_int_distribution dist(0, grid.getNumNodes()-1); + + float min = +9999; + float max = -9999; + + for (int i = 0; i < 10000; ++i) { + int idx = dist(gen); + Node& n = grid[idx]; + const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm); + const Point3 pos_m = pos_cm / 100.0f; + const double prob = func(obs, pos_m); + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob); + } + + if (min == max) {min -= 1;} + + gp << "set cbrange [" << min << ":" << max << "]\n"; + + } + + void addStairs(Floorplan::IndoorMap* map) { + + for (Floorplan::Floor* f : map->floors) { + for (Floorplan::Stair* stair : f->stairs) { + std::vector quads = Floorplan::getQuads(stair->getParts(), f); + for (const Floorplan::Quad3& quad : quads) { + for (int i = 0; i < 4; ++i) { + int idx1 = i; + int idx2 = (i+1) % 4; + pStairs.addSegment( + K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z), + K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z) + ); + } + } + } + } + + } + + 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); + } + } + } + + } + + template void addGrid(Grid& grid) { + pGrid.clear(); + for (const Node& n : grid) { + K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm); + pGrid.add(p/100.0f); + } + } + + template void addParticles(const std::vector>& particles) { + pParticles.clear(); + int i = 0; + for (const SMC::Particle& p : particles) { + if (++i % 25 != 0) {continue;} + K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); + pParticles.add(pos / 100.0f); + } + } + + void show() { + gp.draw(splot); + gp.flush(); + } + + 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 printSingleFloor(const std::string& path, const int floorNum) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_" << floorNum <<".png'\n"; + gp << "set view 0,0\n"; + gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n"; + gp << "set autoscale xy\n"; + } + + void printSideView(const std::string& path, const int degree) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_deg" << degree <<".png'\n"; + gp << "set view 90,"<< degree << "\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + } + + 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"; + } + + +}; + +#endif // PLOTTI_H diff --git a/code/Plotty.h b/code/Plotty.h new file mode 100755 index 0000000..b6c60ae --- /dev/null +++ b/code/Plotty.h @@ -0,0 +1,672 @@ +#ifndef PLOTTY_H +#define PLOTTY_H + +#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::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.20); //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(&pm3doutline); + + splot.add(&points); + points.setPointType(7); + points.setPointSize(0.5); + + splot.add(&cpoints); + cpoints.setPointSize(2); + cpoints.setPointType(7); + + } + + 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.1); + 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.1); + 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.1); + 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.1); + 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.1); + 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.1); + splot.getObjects().add(gpol3d); + + + } + + void addBBox(const BBox3& bb) { + + +// // floor +// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z}); +// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMin().z}); +// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMin().z}); +// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMin().z}); +// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z}); +// mapBBoxes.splitFace(); mapBBoxes.splitFace(); + +// // ceil +// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z}); +// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMax().z}); +// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMax().z}); +// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMax().z}); +// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z}); +// mapBBoxes.splitFace(); mapBBoxes.splitFace(); + +// // up +// mapBBoxes.addSegment({bb.getMin().x, bb.getMin().y, bb.getMin().z}, {bb.getMin().x, bb.getMin().y, bb.getMax().z}); +// mapBBoxes.addSegment({bb.getMax().x, bb.getMin().y, bb.getMin().z}, {bb.getMax().x, bb.getMin().y, bb.getMax().z}); +// mapBBoxes.addSegment({bb.getMin().x, bb.getMax().y, bb.getMin().z}, {bb.getMin().x, bb.getMax().y, bb.getMax().z}); +// mapBBoxes.addSegment({bb.getMax().x, bb.getMax().y, bb.getMin().z}, {bb.getMax().x, bb.getMax().y, bb.getMax().z}); + + } + + 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 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(); + 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;} + } + splot.getAxisCB().setRange(min, max + 0.000001); + } + + // 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)); + } + } + + 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.5, 5.1)) { + 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 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 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[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.5; + + // 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) { + //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(v,v,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.1); // 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(v,v,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.5; + const float v2 = 140 + q.p1.z * 4.5; + + 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(v1,v1,v1); + K::GnuplotColor color2 = K::GnuplotColor::fromRGB(v2,v2,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/code/Settings.h b/code/Settings.h new file mode 100644 index 0000000..20a8468 --- /dev/null +++ b/code/Settings.h @@ -0,0 +1,186 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include +#include + +namespace Settings { + + bool useKLB = false; + + const int numParticles = 5000; + const int numBSParticles = 50; + + namespace IMU { + const float turnSigma = 2.5; // 3.5 + const float stepLength = 1.00; + const float stepSigma = 0.15; //toni changed + } + + const float smartphoneAboveGround = 1.3; + + const float offlineSensorSpeedup = 2; + + 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(100,100); + } + + //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 = 8.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.3; + constexpr float WAF = -11.0; + + const bool optimize = true; + + // 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); + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); + } + + 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 = "../map/"; + const std::string dataDir = "../measurements/"; + 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 */ + struct Data { + + DataSetup Path0 = { + + mapDir + "map42_ap_path0.xml", + + { + dataDir + "museum/Nexus/Path0_4113.csv", + dataDir + "museum/Nexus/Path0_6563.csv", + dataDir + "museum/Pixel/Path0_1498.csv", + dataDir + "museum/Pixel/Path0_2529.csv", + dataDir + "museum/Pixel/Path0_7380.csv", + //dataDir + "museum/Samsung/Path0_4208.csv", + //dataDir + "museum/Samsung/Path0_6812.csv", + }, + + dataDir + "museum/Nexus/fingerprints/wifi_fp.dat", + dataDir + "museum/wifimodel.dat", + 15 + }; + + DataSetup Path1 = { + + mapDir + "map42_ap_path1_hofzu.xml", + + { + dataDir + "museum/Nexus/Path1_1548.csv", + dataDir + "museum/Nexus/Path1_9477.csv", + dataDir + "museum/Pixel/Path1_2468.csv", + dataDir + "museum/Pixel/Path1_5497.csv", + dataDir + "museum/Pixel/Path1_8787.csv", + dataDir + "museum/Samsung/Path1_5745.csv", + dataDir + "museum/Samsung/Path1_9130.csv" + }, + + dataDir + "museum/Nexus/fingerprints/wifi_fp.dat", + dataDir + "museum/wifimodel.dat", + 32 + }; + + DataSetup Path2 = { + + mapDir + "map42_ap_path2.xml", + + { + dataDir + "museum/Nexus/Path2_0374.csv", + dataDir + "museum/Nexus/Path2_4718.csv", + dataDir + "museum/Nexus/Path2_8766.csv", + dataDir + "museum/Pixel/Path2_4336.csv", + dataDir + "museum/Pixel/Path2_4341.csv", + dataDir + "museum/Pixel/Path2_9038.csv", + //dataDir + "museum/Samsung/Path2_3326.csv", + //dataDir + "museum/Samsung/Path2_5814.csv" + }, + + dataDir + "museum/Nexus/fingerprints/wifi_fp.dat", + dataDir + "museum/wifimodel.dat", + 44 + }; + + DataSetup Path3 = { + + mapDir + "map42_ap_path3.xml", + + { + dataDir + "museum/Nexus/Path3_4519.csv", + dataDir + "museum/Nexus/Path3_5929.csv", + dataDir + "museum/Pixel/Path3_6307.csv", + dataDir + "museum/Pixel/Path3_5451.csv", + dataDir + "museum/Pixel/Path3_9243.csv", + //dataDir + "museum/Samsung/Path3_7610.csv", + //dataDir + "museum/Samsung/Path3_7819.csv" + }, + + dataDir + "museum/Nexus/fingerprints/wifi_fp.dat", + dataDir + "museum/wifimodel.dat", + 45 + }; + + } data; + +} + + + + +#endif // SETTINGS_H diff --git a/code/main.cpp b/code/main.cpp new file mode 100755 index 0000000..75ee148 --- /dev/null +++ b/code/main.cpp @@ -0,0 +1,309 @@ +#include "main.h" + +#include "navMesh/mesh.h" +#include "navMesh/filter.h" +#include "Settings.h" +#include "navMesh/meshPlotter.h" +#include "Plotty.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +Stats::Statistics run(Settings::DataSetup setup, int 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; + + //error file + const long int t = static_cast(time(NULL)); + const std::string evalDir = Settings::errorDir + folder; + struct stat statStruct; + stat(evalDir.c_str(), &statStruct); + if(!S_ISDIR(statStruct.st_mode)){ + if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){ + Assert::doThrow("Eval folder couldn't be created!"); + } + } + std::ofstream errorFile; + errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + + // wifi + WiFiModelLogDistCeiling WiFiModel(map); + + // with optimization + if(Settings::WiFiModel::optimize){ + + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!"); + 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); + WiFiModel.addAP(ap.mac, entry); + } + + WiFiModel.saveXML(setup.wifiModel); + } else { + WiFiModel.loadXML(setup.wifiModel); + } + + } else { + // without optimization + 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); + + const Point3 srcPath0(26, 43, 7.5); + const Point3 srcPath1(62, 38, 1.7); + const Point3 srcPath2(62, 38, 1.8); + const Point3 srcPath3(62, 38, 1.8); + + // 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 = 5000; + //auto init = std::make_unique(&mesh, srcPath1); // known position + auto init = std::make_unique(&mesh); // uniform distribution + auto eval = std::make_unique(WiFiModel); + auto trans = std::make_unique(mesh); + + //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.85); + + // sensors + MyControl ctrl; + MyObservation obs; + + StepDetection sd; + PoseDetection pd; + TurnDetection td(&pd); + RelativePressure relBaro; + ActivityDetector act; + relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + Timestamp lastTimestamp = Timestamp::fromMS(0); + + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) { + + const Timestamp ts = Timestamp::fromMS(e.ts); + + if (e.type == Offline::Sensor::WIFI) { + obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; + + } else if (e.type == Offline::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { + ++ctrl.numStepsSinceLastEval; + } + const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; + pd.addAccelerometer(ts, _acc.data); + + //simpleActivity walking / standing + act.add(ts, fr.getAccelerometer()[e.idx].data); + + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[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()[e.idx].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + + //simpleActivity stairs up / down + act.add(ts, fr.getBarometer()[e.idx].data); + obs.activity = act.get(); + } + + if (ctrl.numStepsSinceLastEval > 0) { + + obs.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.1); + lastTimestamp = ts; + + + + //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(); + + plot.showParticles(pf.getParticles()); + plot.setCurEst(est.pos.pos); + plot.setGroundTruth(gtPos); + plot.addEstimationNode(est.pos.pos); + plot.plot(); + + // error calc + float err_m = gtPos.getDistance(est.pos.pos); + errorStats.add(err_m); + errorFile << ts.ms() << " " << err_m << "\n"; + + //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png"); + //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); + + if(ts.ms() == 13410 || ts.ms() == 20802){ + std::ofstream plotFile; + plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); + } + } + } + + + + // 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(); + + //save the .gp buffer into a file +// std::ofstream plotFile; +// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp"); +// dbg.saveToFile(plotFile); +// plotFile.close(); + + std::ofstream plotFile; + plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_" + std::to_string(obs.currentTime.ms()) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); + + //save also a png image, just for a better overview +// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t)); + + return errorStats; +} + +int main(int argc, char** argv) { + + Stats::Statistics statsAVG; + Stats::Statistics statsMedian; + Stats::Statistics statsSTD; + Stats::Statistics statsQuantil; + Stats::Statistics tmp; + + Settings::DataSetup set = Settings::data.Path0; + std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper"; + + for(int i = 0; i < 1; ++i){ + + for(int j = 0; j < 1; ++j){ + tmp = run(set, 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/code/main.h b/code/main.h new file mode 100644 index 0000000..e62a9c5 --- /dev/null +++ b/code/main.h @@ -0,0 +1,6 @@ +#ifndef NAV_MESH_MAIN_H +#define NAV_MESH_MAIN_H + + + +#endif diff --git a/code/navMesh/filter.h b/code/navMesh/filter.h new file mode 100644 index 0000000..f7b65fe --- /dev/null +++ b/code/navMesh/filter.h @@ -0,0 +1,280 @@ +#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 + +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 double val) { + pos.tria = nullptr; // impossible + pos.pos /= val; + return *this; + } + + MyState operator * (const double 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 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; + } + + +}; + +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 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 = 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 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 = dHead.draw(); + p.weight = 1.0 / particles.size(); + } + } + +}; + + +class MyPFTrans : public SMC::ParticleFilterTransition { + + using MyNavMeshWalk = NM::NavMeshWalkSimple; + MyNavMeshWalk walker; + +public: + + MyPFTrans(MyNavMesh& mesh) : walker(mesh) { + + // 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 { + + Distribution::Normal dStepSizeFloor(0.70, 0.1); + Distribution::Normal dStepSizeStair(0.35, 0.1); + Distribution::Normal dHeading(0.0, 0.1); + + #pragma omp parallel for num_threads(3) + for (int 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.1 || params.stepSizes.stepSizeStair_m < 0.1){ + params.stepSizes.stepSizeFloor_m = 0.1; + params.stepSizes.stepSizeStair_m = 0.1; + } + + // 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 + double getStairProb(const SMC::Particle& p, const Activity act) { + + const float kappa = 0.75; + + switch (act) { + + case Activity::WALKING: + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + {return 1-kappa;} + + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} + {return 1-kappa;} + } + return 1.0; + } + +public: + + MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} + + 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 (int i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; + + double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); + double pStair = getStairProb(p, observation.activity); + + //HACK HACK HACK HACK + double prob = 1.0; + if(observation.currentTime.ms() > 20801){ + prob = pWifi * pStair; + } + + p.weight *= prob; + + #pragma omp atomic + sum += prob; + } + + return sum; + + } + +}; + + +using MyFilter = SMC::ParticleFilter; + + +#endif diff --git a/code/navMesh/mesh.h b/code/navMesh/mesh.h new file mode 100644 index 0000000..be6447e --- /dev/null +++ b/code/navMesh/mesh.h @@ -0,0 +1,32 @@ +#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/code/navMesh/meshPlotter.h b/code/navMesh/meshPlotter.h new file mode 100644 index 0000000..2b37838 --- /dev/null +++ b/code/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