FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

minor changes to grid walking

Browse Source
This commit is contained in:
k-a-z-u
2017-12-13 16:37:51 +01:00
parent 1114331fd2
commit d48b0b8fd4
7 changed files with 41 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
grid/Grid.h
View File

@@ -213,6 +213,14 @@ public:
return GridNodeBBox(node, gridSize_cm); 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. * get an UID for the given point.
* this works only for aligned points. * 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]); return neighbors(nodes[idx]);
} }
NeighborForEach neighbors(const T& node) { NeighborForEach neighbors(const T& node) const {
return NeighborForEach(*this, node._idx); return NeighborForEach(*this, node._idx);
} }

1
grid/GridNode.h
View File

@@ -87,6 +87,7 @@ public:
/** set the node's semantic type */ /** set the node's semantic type */
void setType(const uint8_t type) {this->_type = type;} void setType(const uint8_t type) {this->_type = type;}
// /** get the n-th neighbor for this node */ // /** get the n-th neighbor for this node */
// template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const { // template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
// return grid.getNeighbor(_idx, nth); // return grid.getNeighbor(_idx, nth);

12
grid/walk/v3/Walker.h
View File

@@ -121,7 +121,7 @@ namespace GW3 {
if (grid.hasNodeFor(gp)) { if (grid.hasNodeFor(gp)) {
res.position = p3; // update position res.position = p3; // update position
//res.heading; // keep as-is //res.heading; // keep as-is
res.probability *= getP(p3); // keep as-is res.probability *= 1;//getP(p3); // keep as-is
return res; // done return res; // done
} else { } else {
@@ -157,7 +157,7 @@ namespace GW3 {
} }
res.heading = Heading(start.xy(), end.xy()); res.heading = Heading(start.xy(), end.xy());
res.probability *= getP(end); res.probability *= 0.1;//getP(end);
res.position = end; res.position = end;
return res; return res;
@@ -237,7 +237,7 @@ namespace GW3 {
int numVisitedNodes = 0; int numVisitedNodes = 0;
#define MODE 3 #define MODE 2
#if (MODE == 1) #if (MODE == 1)
@@ -249,9 +249,10 @@ namespace GW3 {
for (const Node* dstNode : nodes) { for (const Node* dstNode : nodes) {
const Point3 nodeCenter = Helper::gpToP3(*dstNode); const Point3 nodeCenter = Helper::gpToP3(*dstNode);
const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
double p = 1.0; double p = 1.0;
for (const WalkEvaluator<Node>* eval : evals) { for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, nodeCenter, params); const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
p *= p1; p *= p1;
} }
if (p > bestNodeP) { if (p > bestNodeP) {
@@ -284,10 +285,11 @@ namespace GW3 {
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) // 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 Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
const float walkDist_m = end.getDistance(start);//*1.05;
double p = 1; double p = 1;
for (const WalkEvaluator<Node>* eval : evals) { for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params); const double p1 = eval->getProbability(start, end, walkDist_m, params);
p *= p1; p *= p1;
} }

4
sensors/imu/StepDetection.h
View File

@@ -36,7 +36,7 @@ private:
Timestamp blockUntil; Timestamp blockUntil;
bool waitForUp = false; 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 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! const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing!
@@ -57,7 +57,7 @@ private:
public: public:
/** ctor */ /** 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 #ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n"; gp << "set autoscale xfix\n";

14
synthetic/SyntheticPath.h
View File

@@ -111,14 +111,12 @@ public:
return Base::get(distance); return Base::get(distance);
} }
/** is the given position part of a floor (or in the air) */ /** at the given distance: are we walking on a plain surface or up/down? */
bool isOnFloor(const float distance) const { bool isPlain(const float distance) const {
const Point3 pos = getPosAfterDistance(distance); const Point3 pos1 = getPosAfterDistance(distance);
for (const Floorplan::Floor* floor : map->floors) { const Point3 pos2 = getPosAfterDistance(distance + 0.1);
const float delta = std::abs(floor->atHeight - pos.z); const float delta = std::abs(pos1.z - pos2.z);
if (delta < 0.1) {return true;} return delta < 0.01;
}
return false;
} }
}; };

18
synthetic/SyntheticSteps.h
View File

@@ -34,7 +34,8 @@ private:
float lastStepAtDistance = 0; float lastStepAtDistance = 0;
Timestamp refStepPattern; Timestamp refStepPattern;
Interpolator<Timestamp, AccelerometerData> stepPattern; Interpolator<Timestamp, AccelerometerData> stepPatternPlain;
Interpolator<Timestamp, AccelerometerData> stepPatternStair;
Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2); Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3); Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
@@ -76,10 +77,16 @@ public:
// AccelerometerData acc(x,y,z); // AccelerometerData acc(x,y,z);
// stepPattern.add(Timestamp::fromMS(i), acc); // 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)); stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); 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; (void) curPos;
const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw()); 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; const float nextStepAt = lastStepAtDistance + distAdd;
// 1st, start with random noise on the accelerometer // 1st, start with random noise on the accelerometer

4
synthetic/SyntheticWalker.h
View File

@@ -60,8 +60,8 @@ public:
// get the current position along the path // get the current position along the path
const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance);
const bool isOnFloor = path.isOnFloor(this->walkedDistance); const bool isPlainPart = path.isPlain(this->walkedDistance);
const Type type = (isOnFloor) ? (Type::FLOOR) : (Type::NON_FLOOR); 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() ); Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );