diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h index 29d025c..ec5d73b 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/grid/walk/v3/GridWalker3.h @@ -26,12 +26,19 @@ private: public: + /** paremters for the walk */ struct WalkParams { Point3 start; float distance_m; Heading heading = Heading(0); }; + /** result of the random walk */ + struct WalkResult { + Point3 position; + Heading heading = Heading(0); + }; + using Helper = GridWalk3Helper; using Walk = typename GridWalk3Helper::Walk3; @@ -48,7 +55,7 @@ public: } /** perform the walk based on the configured setup */ - const Point3 getDestination(Grid& grid, const WalkParams& params) { + const WalkResult getDestination(Grid& grid, const WalkParams& params) { //return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m); return _drawThenCheck(grid, params); @@ -62,6 +69,7 @@ public: // throw "error"; // } + /** does the given grid-node contain the provided point-in-question? */ const bool contains(const Grid& grid, const Node* n, Point2 pt) { const float gridSize_m = grid.getGridSize_cm() / 100.0f; const float d = gridSize_m / 2.0f; @@ -71,7 +79,7 @@ public: return bbox.contains(pt); } - const Point3 _drawThenCheck(Grid& grid, const WalkParams& params) { + const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { const GridPoint gpStart = p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); @@ -84,13 +92,15 @@ public: const float range_m = params.distance_m + secBuffer_m; const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m); + WalkResult res; + res.heading = params.heading; + res.position = params.start; float realDist_m = params.distance_m; - Heading realHead = params.heading;// + dHead.draw(); int cnt = 0; while(true) { - const Point2 dir = realHead.asVector(); + const Point2 dir = res.heading.asVector(); const Point2 dst = params.start.xy() + (dir * realDist_m); // is dst reachable? @@ -101,7 +111,9 @@ public: const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); const GridPoint gp = p3ToGp(p3); if (grid.hasNodeFor(gp)) { - return p3; + res.position = p3; // new position + res.heading; // keep as-is + return res; } else { std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl; } @@ -111,11 +123,16 @@ public: // before trying again, modify distance and angle if (1 == 0) { realDist_m *= dDist.draw(); - realHead += dHead.draw(); + res.heading += dHead.draw(); } // reached max retries? - if (++cnt > 10) {return params.start;} // did not work out.... + if (++cnt > 10) { + WalkResult res; + res.position = params.start; + res.heading = params.heading; + return res; + } // did not work out.... } diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 53fe215..b7eed18 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -38,11 +38,31 @@ private: const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs) const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing! + + #ifdef WITH_DEBUG_PLOT + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines lineDet; + K::GnuplotPlotElementLines lineX; + K::GnuplotPlotElementLines lineY; + K::GnuplotPlotElementLines lineZ; + Timestamp plotRef; + int plotCnt = 0; + #endif + + public: /** ctor */ StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { - ; + + #ifdef WITH_DEBUG_PLOT + plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); + plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); + plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); + plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000"); + #endif + } /** does the given data indicate a step? */ @@ -73,22 +93,25 @@ public: step = true; } -// static K::Gnuplot gp; -// static K::GnuplotPlot plot; -// static K::GnuplotPlotElementLines lines1; plot.add(&lines1); -// static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff"); -// static Timestamp ref = ts; -// static int i = 0; + #ifdef WITH_DEBUG_PLOT -// //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); -// lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) ); + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); -// if (++i % 100 == 0) { -// gp.draw(plot); -// gp.flush(); -// usleep(1000*25); -// } + //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); + lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) ); + lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) ); + lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) ); + lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) ); + + if (++plotCnt % 25 == 0) { + gp.draw(plot); + gp.flush(); + //usleep(1000*25); + } + + #endif return step; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h new file mode 100644 index 0000000..fd50715 --- /dev/null +++ b/synthetic/SyntheticPath.h @@ -0,0 +1,109 @@ +#ifndef INDOOR_SYNTHETICPATH_H +#define INDOOR_SYNTHETICPATH_H + +#include "../math/Interpolator.h" +#include "../floorplan/v2/Floorplan.h" +#include "../floorplan/v2/FloorplanHelper.h" + +/** allows interpolation along a synthetic path */ +class SyntheticPath : private Interpolator { + + using Base = Interpolator; + using Entry = Base::InterpolatorEntry; + +public: + + /** create path using the given ground-truth points from the map */ + void create(const Floorplan::IndoorMap* map, std::vector ids) { + + // get all ground-truth points from the map + auto gtps = FloorplanHelper::getGroundTruthPoints(map); + float dist = 0; + + // create distance-based entries within the interpolator + for (int i = 0; i < ids.size(); ++i) { + const int id = ids[i]; + Point3 gtp = gtps[id]; + if (i >= 1) { + Point3 gtpPrev = gtps[id-1]; + dist += gtpPrev.getDistance(gtp); + } + Base::add(dist, gtp); + } + + } + + /** get all individual entries from the underlying data-structure */ + const std::vector& getEntries() const { + return Base::getEntries(); + } + + /** smooth harsh angles */ + void smooth(float delta = 1, int numRuns = 1) { + + float t = delta*2; + + for (int j = 0; j < numRuns; ++j) { + + t/=2; + + for (int i = 1; i < (int)entries.size()-1; ++i) { + + // the entry to-be-replaced by two others + const Entry& e = entries[i]; + const float key = e.key; + + const Entry e1(key-t, Base::get(key-t)); + const Entry e2(key+t, Base::get(key+t)); + + entries.erase(entries.begin()+i); --i; + ++i; entries.insert(entries.begin()+i, e1); + ++i; entries.insert(entries.begin()+i, e2); + + } + + } + + } + +// /** smooth harsh angles */ +// void smooth(float delta = 1, int numRuns = 1) { + +// float t = delta/numRuns; + +// for (int i = 1; i < (int)entries.size()-1; ++i) { + +// // the entry to-be-replaced by several others +// const Entry& e = entries[i]; +// const float key = e.key; + +// std::vector newEntries; +// for (int x = -numRuns; x <= +numRuns; x+= 2) { +// const float percent = (float)x / (float)numRuns; +// const float keyO = key + percent*t; +// const Point3 pos1 = Base::get(keyO-t*2); +// const Point3 pos2 = Base::get(keyO+t*2); +// const Point3 posO = (pos1+pos2) / 2; +// Entry e(keyO, posO); +// newEntries.push_back(e); +// } + +// entries.erase(entries.begin()+i); --i; + +// for (const Entry& e : newEntries) { +// ++i; +// entries.insert(entries.begin()+i, e); +// } + +// } + +// } + + /** get the position along the path after the given walking distance */ + Point3 getPosAfterDistance(const float distance) const { + return Base::get(distance); + } + +}; + +#endif // INDOOR_SYNTEHTICPATH_H diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h new file mode 100644 index 0000000..ddeae70 --- /dev/null +++ b/synthetic/SyntheticSteps.h @@ -0,0 +1,67 @@ +#ifndef SYNTHETICSTEPS_H +#define SYNTHETICSTEPS_H + +#include "../sensors/imu/AccelerometerData.h" +#include "SyntheticWalker.h" +#include "../math/distribution/Normal.h" + +/** fakes accelerometer-data based on synthetic walking data */ +class SyntheticSteps : SyntheticWalker::Listener { + +public: + + class Listener { + public: + virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0; + }; + +private: + + /** the walker to listen to */ + SyntheticWalker* walker; + + /** the pedestrian's step-size (in meter) */ + float stepSize_m = 0.7; + + float lastStepAtDistance = 0; + + Distribution::Normal dX = Distribution::Normal(0, 1); + Distribution::Normal dY = Distribution::Normal(0, 1); + Distribution::Normal dZ = Distribution::Normal(0, 1); + + std::vector listeners; + +public: + + /** ctor with the walker to follow */ + SyntheticSteps(SyntheticWalker* walker) { + walker->addListener(this); + } + + void addListener(Listener* l) { + this->listeners.push_back(l); + } + +protected: + + + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + + const float nextStepAt = (lastStepAtDistance + stepSize_m); + + const float x = dX.draw(); + const float y = dY.draw(); + const float z = dZ.draw(); + AccelerometerData acc(x, y, z); + + if (walkedDistance > nextStepAt) { + lastStepAtDistance = walkedDistance; + } + + for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);} + + } + +}; + +#endif // SYNTHETICSTEPS_H diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h new file mode 100644 index 0000000..8faa777 --- /dev/null +++ b/synthetic/SyntheticTurns.h @@ -0,0 +1,4 @@ +#ifndef SYNTHETICTURNS_H +#define SYNTHETICTURNS_H + +#endif // SYNTHETICTURNS_H diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h new file mode 100644 index 0000000..a641871 --- /dev/null +++ b/synthetic/SyntheticWalker.h @@ -0,0 +1,70 @@ +#ifndef SYNTHETICWALKER_H +#define SYNTHETICWALKER_H + +#include "SyntheticPath.h" + +/** walk along a path using a known walking speed */ +class SyntheticWalker { + +public: + + class Listener { + public: + virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0; + }; + +private: + + /** the path to walk along */ + SyntheticPath path; + + /** walking-speed in meter per sec */ + float walkSpeed_meterPerSec = 1.2; + + /** adjusted while walking */ + float walkedDistance = 0; + + /** adjusted while walking */ + Timestamp walkedTime; + + /** the listener to inform */ + std::vector listeners; + + const char* name = "SynWalker"; + +public: + + /** ctor */ + SyntheticWalker(SyntheticPath path) : path(path) { + ; + } + + /** attach a new listener */ + void addListener(Listener* l) { + this->listeners.push_back(l); + } + + /** increment the walk */ + void tick(const Timestamp timePassed) { + + // update time + this->walkedTime += timePassed; + + // update the walked distance using the walking speed + this->walkedDistance += walkSpeed_meterPerSec * timePassed.sec(); + + // get the current position along the path + const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); + + Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() ); + + // inform listener + for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);} + + } + + + +}; + +#endif // SYNTHETICWALKER_H