From 267aed29b0ca28641c1608d0d34303e311b7334c Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Tue, 5 Jun 2018 16:55:45 +0200 Subject: [PATCH] added walky --- walky/CMakeLists.txt | 121 +++++++++ walky/ParticleWalk.h | 465 +++++++++++++++++++++++++++++++++++ walky/ParticleWalkNavMesh.h | 474 ++++++++++++++++++++++++++++++++++++ walky/ProbViz.h | 69 ++++++ walky/SlimParticleFilter.h | 212 ++++++++++++++++ walky/WalkViz.h | 93 +++++++ walky/WalkViz3.h | 219 +++++++++++++++++ walky/file.h | 24 ++ walky/main.cpp | 18 ++ 9 files changed, 1695 insertions(+) create mode 100644 walky/CMakeLists.txt create mode 100644 walky/ParticleWalk.h create mode 100644 walky/ParticleWalkNavMesh.h create mode 100644 walky/ProbViz.h create mode 100644 walky/SlimParticleFilter.h create mode 100644 walky/WalkViz.h create mode 100644 walky/WalkViz3.h create mode 100644 walky/file.h create mode 100644 walky/main.cpp diff --git a/walky/CMakeLists.txt b/walky/CMakeLists.txt new file mode 100644 index 0000000..6f6ad97 --- /dev/null +++ b/walky/CMakeLists.txt @@ -0,0 +1,121 @@ +# 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(Walky) + +IF(NOT CMAKE_BUILD_TYPE) + MESSAGE(STATUS "No build type selected. Default to Debug") + SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../ + /apps/paper/diss/code +) + + +FILE(GLOB HEADERS + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h + ./tests/data/* + ./tests/data/*/* + ./tests/data/*/*/* +) + + +FILE(GLOB SOURCES + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ../Indoor/lib/tinyxml/tinyxml2.cpp + ../Indoor/lib/Recast/*.cpp + /apps/paper/diss/code/Indoor/lib/tinyxml/tinyxml2.cpp + /apps/paper/diss/code/Indoor/lib/Recast/*.cpp +) + +FIND_PACKAGE( OpenMP REQUIRED) +if(OPENMP_FOUND) + message("OPENMP FOUND") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +endif() + +if(${CMAKE_GENERATOR} MATCHES "Visual Studio") + + SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") + SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") + + set(CMAKE_CONFIGURATION_TYPES Release Debug) + +else() + +# system specific compiler flags +ADD_DEFINITIONS( + + -std=gnu++11 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + + -g3 + -O0 + + + + -march=native + + -DWITH_TESTS + -DWITH_ASSERTIONS + -DWITH_DEBUG_LOG + + -O2 + -DWITH_DEBUG_PLOT + +) + +endif() + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# wifi scan? +#SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3) +#INCLUDE_DIRECTORIES(/usr/include/libnl3/) +#SET(EXTRA_LIBS ${EXTRA_LIBS} iw) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} + #gtest + pthread + ${EXTRA_LIBS} + #-pg +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/walky/ParticleWalk.h b/walky/ParticleWalk.h new file mode 100644 index 0000000..102505a --- /dev/null +++ b/walky/ParticleWalk.h @@ -0,0 +1,465 @@ +#ifndef PARTICLEWALK_H +#define PARTICLEWALK_H + +#include "file.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 "SlimParticleFilter.h" + +#include + +#include "WalkViz.h" +#include "WalkViz3.h" +#include "ProbViz.h" + + +//template +//std::unique_ptr make_unique(Args&&... args) { +// return std::unique_ptr(new T(std::forward(args)...)); +//} + + +class ParticleWalk { + +public: + + + struct MyWalkState { + GridPoint position; + }; + + struct MyControl { + int steps = 0; + float headChange_rad = 0; + }; + + struct MyState { + + //GridPoint pos; + Point3 pos; + Heading head = Heading(0); + + MyState operator * (const double w) const { + MyState res; res.pos = this->pos*w; return res; + } + MyState& operator += (const MyState& s) { + this->pos += s.pos; + return *this; + } + MyState& operator /= (const double d) { + this->pos /= d; + return *this; + } + + }; + + struct MyObservation { + + }; + +// struct MyPfInit : public K::ParticleFilterInitializer { +// Point3 pos; +// MyPfInit(Point3 pos) : pos(pos) {;} +// virtual void initialize(std::vector>& particles) override { +// for (K::Particle& p : particles) { +// p.state.pos = pos; +// p.state.head = Heading(0); +// p.state.weight = 1; +// } +// } +// }; + + struct MyGridNode : public GridNode, public GridPoint, public GridNodeImportance { + + public: + + MyGridNode(int x, int y, int z) : GridPoint(x,y,z) {;} + + }; + + struct Test : public SyntheticSteps::Listener, public SyntheticTurns::Listener, public Offline::Listener { + + MyControl ctrl; + PoseDetection* poseDetect = new PoseDetection(); + StepDetection* stepDetect = new StepDetection(); // TODO + TurnDetection* turnDetect = new TurnDetection(poseDetect); + + public: + + void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) override { + const bool step = stepDetect->add(ts, acc); + if (step) {++ctrl.steps;} + } + + void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) override { + poseDetect->addAccelerometer(ts, acc); + const float rad = turnDetect->addGyroscope(ts, gyro); + ctrl.headChange_rad += rad; + } + + virtual void onGyroscope(const Timestamp ts, const GyroscopeData gyro) { + const float rad = turnDetect->addGyroscope(ts, gyro); + ctrl.headChange_rad += rad; + } + + virtual void onAccelerometer(const Timestamp ts, const AccelerometerData acc) { + poseDetect->addAccelerometer(ts, acc); + const bool step = stepDetect->add(ts, acc); + if (step) {++ctrl.steps;} + } + + virtual void onGravity(const Timestamp ts, const GravityData data) {}; + virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) {}; + virtual void onBarometer(const Timestamp ts, const BarometerData data) {}; + virtual void onGPS(const Timestamp ts, const GPSData data) {}; + virtual void onCompass(const Timestamp ts, const CompassData data) {}; + virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) {}; + + + void reset() { + ctrl.steps = 0; + ctrl.headChange_rad = 0; + } + + }; + + WalkViz3 viz; + WalkViz3 vizGrid; + + const int gridSize_cm = 50;//22; + const int numParticles = 2500; + + void run() { + + Test test; + + + #define REAL + + #ifdef FAKE + + // load the map and ground-truth + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map2b.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9}; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3b.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3c.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9,10}; + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_stair1.xml")); std::vector ids = {0,1,2,3,4,5,6}; + + // get a walk + SyntheticPath walk1; + walk1.create(map, ids); + walk1.smooth(1,3); + viz.showPath(walk1); + + // fake estimation sensor data + SyntheticWalker synthWalker(walk1); + SyntheticSteps synSteps(&synthWalker, 0.65, 0.35, 0.00005, 0.7); // TODO, why does step-sigma have such a huge impact? + SyntheticTurns synTurns(&synthWalker, 0.01,0.05, 0.4); + + synSteps.addListener(&test); + synTurns.addListener(&test); + + const Point3 startP3 = walk1.getPosAfterDistance(0); + + #endif + + #ifdef REAL + + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml")); + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_stair_down_and_up_again_map_b.xml"));const Point3 startP3 = Point3(0,0,6); + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_free.xml")); const Point3 startP3 = Point3(0,0,0); + + // data source + //Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv")); const Point3 startP3 = Point3(0,0,0); + Offline::FileReader reader(File::getData("/walks/walk_stair_down_and_up_again.csv")); + Offline::FilePlayer player(&reader, &test); + + + + #endif + + viz.addMap(map); + vizGrid.addMap(map); + + // build the grid + Grid grid(gridSize_cm); +// GridFactory gf(grid); +// gf.build(map); + GridFactory3 gf(grid); + gf.build(map); + + // add importance + Importance::addImportance(grid); + + // show + vizGrid.addGridWalkImp(grid, true); // also debug-plot the grid + vizGrid.draw(); + + + + +#define WALK_MODE 1 + +#if (WALK_MODE==1) + + using GridWalker = GW3::WalkerDirectDestination; + Distribution::Normal dDistPlane(0.70, 0.13); // walker walks straight -> we have to add noise ourselves + Distribution::Normal dDistStair(0.35, 0.13); // walker walks straight -> we have to add noise ourselves + Distribution::Normal dHead(1, 0.09); + +#elif (WALK_MODE==2) + + using GridWalker = GW3::WalkerWeightedRandom; + Distribution::Normal dDistPlane(0.65, 0.001); // walker walks straight -> we have to add noise ourselves + Distribution::Normal dDistStair(0.35, 0.001); // walker walks straight -> we have to add noise ourselves + Distribution::Normal dHead(1, 0.00000015); + +#endif + + GridWalker walker(grid); + + // eval + const float stepSizeSigma_m = 0.1; + const float turnSigma_rad = Angle::degToRad(2.5); + walker.addEvaluator(new GW3::WalkEvalDistance(grid, stepSizeSigma_m)); + walker.addEvaluator(new GW3::WalkEvalHeadingStartEnd(turnSigma_rad)); + //walker.addEvaluator(new GW3::WalkEvalEndNodeProbability(&grid)); + + std::vector particles; particles.resize(numParticles); + + // initialize particles + +// //const GridPoint startPoint(startP3.x*100, startP3.y*100, startP3.z*100); +// for (MyState& p : particles) { +// p.pos = startP3; +// } + + + + + int cnt = 0; + + //auto pfInit = make_unique(new MyPfInit(startP3)); + //K::ParticleFilter pf(1000, std::move(pfInit); + + auto fInit = [startP3] (SPF::Particle& p) { + p.state.pos = startP3; + p.state.head = Heading(0); + p.weight = 1; + }; + + auto fEval = [] (const SPF::Particle& p, const MyObservation& observation) { + return 1.0; + }; + + MyControl ctrl; + MyObservation obs; + + SPF::Filter pf; // OpenMP + pf.initialize(numParticles, fInit); + + K::Statistics sss; + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + + + for (int i = 0; i < 300000; ++i) { + + // increment the walk + const Timestamp timePassed = Timestamp::fromMS(10); + + #ifdef FAKE + const Point3 pos = synthWalker.tick(timePassed); + viz.setGT(pos); + #endif + + #ifdef REAL + player.tick(); + #endif + + auto fTrans = [&] (SPF::Particle& p, const MyControl& ctrl) { + + // adjust the heading + p.state.head = p.state.head + test.ctrl.headChange_rad * dHead.draw(); + + //const GridPoint start = p.pos; + const Point3 start = p.state.pos; + + const MyGridNode& nStart = grid.getNodeFor(GridPoint(start.x*100, start.y*100, start.z*100)); + + //const GridPoint dst = walker.getDestination(grid, start, ctrl, pathDistance); + //p.pos = dst; + //K::GnuplotPoint2 gp(p.pos.x_cm, p.pos.y_cm); + + //kein kopieren von distribution erlauben!" das macht nicht was es soll!" + + //const float add_m = (nStart.getType() == GridNode::TYPE_STAIR) ? (dDistStair.draw()) : (dDistPlane.draw()); + //const float add_m = (grid.isPlain(nStart)) ? (dDistPlane.draw()) : (dDistStair.draw()); + + //const float add_m = dDistPlane.draw(); + + //const float walkDistance = test.ctrl.steps * add_m; + + GW3::WalkParams params; + params.stepSizes.stepSizeFloor_m = dDistPlane.draw(); + params.stepSizes.stepSizeStair_m = dDistStair.draw(); + params.start = start; + params.heading = p.state.head; + //params.distance_m = walkDistance; + params.lookFurther_m = 1; + params.numSteps = 1; + + const GW3::WalkResult res = walker.getDestination(params); + p.state.pos = res.position; + p.state.head = res.heading; + p.weight *= res.probability; + K::GnuplotPoint3 gp(p.state.pos.x, p.state.pos.y, p.state.pos.z); + + //viz.particles.add(gp); + //std::cout << node->asString() << std::endl; + + }; + + + + // every step + static int cnt = 0; ++cnt; + + + //++cnt; // 500 msec + //if (cnt % 50 == 0) { + if (test.ctrl.steps != 0) { + //viz.particles.clear(); + + auto t1 = std::chrono::system_clock::now(); + MyState est = pf.update(ctrl, obs, fTrans, fEval); + auto t2 = std::chrono::system_clock::now(); + auto duration = std::chrono::duration_cast(t2-t1); + sss.add(duration.count()); + std::cout << sss.asString() << std::endl; + + viz.pathEstimated.add(K::GnuplotPoint3(est.pos.x, est.pos.y, est.pos.z)); + + viz.showParticles(pf.getParticles()); + test.reset(); + //viz.draw(); + + viz.setCurPos(est.pos); + + + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + } + + if (cnt % 150 == 0) { + static int nr = 0; + viz.plot.getView().setCamera(45, (cnt/70)%360); + viz.plot.getView().setEnabled(true); + viz.draw(); + viz.gp << "unset colorbox\n"; + + //viz.gp.setTerminal("pngcairo", K::GnuplotSize(20,15)); + //viz.gp.setTerminal("pngcairo", K::GnuplotSize(500*3,350*3)); + //viz.gp.setOutput("/tmp/123/" + std::to_string(nr) + ".png"); + ++nr; + } + +// for (const SPF::Particle& p : pf.getParticles()) { +// viz.particles.add(gp); +// } + + } + + +// // walk along the path +// for (int i = 0; i < 30000; ++i) { + +// viz.particles.clear();; + +// // increment the walk +// const Timestamp timePassed = Timestamp::fromMS(10); +// const Point3 pos = synthWalker.tick(timePassed); + +// ++cnt; +// if (cnt % 50 == 0) { + +// for (MyParticle& p : particles) { + +// // adjust the heading +// p.head = p.head + test.ctrl.headChange_rad * dHead.draw(); + +// //const GridPoint start = p.pos; +// const Point3 start = p.pos; + +// //const GridPoint dst = walker.getDestination(grid, start, ctrl, pathDistance); +// //p.pos = dst; +// //K::GnuplotPoint2 gp(p.pos.x_cm, p.pos.y_cm); + +// const float walkDistance = test.ctrl.steps * 0.7 * dDist.draw(); + +// GridWalker::WalkParams params; +// params.start = start; +// params.heading = p.head; +// params.distance_m = walkDistance; + +// const GridWalker::WalkResult res = walker.getDestination(grid, params); +// p.pos = res.position; +// p.head = res.heading; +// p.weight *= res.probability; +// K::GnuplotPoint3 gp(p.pos.x, p.pos.y, p.pos.z); + +// viz.particles.add(gp); +// //std::cout << node->asString() << std::endl; + +// } + +// test.reset(); +// // pViz.show(particles); + +// viz.draw(); + +// } + +// std::this_thread::sleep_for(std::chrono::milliseconds(1)); +// } + + } + +}; + +#endif // PARTICLEWALK_H diff --git a/walky/ParticleWalkNavMesh.h b/walky/ParticleWalkNavMesh.h new file mode 100644 index 0000000..e3c3bc6 --- /dev/null +++ b/walky/ParticleWalkNavMesh.h @@ -0,0 +1,474 @@ +#ifndef PARTICLEWALKNAVMESH_H +#define PARTICLEWALKNAVMESH_H + + +#include "file.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 + +#include + + +#include "SlimParticleFilter.h" + +#include + +#include "WalkViz.h" +#include "WalkViz3.h" +#include "ProbViz.h" + + +class ParticleWalkNavMesh { + +public: + + struct MyNavMeshTria : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra { + + public: + + MyNavMeshTria(Point3 p1, Point3 p2, Point3 p3, uint8_t t) : NM::NavMeshTriangle(p1,p2,p3,t) {;} + + }; + + struct MyWalkState { + GridPoint position; + }; + + struct MyControl { + int steps = 0; + float headChange_rad = 0; + }; + + struct MyState { + + //GridPoint pos; + NM::NavMeshLocation pos; + Heading head = Heading(0); + + MyState operator * (const double w) const { + MyState res; res.pos.pos = this->pos.pos*w; return res; + } + MyState& operator += (const MyState& s) { + this->pos.pos += s.pos.pos; + return *this; + } + MyState& operator /= (const double d) { + this->pos.pos /= d; + return *this; + } + + }; + + struct MyObservation { + + }; + + + struct Test : public SyntheticSteps::Listener, public SyntheticTurns::Listener, public Offline::Listener { + + MyControl ctrl; + PoseDetection* poseDetect = new PoseDetection(); + StepDetection* stepDetect = new StepDetection(); // TODO + TurnDetection* turnDetect = new TurnDetection(poseDetect); + + public: + + void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) override { + const bool step = stepDetect->add(ts, acc); + if (step) {++ctrl.steps;} + } + + void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) override { + poseDetect->addAccelerometer(ts, acc); + const float rad = turnDetect->addGyroscope(ts, gyro); + ctrl.headChange_rad += rad; + } + + virtual void onGyroscope(const Timestamp ts, const GyroscopeData gyro) { + const float rad = turnDetect->addGyroscope(ts, gyro); + ctrl.headChange_rad += rad; + } + + virtual void onAccelerometer(const Timestamp ts, const AccelerometerData acc) { + poseDetect->addAccelerometer(ts, acc); + const bool step = stepDetect->add(ts, acc); + if (step) {++ctrl.steps;} + } + + virtual void onGravity(const Timestamp ts, const GravityData data) {}; + virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) {}; + virtual void onBarometer(const Timestamp ts, const BarometerData data) {}; + virtual void onGPS(const Timestamp ts, const GPSData data) {}; + virtual void onCompass(const Timestamp ts, const CompassData data) {}; + virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) {}; + + + void reset() { + ctrl.steps = 0; + ctrl.headChange_rad = 0; + } + + }; + + //WalkViz3 viz; + NM::NavMeshDebug vizNM; + + const int gridSize_cm = 50;//22; + const int numParticles = 3333; //3333; + + void run() { + + Test test; + + + #define REAL + + #ifdef FAKE + + // load the map and ground-truth + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map2b.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9}; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3b_for_nav_mesh.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3c.xml")); std::vector ids = {0,1,2,3,4,5,6,7,8,9,10}; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_stair1.xml")); std::vector ids = {0,1,2,3,4,5,6}; + + // get a walk + SyntheticPath walk1; + walk1.create(map, ids); + walk1.smooth(1,3); + //viz.showPath(walk1); + + // fake estimation sensor data + SyntheticWalker synthWalker(walk1); + SyntheticSteps synSteps(&synthWalker, 0.70, 0.35, 0.00005, 0.6); // TODO, why does step-sigma have such a huge impact? + SyntheticTurns synTurns(&synthWalker, 0.01,0.05, 0.4); + + synSteps.addListener(&test); + synTurns.addListener(&test); + + const Point3 startP3 = walk1.getPosAfterDistance(0); + + #endif + + #ifdef REAL + + //float startHead = 0; + + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15); + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_stair_down_and_up_again_map_b.xml"));const Point3 startP3 = Point3(0,0,6); + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_free.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15); + +// Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/SHL41_nm.xml")); +// map->floors[0]->enabled = false; map->floors[1]->enabled = false; map->floors[3]->enabled = false; +// Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv")); +// const Point3 startP3 = Point3(61.0, 39.8, 7.4); float startHead = M_PI/2; +// vizNM.plot.getAxisX().setRange(50, 64); +// vizNM.plot.getAxisY().setRange(37, 47); +// vizNM.plot.getAxisZ().setRange(7.3, 7.5); + +// Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15); +// Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv")); //const Point3 startP3 = Point3(0,0,0); +// const float startHead = 0; + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_stair_down_and_up_again_map_b.xml"));const Point3 startP3 = Point3(0,0,6); + Offline::FileReader reader(File::getData("/walks/walk_stair_down_and_up_again.csv")); + const float startHead = 0; + + Offline::FilePlayer player(&reader, &test); + + + + #endif + + + + //viz.addMap(map); + + // build the NavMesh + NM::NavMesh mesh; + NM::NavMeshSettings settings; + NM::NavMeshFactory fac(&mesh, settings); + fac.build(map); + //NM::NavMeshDebug::show(mesh); + + vizNM.addMesh(mesh); + +#define WALK_MODE 4 +//#define WALK_USE_MANY + +#if (WALK_MODE==1) + + using MeshWalker = NM::NavMeshWalkSimple; +// Distribution::Normal dDistPlane(0.70, 0.13); // walker walks straight -> we have to add noise ourselves +// Distribution::Normal dDistStair(0.35, 0.13); // walker walks straight -> we have to add noise ourselves +// Distribution::Normal dHead(1, 0.09); + + Distribution::Normal dDistPlane(0.70, 0.09); // walker walks straight -> we have to add noise ourselves + Distribution::Normal dDistStair(0.35, 0.09); // walker walks straight -> we have to add noise ourselves + //Distribution::Normal dHeadPercent(1, 0.09); + Distribution::Normal dHead(0, 0.08); + +#elif (WALK_MODE==2) + + using MeshWalker = NM::NavMeshWalkRandom; + Distribution::Normal dDistPlane(0.70, 0.0009); + Distribution::Normal dDistStair(0.35, 0.0009); + Distribution::Normal dHead(0, 0.0005); + +#elif (WALK_MODE==3) + + using MeshWalker = NM::NavMeshWalkSemiRandom; + Distribution::Normal dDistPlane(0.70, 0.0009); // sieht schlecht aus mit diesen params! + Distribution::Normal dDistStair(0.35, 0.0009); + Distribution::Normal dHead(0, 0.0005); + +#elif (WALK_MODE==4) + + using MeshWalker = NM::NavMeshWalkSemiDirected; + Distribution::Const dDistPlane(0.65); // no scatter needed + Distribution::Const dDistStair(0.35); + Distribution::Const dHead(0); + +#endif + + + + MeshWalker walker(mesh); + + // eval + const float stepSizeSigma_m = 0.15;//0.1; + const float turnSigma_rad = Angle::degToRad(5.5); //2.5); + walker.addEvaluator(new NM::WalkEvalDistance(stepSizeSigma_m)); + walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(turnSigma_rad)); + + + + + + // path + NM::NavMeshDijkstra::stamp(mesh, Point3(-0.3, -0.3, 0.0)); // walkin down the stair + walker.addEvaluator(new NM::WalkEvalApproachesTarget()); + + + + + + + + std::vector particles; particles.resize(numParticles); + + // initialize particles + +// //const GridPoint startPoint(startP3.x*100, startP3.y*100, startP3.z*100); +// for (MyState& p : particles) { +// p.pos = startP3; +// } + + + + + int cnt = 0; + + //auto pfInit = make_unique(new MyPfInit(startP3)); + //K::ParticleFilter pf(1000, std::move(pfInit); + + auto fInit = [startP3,startHead,&mesh] (SPF::Particle& p) { + p.state.pos = mesh.getLocation(startP3); + p.state.head = Heading(startHead); + p.weight = 1; + }; + + auto fEval = [] (const SPF::Particle& p, const MyObservation& observation) { + return 1.0; + }; + + MyControl ctrl; + MyObservation obs; + + SPF::Filter pf; // OpenMP + pf.initialize(numParticles, fInit); + + K::Statistics sss; + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + + //gewichten basierend auf control und nicht control + distMod(gen)!! + + + for (int i = 0; i < 300000; ++i) { + + // increment the walk + const Timestamp timePassed = Timestamp::fromMS(10); + + #ifdef FAKE + const int updateEvery = 40; + if (!synthWalker.done()) { + const Point3 pos = synthWalker.tick(timePassed); + //viz.setGT(pos); + vizNM.setGT(pos); + } else { + sleep(1000); + } + #endif + + #ifdef REAL + player.tick(); + const int updateEvery = 150; + #endif + + auto fTransOne = [&] (SPF::Particle& p, const MyControl& ctrl) { + + NM::NavMeshWalkParams params; + params.stepSizes.stepSizeFloor_m = dDistPlane.draw(); + params.stepSizes.stepSizeStair_m = dDistStair.draw(); + params.start = p.state.pos; + params.heading = p.state.head + test.ctrl.headChange_rad + dHead.draw(); + params.numSteps = 1; + + const MeshWalker::ResultEntry e = walker.getOne(params); + p.state.pos = e.location; + p.state.head = e.heading; + p.weight *= e.probability; + + }; + + auto fTransMany = [&] (std::vector>& particles, const MyControl& ctrl) { + + std::vector> newParticles; + + for (const SPF::Particle& _p : particles) { + + NM::NavMeshWalkParams params; + params.stepSizes.stepSizeFloor_m = dDistPlane.draw(); + params.stepSizes.stepSizeStair_m = dDistStair.draw(); + params.start = _p.state.pos; + params.heading = _p.state.head + test.ctrl.headChange_rad + dHead.draw(); + params.numSteps = 1; + + const MeshWalker::ResultList res = walker.getMany(params); + for (const MeshWalker::ResultEntry& e : res) { + SPF::Particle p = _p; // particle copy + p.state.pos = e.location; + p.state.head = e.heading; + p.weight *= e.probability; + newParticles.push_back(p); + } + + } + + auto comp = [] (const SPF::Particle& p1, const SPF::Particle& p2) {return p1.weight > p2.weight;}; + std::sort(newParticles.begin(), newParticles.end(), comp); + newParticles.erase(newParticles.begin()+numParticles, newParticles.end()); + + std::swap(particles, newParticles); + + }; + + + + // every step + static int cnt = 0; ++cnt; + + + //++cnt; // 500 msec + //if (cnt % 50 == 0) { + if (test.ctrl.steps != 0) { + //viz.particles.clear(); + + auto t1 = std::chrono::system_clock::now(); + + #ifdef WALK_USE_MANY + MyState est = pf.update(ctrl, obs, fTransMany, fEval); + #else + MyState est = pf.update(ctrl, obs, fTransOne, fEval); + #endif + auto t2 = std::chrono::system_clock::now(); + auto duration = std::chrono::duration_cast(t2-t1); + sss.add(duration.count()); + std::cout << sss.asString() << std::endl; + + //viz.pathEstimated.add(K::GnuplotPoint3(est.pos.pos.x, est.pos.pos.y, est.pos.pos.z)); + vizNM.pathEstimated.add(K::GnuplotPoint3(est.pos.pos.x, est.pos.pos.y, est.pos.pos.z)); + + //viz.showParticles(pf.getParticles()); + test.reset(); + + //viz.setCurPos(est.pos.pos); + + + vizNM.showParticles(pf.getParticles()); + vizNM.setCurPos(est.pos.pos); + + PERF_DUMP(); + + //std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + } + + if (cnt % updateEvery == 0) { + static int nr = 0; + //viz.plot.getView().setCamera(45, (cnt/70)%360); + //viz.plot.getView().setEnabled(true); + //viz.draw(); + //viz.gp << "unset colorbox\n"; + + vizNM.gp << "unset colorbox\n"; + vizNM.plot.getView().setCamera(60, (cnt/70)%360); + vizNM.plot.getView().setEnabled(true); + + //viz.gp.setTerminal("pngcairo", K::GnuplotSize(20,15)); + //viz.gp.setTerminal("pngcairo", K::GnuplotSize(500*3,350*3)); + //viz.gp.setOutput("/tmp/123/" + std::to_string(nr) + ".png"); + + //vizNM.gp.setTerminal("pngcairo", K::GnuplotSize(25,19)); + //vizNM.gp.setOutput("/tmp/ani/" + std::to_string(nr) + ".png"); + // ffmpeg -r 15 -i %d.png -preset slow -crf 22 -pix_fmt yuv420p -b:v 700k -filter:v "crop=496:464:140:40" out.mp4 + + vizNM.draw(); + + + ++nr; + + } + + + } + + + + } + +}; + +#endif // PARTICLEWALKNAVMESH_H diff --git a/walky/ProbViz.h b/walky/ProbViz.h new file mode 100644 index 0000000..4ce44a5 --- /dev/null +++ b/walky/ProbViz.h @@ -0,0 +1,69 @@ +#ifndef PROBVIZ_H +#define PROBVIZ_H + +#include +#include + +#include +#include +#include + +#include +#include + +class ProbViz { + + + K::Gnuplot gp; + K::GnuplotSplot plot; + + K::GnuplotSplotElementHeatMap heat; + +public: + + ProbViz() { + plot.add(&heat); + } + + template void show(std::vector& particles) { + + heat.clear(); + + float x1 = -0; + float x2 = +20; + float y1 = -0; + float y2 = +20; + float s = 0.5; + + plot.getAxisX().setRange(x1,x2); + plot.getAxisY().setRange(y1,y2); + + Distribution::Normal nd(0.0, 1.0); + + for (float y = y1; y < y2; y+=s) { + for (float x = x1; x < x2; x+=s) { + + const Point2 p1(x,y); + double prob = 1; + + for (const T& p : particles) { + const Point2 p2 = p.pos.xy(); + const float dist = p1.getDistance(p2); + prob += nd.getProbability(dist); + } + prob /= particles.size(); + + K::GnuplotPoint3 p3(x,y,prob); + heat.add(p3); + + } + } + + gp.draw(plot); + gp.flush(); + + } + +}; + +#endif // PROBVIZ_H diff --git a/walky/SlimParticleFilter.h b/walky/SlimParticleFilter.h new file mode 100644 index 0000000..3f51729 --- /dev/null +++ b/walky/SlimParticleFilter.h @@ -0,0 +1,212 @@ +#ifndef SLIMPARTICLEFILTER_H +#define SLIMPARTICLEFILTER_H + +#include +#include +#include +#include + +namespace SPF { + + template struct Particle { + State state; + double weight; + }; + + template class Filter { + + using _Particle = Particle; + using FuncInitSingle = std::function; + using FuncInitAll = std::function&)>; + using FuncEvalSingle = std::function; + using FuncTransitionSingle = std::function; + using FuncTransitionAll = std::function&, const Control& ctrl)>; + + std::vector> particles; + + public: + + /** initialize the given number of particles */ + void initialize(const int num, FuncInitAll fInit) { + particles.resize(num); + fInit(particles); + } + + /** initialize the given number of particles */ + void initialize(const int num, FuncInitSingle fInit) { + particles.resize(num); + for (_Particle& p : particles) { + fInit(p); + } + } + + const std::vector<_Particle>& getParticles() const { + return particles; + } + + double lastNEff = 1; + double nEffThresholdPercent = 0.9; + + /** perform resampling -> transition -> evaluation -> estimation */ + template + State update(const Control& control, const Observation& observation, Transition fTrans, FuncEvalSingle fEval) { + + // if the number of efficient particles is too low, perform resampling + if (lastNEff < particles.size() * nEffThresholdPercent) { resample(particles); } + //resample(particles); + + // perform the transition step + transition(fTrans, control); + + // perform the evaluation step + #pragma omp parallel for if(OMP) + for (size_t i = 0; i < particles.size(); ++i) { + particles[i].weight *= fEval(particles[i], observation); + } + + // normalize the particle weights and thereby calculate N_eff + lastNEff = normalize(); + std::cout << "NEff: " << lastNEff << std::endl; + + //std::cout << "normalized. n_eff is " << lastNEff << std::endl; + + // estimate the current state + const State est = estimate(particles); + + // done + return est; + + } + + private: + + /** all particles at once transition */ + void transition(FuncTransitionAll fTrans, const Control& control) { + fTrans(particles, control); + } + + /** single particle at once transition */ + void transition(FuncTransitionSingle fTrans, const Control& control) { + #pragma omp parallel for if(OMP) + for (size_t i = 0; i < particles.size(); ++i) { + fTrans(particles[i], control); + } + } + + State estimate(std::vector<_Particle>& particles) const { + + State tmp; + + // calculate weighted average + double weightSum = 0; + for (const _Particle& p : particles) { + const double weight = p.weight;// * p.weight * p.weight; + tmp += p.state * weight; + weightSum += weight; + } + + _assertTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + _assertTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + return tmp; + + } + + /** normalize the weight of all particles to 1.0 and perform some sanity checks */ + double normalize() { + + // calculate sum(weights) + //double min1 = 9999999; + double weightSum = 0.0; + for (const _Particle& p : particles) { + weightSum += p.weight; + //if (p.weight < min1) {min1 = p.weight;} + } + + // sanity check. always! + if (weightSum != weightSum) { + throw Exception("sum of paticle-weights is NaN"); + } + if (weightSum == 0) { + throw Exception("sum of paticle-weights is 0.0"); + } + + // normalize and calculate N_eff + double sum2 = 0.0; + //double min2 = 9999999; + for (_Particle& p : particles) { + p.weight /= weightSum; + //if (p.weight < min2) {min2 = p.weight;} + sum2 += (p.weight * p.weight); + } + + // N_eff + return 1.0 / sum2; + + } + + + + std::vector<_Particle> particlesCopy; + std::minstd_rand gen; + + void resample(std::vector<_Particle>& particles) { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + //static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // now draw from the copy vector and fill the original one + // with the resampled particle-set + for (uint32_t i = 0; i < cnt; ++i) { + particles[i] = draw(cumWeight); + particles[i].weight = equalWeight; + } + + } + + + /** draw one particle according to its weight from the copy vector */ + const _Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + }; + +} + + +#endif // SLIMPARTICLEFILTER_H diff --git a/walky/WalkViz.h b/walky/WalkViz.h new file mode 100644 index 0000000..dffe201 --- /dev/null +++ b/walky/WalkViz.h @@ -0,0 +1,93 @@ +#ifndef WALKVIZ_H +#define WALKVIZ_H + +#include +#include + +#include +#include +#include +#include + +struct WalkViz { + + K::Gnuplot gp; + K::GnuplotPlot plot; + + K::GnuplotPlotElementPoints nodes; + K::GnuplotPlotElementLines outline; + K::GnuplotPlotElementLines obstacles; + K::GnuplotPlotElementPoints particles; + K::GnuplotPlotElementLines path; + + WalkViz() { + plot.add(&obstacles); + plot.add(&outline); + plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(1); nodes.getColor().setHexStr("#999999"); + plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.1); + plot.add(&path); path.getStroke().setWidth(2); + gp << "set size ratio -1\n"; + } + + void add(Floorplan::FloorOutlinePolygon* poly) { + Floorplan::Polygon2 pol2 = poly->poly; + const int num = pol2.points.size(); + for (int i = 0; i < num; ++i) { + const Point2 pt1 = pol2.points[(i+0)]; + const Point2 pt2 = pol2.points[(i+1)%num]; + K::GnuplotPoint2 gp1(pt1.x*100, pt1.y*100); + K::GnuplotPoint2 gp2(pt2.x*100, pt2.y*100); + outline.addSegment(gp1, gp2); + } + } + + void addObstacle(Floorplan::FloorObstacle* obs) { + + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + Floorplan::FloorObstacleCircle* circle = dynamic_cast(obs); + + if (line) { + K::GnuplotPoint2 gp1(line->from.x*100, line->from.y*100); + K::GnuplotPoint2 gp2(line->to.x*100, line->to.y*100); + obstacles.addSegment(gp1,gp2); + } + + } + + void addFloor(Floorplan::Floor* floor) { + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + addObstacle(obs); + } + } + + void addMap(Floorplan::IndoorMap* map) { + for (Floorplan::Floor* floor : map->floors) { + addFloor(floor); + } + } + + template void addGrid(Grid& grid) { + for (const Node& n : grid) { + K::GnuplotPoint2 gp(n.x_cm, n.y_cm); + nodes.add(gp); + } + } + + void showPath(const Interpolator& interpol) { + for (const auto& entry : interpol.getEntries()) { + const Point3 p = entry.value; + K::GnuplotPoint2 gp(p.x * 100 , p.y*100); + path.add(gp); + } + } + + void draw() { + gp.draw(plot); + gp.flush(); + } + + +}; + + +#endif // WALKVIZ_H diff --git a/walky/WalkViz3.h b/walky/WalkViz3.h new file mode 100644 index 0000000..62b0bf8 --- /dev/null +++ b/walky/WalkViz3.h @@ -0,0 +1,219 @@ +#ifndef WALKVIZ3_H +#define WALKVIZ3_H + + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +struct WalkViz3 { + + K::Gnuplot gp; + K::GnuplotSplot plot; + + K::GnuplotSplotElementPoints nodes; + K::GnuplotSplotElementLines edges; + + K::GnuplotSplotElementColorPoints nodesCol; + + K::GnuplotSplotElementLines outline; + K::GnuplotSplotElementLines obstacles; + K::GnuplotSplotElementColorPoints particles; + K::GnuplotSplotElementLines path; + K::GnuplotSplotElementLines pathEstimated; + K::GnuplotObjectPolygon oPos; + + WalkViz3() { + plot.add(&obstacles); + plot.add(&outline); + + plot.add(&edges); edges.getStroke().getColor().setHexStr("#ff0000"); + plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(0.4); nodes.getColor().setHexStr("#999999"); + + plot.add(&nodesCol); nodesCol.setPointType(7); nodesCol.setPointSize(0.4); + + plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2); + plot.add(&path); path.getStroke().setWidth(2); path.setShowPoints(true); + plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00"); + + plot.getObjects().add(&oPos); + + //gp << "set size ratio -1\n"; + //gp << "set view equal xy\n"; + //gp << "set view equal xyz\n"; + + plot.getView().setEnabled(false); + } + + void add(Floorplan::Floor* floor, Floorplan::FloorOutlinePolygon* poly) { + Floorplan::Polygon2 pol2 = poly->poly; + const int num = pol2.points.size(); + for (int i = 0; i < num; ++i) { + const Point2 pt1 = pol2.points[(i+0)]; + const Point2 pt2 = pol2.points[(i+1)%num]; + K::GnuplotPoint3 gp1(pt1.x, pt1.y, floor->atHeight); + K::GnuplotPoint3 gp2(pt2.x, pt2.y, floor->atHeight); + outline.addSegment(gp1, gp2); + } + } + + void addObstacle(Floorplan::Floor* floor, Floorplan::FloorObstacle* obs) { + + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + Floorplan::FloorObstacleCircle* circle = dynamic_cast(obs); + + if (line) { + K::GnuplotPoint3 gp1(line->from.x, line->from.y, floor->atHeight); + K::GnuplotPoint3 gp2(line->to.x, line->to.y, floor->atHeight); + obstacles.addSegment(gp1,gp2); + } + + } + + void showPos(const Point3 pos) { + oPos.clear(); + oPos.add( K::GnuplotCoordinate3(pos.x-1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) ); + oPos.add( K::GnuplotCoordinate3(pos.x+1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) ); + oPos.add( K::GnuplotCoordinate3(pos.x+0,pos.y+1,pos.z, K::GnuplotCoordinateSystem::FIRST) ); + oPos.close(); + } + + template void showParticlesGrid(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.x, p.state.pos.y, p.state.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 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.x, p.state.pos.y, p.state.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); + } + + void addFloor(Floorplan::Floor* floor) { + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + addObstacle(floor, obs); + } + for (Floorplan::Stair* s : floor->stairs) { + addStair(floor, s); + } + } + + void addStair(Floorplan::Floor* floor, Floorplan::Stair* s) { + + std::vector quads = Floorplan::getQuads(s->getParts(), floor); + for (const Floorplan::Quad3& quad : quads) { + K::GnuplotPoint3 gp1(quad.p1.x, quad.p1.y, quad.p1.z); + K::GnuplotPoint3 gp2(quad.p2.x, quad.p2.y, quad.p2.z); + K::GnuplotPoint3 gp3(quad.p3.x, quad.p3.y, quad.p3.z); + K::GnuplotPoint3 gp4(quad.p4.x, quad.p4.y, quad.p4.z); + obstacles.addSegment(gp1,gp2); + obstacles.addSegment(gp2,gp3); + obstacles.addSegment(gp3,gp4); + obstacles.addSegment(gp4,gp1); + } + + } + + void addMap(Floorplan::IndoorMap* map) { + for (Floorplan::Floor* floor : map->floors) { + addFloor(floor); + } + } + + void setGT(const Point3 pt) { + gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.7) << " 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+1.7) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n"; + } + + + template void addGrid(Grid& grid, bool addEdges = false) { + nodes.clear(); + edges.clear(); + for (const Node& n : grid) { + const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f); + nodes.add(gp); + } + if (addEdges) { + for (const Node& n : grid) { + for (const Node& n2 : grid.neighbors(n)) { + if (n.getIdx() < n2.getIdx()) { // uni-directional.. prevent duplicates + edges.addSegment( + K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f, + K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f + ); + } + } + } + } + } + + template void addGridWalkImp(Grid& grid, bool addEdges = false) { + double pMin = +999; + double pMax = -999; + for (const Node& n : grid) { + const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f); + const double p = n.getWalkImportance(); + nodesCol.add(gp, p); + if (p > pMax) {pMax = p;} + if (p < pMin) {pMin = p;} + } + if (addEdges) { + for (const Node& n : grid) { + for (const Node& n2 : grid.neighbors(n)) { + if (n.getIdx() < n2.getIdx()) { // uni-directional.. prevent duplicates + edges.addSegment( + K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f, + K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f + ); + } + } + } + } + plot.getAxisCB().setRange(pMin, pMax); + } + + void showPath(const SyntheticPath& interpol) { + for (const auto& entry : interpol.getEntries()) { + const Point3 p = entry.value; + K::GnuplotPoint3 gp(p.x, p.y, p.z); + path.add(gp); + } + } + + void draw() { + gp.draw(plot); + gp.flush(); + } + + +}; + + + +#endif // WALKVIZ3_H diff --git a/walky/file.h b/walky/file.h new file mode 100644 index 0000000..12bed5b --- /dev/null +++ b/walky/file.h @@ -0,0 +1,24 @@ +#ifndef FILE_H +#define FILE_H + +#include "string" + +namespace File { + + const std::string basePath = "/apps/paper/diss/code/walkModel/"; + //const std::string basePath = "/mnt/vm/paper/diss/code/walkModel/"; + + const std::string dataPath = "/apps/paper/diss/data/"; + //const std::string dataPath = "/mnt/vm/paper/diss/data"; + + static inline std::string get(const std::string& name) { + return basePath + name; + } + + static inline std::string getData(const std::string& name) { + return dataPath + name; + } + +} + +#endif // FILE_H diff --git a/walky/main.cpp b/walky/main.cpp new file mode 100644 index 0000000..aba47bc --- /dev/null +++ b/walky/main.cpp @@ -0,0 +1,18 @@ + +#include +#include + + +#include "file.h" + +//#include "ParticleWalk.h" +#include "ParticleWalkNavMesh.h" + +int main(void) { + + // let gnuplot write outputs to here + chdir("/tmp/gp"); + + ParticleWalkNavMesh walk; walk.run(); + +}