diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index fd50715..f194729 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -4,18 +4,23 @@ #include "../math/Interpolator.h" #include "../floorplan/v2/Floorplan.h" #include "../floorplan/v2/FloorplanHelper.h" +#include "../floorplan/v2/FloorplanHelper.h" /** allows interpolation along a synthetic path */ class SyntheticPath : private Interpolator { using Base = Interpolator; using Entry = Base::InterpolatorEntry; + const Floorplan::IndoorMap* map; + public: /** create path using the given ground-truth points from the map */ void create(const Floorplan::IndoorMap* map, std::vector ids) { + this->map = map; + // get all ground-truth points from the map auto gtps = FloorplanHelper::getGroundTruthPoints(map); float dist = 0; @@ -38,6 +43,8 @@ public: return Base::getEntries(); } + + /** smooth harsh angles */ void smooth(float delta = 1, int numRuns = 1) { @@ -104,6 +111,16 @@ public: return Base::get(distance); } + /** is the given position part of a floor (or in the air) */ + bool isOnFloor(const float distance) const { + const Point3 pos = getPosAfterDistance(distance); + for (const Floorplan::Floor* floor : map->floors) { + const float delta = std::abs(floor->atHeight - pos.z); + if (delta < 0.1) {return true;} + } + return false; + } + }; #endif // INDOOR_SYNTEHTICPATH_H diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index 7bfb885..ee0ad51 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -25,8 +25,11 @@ private: /** the walker to listen to */ SyntheticWalker* walker; - /** the pedestrian's step-size (in meter) */ - float stepSize_m = 0.7; + ///** the pedestrian's step-size (in meter) */ + //float stepSize_m = 0; + + ///** when walking stairs, the step size is much smaller */ + //float stepSizeStair_m = 0; float lastStepAtDistance = 0; @@ -46,13 +49,14 @@ private: //float stepSizeSigma_m; float noiseLevel; Distribution::Normal dNextStep; + Distribution::Normal dNextStepStair; public: /** ctor with the walker to follow */ - SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : + SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.3, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : //stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m), - noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m) { + noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) { walker->addListener(this); dX.setSeed(1); @@ -87,10 +91,11 @@ public: protected: - void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override { (void) curPos; - const float nextStepAt = lastStepAtDistance + dNextStep.draw(); + const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw()); + const float nextStepAt = lastStepAtDistance + distAdd; // 1st, start with random noise on the accelerometer const float x = dX.draw(); diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index d9d8ef8..ba8beb0 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -85,7 +85,7 @@ protected: return val; } - void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override { // time sine last onWalk(); if (lastTs.isZero()) {lastTs = walkedTime; return;} diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index 2bb5c1e..0e290c8 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -8,9 +8,14 @@ class SyntheticWalker { public: + enum class Type { + FLOOR, + NON_FLOOR, + }; + class Listener { public: - virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0; + virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos, const Type type) = 0; }; private: @@ -55,11 +60,13 @@ public: // get the current position along the path const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); + const bool isOnFloor = path.isOnFloor(this->walkedDistance); + const Type type = (isOnFloor) ? (Type::FLOOR) : (Type::NON_FLOOR); 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);} + for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath, type);} return curPosOnPath;