475 lines
14 KiB
C++
475 lines
14 KiB
C++
#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
|