466 lines
13 KiB
C++
466 lines
13 KiB
C++
#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
|