FHWS/DSem1
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added walky

Browse Source
This commit is contained in:
k-a-z-u
2018-06-05 16:55:45 +02:00
parent 4563391938
commit 267aed29b0
9 changed files with 1695 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
walky/CMakeLists.txt Normal file
View File

@@ -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})

465
walky/ParticleWalk.h Normal file
View File

@@ -0,0 +1,465 @@
#ifndef PARTICLEWALK_H
#define PARTICLEWALK_H
#include "file.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/grid/factory/v3/GridFactory3.h>
#include <Indoor/grid/walk/v3/Walker.h>
#include <Indoor/grid/walk/GridWalkState.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/FilePlayer.h>
#include <Indoor/math/Interpolator.h>
#include <Indoor/synthetic/SyntheticWalker.h>
#include <Indoor/synthetic/SyntheticSteps.h>
#include <Indoor/synthetic/SyntheticTurns.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/statistics/Statistics.h>
#include "SlimParticleFilter.h"
#include <thread>
#include "WalkViz.h"
#include "WalkViz3.h"
#include "ProbViz.h"
//template<typename T, typename... Args>
//std::unique_ptr<T> make_unique(Args&&... args) {
// return std::unique_ptr<T>(new T(std::forward<Args>(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<MyState> {
// Point3 pos;
// MyPfInit(Point3 pos) : pos(pos) {;}
// virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
// for (K::Particle<MyState>& 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<int> ids = {0,1,2,3,4,5,6,7,8,9};
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3b.xml")); std::vector<int> 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<int> 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<int> 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<MyGridNode> grid(gridSize_cm);
// GridFactory<MyGridNode> gf(grid);
// gf.build(map);
GridFactory3<MyGridNode> 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<MyGridNode>;
Distribution::Normal<float> dDistPlane(0.70, 0.13); // walker walks straight -> we have to add noise ourselves
Distribution::Normal<float> dDistStair(0.35, 0.13); // walker walks straight -> we have to add noise ourselves
Distribution::Normal<float> dHead(1, 0.09);
#elif (WALK_MODE==2)
using GridWalker = GW3::WalkerWeightedRandom<MyGridNode>;
Distribution::Normal<float> dDistPlane(0.65, 0.001); // walker walks straight -> we have to add noise ourselves
Distribution::Normal<float> dDistStair(0.35, 0.001); // walker walks straight -> we have to add noise ourselves
Distribution::Normal<float> 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<MyGridNode>(grid, stepSizeSigma_m));
walker.addEvaluator(new GW3::WalkEvalHeadingStartEnd<MyGridNode>(turnSigma_rad));
//walker.addEvaluator(new GW3::WalkEvalEndNodeProbability<MyGridNode>(&grid));
std::vector<MyState> 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<MyPfInit>(new MyPfInit(startP3));
//K::ParticleFilter<MyState, MyControl, MyObservation> pf(1000, std::move(pfInit);
auto fInit = [startP3] (SPF::Particle<MyState>& p) {
p.state.pos = startP3;
p.state.head = Heading(0);
p.weight = 1;
};
auto fEval = [] (const SPF::Particle<MyState>& p, const MyObservation& observation) {
return 1.0;
};
MyControl ctrl;
MyObservation obs;
SPF::Filter<MyState, MyControl, MyObservation, false> pf; // OpenMP
pf.initialize(numParticles, fInit);
K::Statistics<float> 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<MyState>& 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<std::chrono::milliseconds>(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<MyState>& 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

474
walky/ParticleWalkNavMesh.h Normal file
View File

@@ -0,0 +1,474 @@
#ifndef PARTICLEWALKNAVMESH_H
#define PARTICLEWALKNAVMESH_H
#include "file.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/navMesh/NavMeshTriangle.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshFactory.h>
#include <Indoor/navMesh/NavMeshDebug.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkRandom.h>
#include <Indoor/navMesh/walk/NavMeshWalkSemiRandom.h>
#include <Indoor/navMesh/walk/NavMeshWalkSemiDirected.h>
#include <Indoor/navMesh/meta/NavMeshDijkstra.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/FilePlayer.h>
#include <Indoor/math/Interpolator.h>
#include <Indoor/synthetic/SyntheticWalker.h>
#include <Indoor/synthetic/SyntheticSteps.h>
#include <Indoor/synthetic/SyntheticTurns.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/statistics/Statistics.h>
#include "SlimParticleFilter.h"
#include <thread>
#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<MyNavMeshTria> 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<int> 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<int> 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<int> 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<int> 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<MyNavMeshTria> mesh;
NM::NavMeshSettings settings;
NM::NavMeshFactory<MyNavMeshTria> 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<MyNavMeshTria>;
// Distribution::Normal<float> dDistPlane(0.70, 0.13); // walker walks straight -> we have to add noise ourselves
// Distribution::Normal<float> dDistStair(0.35, 0.13); // walker walks straight -> we have to add noise ourselves
// Distribution::Normal<float> dHead(1, 0.09);
Distribution::Normal<float> dDistPlane(0.70, 0.09); // walker walks straight -> we have to add noise ourselves
Distribution::Normal<float> dDistStair(0.35, 0.09); // walker walks straight -> we have to add noise ourselves
//Distribution::Normal<float> dHeadPercent(1, 0.09);
Distribution::Normal<float> dHead(0, 0.08);
#elif (WALK_MODE==2)
using MeshWalker = NM::NavMeshWalkRandom<MyNavMeshTria>;
Distribution::Normal<float> dDistPlane(0.70, 0.0009);
Distribution::Normal<float> dDistStair(0.35, 0.0009);
Distribution::Normal<float> dHead(0, 0.0005);
#elif (WALK_MODE==3)
using MeshWalker = NM::NavMeshWalkSemiRandom<MyNavMeshTria>;
Distribution::Normal<float> dDistPlane(0.70, 0.0009); // sieht schlecht aus mit diesen params!
Distribution::Normal<float> dDistStair(0.35, 0.0009);
Distribution::Normal<float> dHead(0, 0.0005);
#elif (WALK_MODE==4)
using MeshWalker = NM::NavMeshWalkSemiDirected<MyNavMeshTria>;
Distribution::Const<float> dDistPlane(0.65); // no scatter needed
Distribution::Const<float> dDistStair(0.35);
Distribution::Const<float> 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<MyNavMeshTria>(stepSizeSigma_m));
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTria>(turnSigma_rad));
// path
NM::NavMeshDijkstra::stamp(mesh, Point3(-0.3, -0.3, 0.0)); // walkin down the stair
walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTria>());
std::vector<MyState> 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<MyPfInit>(new MyPfInit(startP3));
//K::ParticleFilter<MyState, MyControl, MyObservation> pf(1000, std::move(pfInit);
auto fInit = [startP3,startHead,&mesh] (SPF::Particle<MyState>& p) {
p.state.pos = mesh.getLocation(startP3);
p.state.head = Heading(startHead);
p.weight = 1;
};
auto fEval = [] (const SPF::Particle<MyState>& p, const MyObservation& observation) {
return 1.0;
};
MyControl ctrl;
MyObservation obs;
SPF::Filter<MyState, MyControl, MyObservation, false> pf; // OpenMP
pf.initialize(numParticles, fInit);
K::Statistics<float> 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<MyState>& p, const MyControl& ctrl) {
NM::NavMeshWalkParams<MyNavMeshTria> 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<SPF::Particle<MyState>>& particles, const MyControl& ctrl) {
std::vector<SPF::Particle<MyState>> newParticles;
for (const SPF::Particle<MyState>& _p : particles) {
NM::NavMeshWalkParams<MyNavMeshTria> 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<MyState> 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<MyState>& p1, const SPF::Particle<MyState>& 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<std::chrono::milliseconds>(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

69
walky/ProbViz.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef PROBVIZ_H
#define PROBVIZ_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/math/Interpolator.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementHeatMap.h>
#include <Indoor/math/Distributions.h>
#include <vector>
class ProbViz {
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementHeatMap heat;
public:
ProbViz() {
plot.add(&heat);
}
template <typename T> void show(std::vector<T>& 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<double> 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

212
walky/SlimParticleFilter.h Normal file
View File

@@ -0,0 +1,212 @@
#ifndef SLIMPARTICLEFILTER_H
#define SLIMPARTICLEFILTER_H
#include <functional>
#include <vector>
#include <random>
#include <algorithm>
namespace SPF {
template <typename State> struct Particle {
State state;
double weight;
};
template <typename State, typename Control, typename Observation, bool OMP> class Filter {
using _Particle = Particle<State>;
using FuncInitSingle = std::function<void(_Particle&)>;
using FuncInitAll = std::function<void(std::vector<_Particle>&)>;
using FuncEvalSingle = std::function<double(const _Particle&, const Observation& obs)>;
using FuncTransitionSingle = std::function<void(_Particle&, const Control& ctrl)>;
using FuncTransitionAll = std::function<void(std::vector<_Particle>&, const Control& ctrl)>;
std::vector<Particle<State>> 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 <typename Transition>
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<State>::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<float> 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<State>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
return *it;
}
};
}
#endif // SLIMPARTICLEFILTER_H

93
walky/WalkViz.h Normal file
View File

@@ -0,0 +1,93 @@
#ifndef WALKVIZ_H
#define WALKVIZ_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/math/Interpolator.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
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<Floorplan::FloorObstacleLine*>(obs);
Floorplan::FloorObstacleCircle* circle = dynamic_cast<Floorplan::FloorObstacleCircle*>(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 <typename Node> void addGrid(Grid<Node>& grid) {
for (const Node& n : grid) {
K::GnuplotPoint2 gp(n.x_cm, n.y_cm);
nodes.add(gp);
}
}
void showPath(const Interpolator<float, Point3>& 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

219
walky/WalkViz3.h Normal file
View File

@@ -0,0 +1,219 @@
#ifndef WALKVIZ3_H
#define WALKVIZ3_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/synthetic/SyntheticPath.h>
#include <Indoor/grid/Grid.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
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<Floorplan::FloorObstacleLine*>(obs);
Floorplan::FloorObstacleCircle* circle = dynamic_cast<Floorplan::FloorObstacleCircle*>(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 <typename T> void showParticlesGrid(const std::vector<T>& 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 <typename T> void showParticles(const std::vector<T>& 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<Floorplan::Quad3> 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 <typename Node> void addGrid(Grid<Node>& 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 <typename Node> void addGridWalkImp(Grid<Node>& 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

24
walky/file.h Normal file
View File

@@ -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

18
walky/main.cpp Normal file
View File

@@ -0,0 +1,18 @@
#include <unistd.h>
#include <string>
#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();
}