From d48b0b8fd4b967ed117df0d230b8c27b5831e1ac Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 13 Dec 2017 16:37:51 +0100 Subject: [PATCH] minor changes to grid walking --- grid/Grid.h | 12 ++++++++++-- grid/GridNode.h | 1 + grid/walk/v3/Walker.h | 12 +++++++----- sensors/imu/StepDetection.h | 4 ++-- synthetic/SyntheticPath.h | 14 ++++++-------- synthetic/SyntheticSteps.h | 18 +++++++++++++----- synthetic/SyntheticWalker.h | 4 ++-- 7 files changed, 41 insertions(+), 24 deletions(-) diff --git a/grid/Grid.h b/grid/Grid.h index aa4da03..259cb52 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -213,6 +213,14 @@ public: return GridNodeBBox(node, gridSize_cm); } + /** is this node part of a non-plain stair/escalator */ + bool isPlain(const T& n1) const { + for (const T& n2 : neighbors(n1)) { + if (n2.z_cm != n1.z_cm) {return false;} + } + return true; + } + /** * get an UID for the given point. * this works only for aligned points. @@ -469,11 +477,11 @@ public: - NeighborForEach neighbors(const int idx) { + NeighborForEach neighbors(const int idx) const { return neighbors(nodes[idx]); } - NeighborForEach neighbors(const T& node) { + NeighborForEach neighbors(const T& node) const { return NeighborForEach(*this, node._idx); } diff --git a/grid/GridNode.h b/grid/GridNode.h index 306105c..8b73500 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -87,6 +87,7 @@ public: /** set the node's semantic type */ void setType(const uint8_t type) {this->_type = type;} + // /** get the n-th neighbor for this node */ // template inline T& getNeighbor(const int nth, const Grid& grid) const { // return grid.getNeighbor(_idx, nth); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index c1a29f2..bee1178 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -121,7 +121,7 @@ namespace GW3 { if (grid.hasNodeFor(gp)) { res.position = p3; // update position //res.heading; // keep as-is - res.probability *= getP(p3); // keep as-is + res.probability *= 1;//getP(p3); // keep as-is return res; // done } else { @@ -157,7 +157,7 @@ namespace GW3 { } res.heading = Heading(start.xy(), end.xy()); - res.probability *= getP(end); + res.probability *= 0.1;//getP(end); res.position = end; return res; @@ -237,7 +237,7 @@ namespace GW3 { int numVisitedNodes = 0; - #define MODE 3 + #define MODE 2 #if (MODE == 1) @@ -249,9 +249,10 @@ namespace GW3 { for (const Node* dstNode : nodes) { const Point3 nodeCenter = Helper::gpToP3(*dstNode); + const float walkDist_m = nodeCenter.getDistance(start);//*1.05; double p = 1.0; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, nodeCenter, params); + const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params); p *= p1; } if (p > bestNodeP) { @@ -284,10 +285,11 @@ namespace GW3 { // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); + const float walkDist_m = end.getDistance(start);//*1.05; double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); + const double p1 = eval->getProbability(start, end, walkDist_m, params); p *= p1; } diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 68edfbb..e0e7b74 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -36,7 +36,7 @@ private: Timestamp blockUntil; bool waitForUp = false; - const Timestamp blockTime = Timestamp::fromMS(250); // 150-250 looks good + const Timestamp blockTime; // 150-250 looks good 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! @@ -57,7 +57,7 @@ private: public: /** ctor */ - StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { + StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { #ifdef WITH_DEBUG_PLOT gp << "set autoscale xfix\n"; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index f194729..f173852 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -111,14 +111,12 @@ 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; + /** at the given distance: are we walking on a plain surface or up/down? */ + bool isPlain(const float distance) const { + const Point3 pos1 = getPosAfterDistance(distance); + const Point3 pos2 = getPosAfterDistance(distance + 0.1); + const float delta = std::abs(pos1.z - pos2.z); + return delta < 0.01; } }; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index ee0ad51..a24efd7 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -34,7 +34,8 @@ private: float lastStepAtDistance = 0; Timestamp refStepPattern; - Interpolator stepPattern; + Interpolator stepPatternPlain; + Interpolator stepPatternStair; Distribution::Normal dX = Distribution::Normal(0, 0.2); Distribution::Normal dY = Distribution::Normal(0, 0.3); @@ -76,10 +77,16 @@ public: // AccelerometerData acc(x,y,z); // stepPattern.add(Timestamp::fromMS(i), acc); // } - stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); - stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3)); - stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); - stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); + + stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); + stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3)); + stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); + stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); + + stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); + stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4)); + stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5)); + stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0)); } @@ -95,6 +102,7 @@ protected: (void) curPos; const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw()); + const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair); const float nextStepAt = lastStepAtDistance + distAdd; // 1st, start with random noise on the accelerometer diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index 0e290c8..b4fadfb 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -60,8 +60,8 @@ 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); + const bool isPlainPart = path.isPlain(this->walkedDistance); + const Type type = (isPlainPart) ? (Type::FLOOR) : (Type::NON_FLOOR); Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );