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

worked on grid-walker and synthetic steps/turns

Browse Source
This commit is contained in:
k-a-z-u
2017-11-29 16:35:29 +01:00
parent 55c061b344
commit 63bc2f3046
5 changed files with 370 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
grid/walk/v3/Helper.h
View File

@@ -193,10 +193,22 @@ namespace GW3 {
return bbox.contains(pt); return bbox.contains(pt);
} }
// /** does one of the given grid-nodes contains the provided point-in-question? */
// static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
// for (const Node* n : nodes) {
// if (contains(grid, n, pt)) {
// return n;
// }
// }
// return nullptr;
// }
/** does one of the given grid-nodes contains the provided point-in-question? */ /** does one of the given grid-nodes contains the provided point-in-question? */
static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) { static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& nodes, Point2 pt) {
for (const Node* n : nodes) { for (const Node* n : nodes) {
if (contains(grid, n, pt)) {return n;} if (contains(grid, n, pt)) {
return n;
}
} }
return nullptr; return nullptr;
} }

491
grid/walk/v3/Reachable.h
View File

@@ -7,250 +7,333 @@
namespace GW3 { namespace GW3 {
#define likely(x) __builtin_expect((x),1) #define likely(x) __builtin_expect((x),1)
#define unlikely(x) __builtin_expect((x),0) #define unlikely(x) __builtin_expect((x),0)
/** /**
* get all grid nodes that are reachable within x-edges (depth) * get all grid nodes that are reachable within x-edges (depth)
*/ */
template <typename Node> class ReachableByDepthUnsorted { template <typename Node> class ReachableByDepthUnsorted {
struct VisitEntry { struct VisitEntry {
const Node* gn; const Node* gn;
int depth; int depth;
VisitEntry() {;} VisitEntry() {;}
VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
}; };
struct Visits { struct Visits {
VisitEntry visits[512];// __attribute__((aligned(16))); VisitEntry visits[512];// __attribute__((aligned(16)));
size_t head = 0; size_t head = 0;
size_t tail = 0; size_t tail = 0;
VisitEntry& getNext() { VisitEntry& getNext() {
return visits[tail++]; return visits[tail++];
} }
void add(const VisitEntry& e) { void add(const VisitEntry& e) {
visits[head++] = e; visits[head++] = e;
assert(head < 512); assert(head < 512);
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
} }
bool hasMore() const { bool hasMore() const {
return head > tail; return head > tail;
} }
}; };
const Grid<Node>& grid; const Grid<Node>& grid;
public: public:
ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) { ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
; ;
}
/** get all nodes reachable from start using maxDepth steps */
std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
std::unordered_set<const Node*> checked;
// assuming max 8 neighbors per node, we need
// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
// which is 1 + (1+2+3+4+5)*neighbors
// which is 1 + (n*n + n)/2*neighbors
// however this seems to be slow?!
//const int n = maxDepth + 1;
//const int maxEntries = (n * n + n) / 2 * 10 + 1;
//const int toAlloc = 4096 / sizeof(VisitEntry);
//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
//if (maxDepth > 9) {throw Exception("will not fit!");}
Visits toVisit;
// directly start with the node itself and all its neighbors
checked.insert(&start);
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
const int nIdx = start.getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
checked.insert(&gnNext);
toVisit.add(VisitEntry(&gnNext, 1));
} }
// check all to-be-visited nodes /** get all nodes reachable from start using maxDepth steps */
while ( likely(toVisit.hasMore()) ) { std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
const VisitEntry& e = toVisit.getNext(); std::unordered_set<const Node*> checked;
if ( likely(e.depth <= maxDepth) ) { // assuming max 8 neighbors per node, we need
// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
// which is 1 + (1+2+3+4+5)*neighbors
// which is 1 + (n*n + n)/2*neighbors
// however this seems to be slow?!
//const int n = maxDepth + 1;
//const int maxEntries = (n * n + n) / 2 * 10 + 1;
//const int toAlloc = 4096 / sizeof(VisitEntry);
//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
//if (maxDepth > 9) {throw Exception("will not fit!");}
const Node* gnCur = e.gn; Visits toVisit;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i); // directly start with the node itself and all its neighbors
const Node& gnNext = grid[nIdx]; checked.insert(&start);
if ( unlikely(checked.find(&gnNext) == checked.end()) ) { for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
toVisit.add(VisitEntry(&gnNext, e.depth+1)); const int nIdx = start.getNeighborIdx(i);
checked.insert(&gnNext); const Node& gnNext = grid[nIdx];
checked.insert(&gnNext);
toVisit.add(VisitEntry(&gnNext, 1));
}
// check all to-be-visited nodes
while ( likely(toVisit.hasMore()) ) {
const VisitEntry& e = toVisit.getNext();
if ( likely(e.depth <= maxDepth) ) {
const Node* gnCur = e.gn;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
toVisit.add(VisitEntry(&gnNext, e.depth+1));
checked.insert(&gnNext);
}
} }
} }
} }
} return checked;
return checked;
}
};
/**
* get all grid nodes that are reachable within x-edges (depth)
* additionally returns the needed walking distance in meter
*/
template <typename Node> class ReachableByDepthWithDistanceSorted {
struct VisitEntry {
const Node* gn;
int depth;
float dist_m;
int myIdx;
VisitEntry() {;}
VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
};
struct Visits {
VisitEntry visits[1024];// __attribute__((aligned(16)));
size_t head = 0;
size_t tail = 0;
VisitEntry& getNext() {
return visits[tail++];
}
void add(const VisitEntry& e) {
visits[head++] = e;
assert(head < 1024);
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
}
bool hasMore() const {
return head > tail;
}
void sort() {
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
return e1.dist_m < e2.dist_m;
};
std::sort(&visits[tail], &visits[head], comp);
}
};
const Grid<Node>& grid;
public:
/** result */
struct Entry {
const Node* node;
const float walkDistToStart_m;
const int prevIdx;
Entry(const Node* node, const float dist, const size_t prevIdx) :
node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
bool hasPrev() const {
return prevIdx >= 0;
} }
}; };
ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
;
}
/** get all nodes reachable from start using maxDepth steps */ /**
std::vector<Entry> get(const Node& start, const int maxDepth) { * get all grid nodes that are reachable within x-edges (depth)
* additionally returns the needed walking distance in meter
*/
template <typename Node> class ReachableByDepthWithDistanceSorted {
std::unordered_set<const Node*> checked; struct VisitEntry {
std::vector<Entry> res; const Node* gn;
int depth;
float dist_m;
int myIdx;
VisitEntry() {;}
VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
};
Visits toVisit; struct Visits {
VisitEntry visits[1024];// __attribute__((aligned(16)));
// directly start with the node itself and all its neighbors size_t head = 0;
checked.insert(&start); size_t tail = 0;
res.push_back(Entry(&start, 0, -1)); VisitEntry& getNext() {
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { return visits[tail++];
const int nIdx = start.getNeighborIdx(i); }
const Node& gnNext = grid[nIdx]; void add(const VisitEntry& e) {
const float dist_m = gnNext.getDistanceInMeter(start); visits[head++] = e;
toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); assert(head < 1024);
res.push_back(Entry(&gnNext, dist_m, 0)); //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
checked.insert(&gnNext); }
} bool hasMore() const {
toVisit.sort(); return head > tail;
}
// check all to-be-visited nodes void sort() {
while ( likely(toVisit.hasMore()) ) {
const VisitEntry& e = toVisit.getNext();
if ( likely(e.depth <= maxDepth) ) {
const Node* gnCur = e.gn;
// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
// const int nIdx = gnCur->getNeighborIdx(i);
// const Node& gnNext = grid[nIdx];
// if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
// const float dist_m = e.dist_m + nodeNodeDist_m;
// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
// res.push_back(Entry(&gnNext, dist_m, e.myIdx));
// checked.insert(&gnNext);
// }
// }
// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
std::vector<VisitEntry> sub;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
const float dist_m = e.dist_m + nodeNodeDist_m;
//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
res.push_back(Entry(&gnNext, dist_m, e.myIdx));
checked.insert(&gnNext);
}
}
// dijkstra.. sort the new nodes by destination to start
// only sorting the 8 new nodes seems enough due to the graph's layout
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
return e1.dist_m < e2.dist_m; return e1.dist_m < e2.dist_m;
}; };
std::sort(&visits[tail], &visits[head], comp);
}
};
std::sort(sub.begin(), sub.end(), comp); const Grid<Node>& grid;
public:
/** result */
struct Entry {
const Node* node;
const float walkDistToStart_m;
const int prevIdx;
Entry(const Node* node, const float dist, const size_t prevIdx) :
node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
bool hasPrev() const {
return prevIdx >= 0;
}
};
ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
;
}
/** get all nodes reachable from start using maxDepth steps */
std::vector<Entry> get(const Node& start, const int maxDepth) {
std::unordered_set<const Node*> checked;
std::vector<Entry> res;
Visits toVisit;
// directly start with the node itself and all its neighbors
checked.insert(&start);
res.push_back(Entry(&start, 0, -1));
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
const int nIdx = start.getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
const float dist_m = gnNext.getDistanceInMeter(start);
toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
res.push_back(Entry(&gnNext, dist_m, 0));
checked.insert(&gnNext);
}
toVisit.sort();
// check all to-be-visited nodes
while ( likely(toVisit.hasMore()) ) {
const VisitEntry& e = toVisit.getNext();
if ( likely(e.depth <= maxDepth) ) {
const Node* gnCur = e.gn;
// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
// const int nIdx = gnCur->getNeighborIdx(i);
// const Node& gnNext = grid[nIdx];
// if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
// const float dist_m = e.dist_m + nodeNodeDist_m;
// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
// res.push_back(Entry(&gnNext, dist_m, e.myIdx));
// checked.insert(&gnNext);
// }
// }
// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
std::vector<VisitEntry> sub;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
const float dist_m = e.dist_m + nodeNodeDist_m;
//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
res.push_back(Entry(&gnNext, dist_m, e.myIdx));
checked.insert(&gnNext);
}
}
// dijkstra.. sort the new nodes by destination to start
// only sorting the 8 new nodes seems enough due to the graph's layout
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
return e1.dist_m < e2.dist_m;
};
std::sort(sub.begin(), sub.end(), comp);
for (const VisitEntry& e : sub) {
toVisit.add(e);
}
}
// slower with same result ;)
//toVisit.sort();
}
return res;
}
};
/**
* data-structure to track to-be-visited nodes
* push_back, pop_front
* as pop_front is costly, we omit the pop and use a head-index instead
* memory-consumption vs speed
*/
struct _ToVisit {
size_t nextIdx = 0;
std::vector<uint32_t> vec;
_ToVisit() {vec.reserve(256);}
void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
uint32_t next() {return vec[nextIdx++];}
bool empty() const {return nextIdx >= vec.size();}
};
/** get a list of all nodes that are reachable after checking several conditions */
template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
public:
static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
//Node* curNode = nullptr;
std::unordered_set<uint32_t> scheduled;
_ToVisit toVisit;
toVisit.add(start.getIdx());
std::vector<const Node*> res;
while(!toVisit.empty()) {
// get the next to-be-visited node
const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance)
const Node& curNode = grid[curIdx];
// process current node
res.push_back(&curNode);
scheduled.insert(curIdx);
// get all neighbors
const int numNeighbors = curNode.getNumNeighbors();
for (int i = 0; i < numNeighbors; ++i) {
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
// already visited?
if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
scheduled.insert(neighborIdx);
// matches the used condition?
const Node& neighbor = grid[neighborIdx];
if (!cond.visit(neighbor)) {continue;}
// OK!
toVisit.add(neighborIdx);
for (const VisitEntry& e : sub) {
toVisit.add(e);
} }
} }
// slower with same result ;) // done
//toVisit.sort(); return res;
} }
return res;
}
}; //const Node& next(const std::function<bool(const Node&)>& skip) {
//template <typename Skip> const Node& next(const Skip skip) {
const Node& next() {
}
};
} }

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

@@ -65,35 +65,63 @@ namespace GW3 {
const GridPoint gpStart = Helper::p3ToGp(params.start); const GridPoint gpStart = Helper::p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart); const Node* startNode = grid.getNodePtrFor(gpStart);
// calculate a walk's probability
auto getP = [&] (const Point3 dst) {
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params);
p *= p1;
}
return p;
};
// include one additional grid-cell (increased distance) // include one additional grid-cell (increased distance)
const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1); //const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
ReachableSettings set; const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
set.limitDistance = true;
set.dist_m = params.distance_m + secBuffer_m; // ReachableSettings set;
set.limitHeading = false; // set.limitDistance = true;
set.heading = params.heading; // set.dist_m = params.distance_m + secBuffer_m;
set.maxHeadingDiff_rad = M_PI/2; // set.limitHeading = false;
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); // set.heading = params.heading;
// set.maxHeadingDiff_rad = M_PI/2;
// // get all nodes that satisfy above constraints
// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
struct Cond {
const float maxDist_m;
const Node* startNode;
Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
bool visit(const Node& n) const {
return (startNode->getDistanceInMeter(n)) < maxDist_m;
}
};
Cond cond(params.distance_m+secBuffer_m, startNode);
std::vector<const Node*> reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
WalkResult res; WalkResult res;
res.heading = params.heading; res.heading = params.heading;
res.position = params.start; res.position = params.start;
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = res.heading.asVector(); const Point2 dir = res.heading.asVector();
const Point2 dst = params.start.xy() + (dir * params.distance_m); const Point2 dst = params.start.xy() + (dir * params.distance_m);
// is dst reachable? // is above destination reachable?
const Node* n = Helper::contains(grid, reachableNodes, dst); const Node* n = Helper::contains(grid, reachableNodes, dst);
//const Node* n = ri.contains(dst);
if (n) { if (n) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = Helper::p3ToGp(p3); const GridPoint gp = Helper::p3ToGp(p3);
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; // keep as-is res.probability *= getP(p3); // keep as-is
return res; // done return res; // done
} else { } else {
@@ -129,7 +157,7 @@ namespace GW3 {
} }
res.heading = Heading(start.xy(), end.xy()); res.heading = Heading(start.xy(), end.xy());
res.probability = p; res.probability *= getP(end);
res.position = end; res.position = end;
return res; return res;

20
synthetic/SyntheticSteps.h
View File

@@ -37,14 +37,22 @@ private:
Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3); Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4); Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
int stepPatternPos = -1; int stepPatternPos = -1;
std::vector<Listener*> listeners; std::vector<Listener*> listeners;
//float stepSize_m;
//float stepSizeSigma_m;
float noiseLevel;
Distribution::Normal<float> dNextStep;
public: public:
/** ctor with the walker to follow */ /** ctor with the walker to follow */
SyntheticSteps(SyntheticWalker* walker) { SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, 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) {
walker->addListener(this); walker->addListener(this);
dX.setSeed(1); dX.setSeed(1);
@@ -82,15 +90,15 @@ protected:
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
(void) curPos; (void) curPos;
const float nextStepAt = (lastStepAtDistance + stepSize_m); const float nextStepAt = lastStepAtDistance + dNextStep.draw();
// 1st, start with random noise on the accelerometer // 1st, start with random noise on the accelerometer
const float x = dX.draw(); const float x = dX.draw();
const float y = dY.draw(); const float y = dY.draw();
const float z = dZ.draw(); const float z = dZ.draw();
const AccelerometerData base(0, 4, 9.7); const AccelerometerData aBase(0, 4, 9.7);
const AccelerometerData noise(x, y, z); const AccelerometerData aNoise(x, y, z);
AccelerometerData acc = base + noise; AccelerometerData acc = aBase + aNoise * noiseLevel;
// is it time to inject a "step" into the accelerometer data? // is it time to inject a "step" into the accelerometer data?
if (walkedDistance > nextStepAt) { if (walkedDistance > nextStepAt) {
@@ -105,7 +113,7 @@ protected:
refStepPattern = Timestamp::fromMS(0); refStepPattern = Timestamp::fromMS(0);
} else { } else {
const AccelerometerData step = stepPattern.get(curPatPos); const AccelerometerData step = stepPattern.get(curPatPos);
acc = base + noise*2.5f + step; acc = aBase + (aNoise * noiseLevel) + step;
} }
} }

23
synthetic/SyntheticTurns.h
View File

@@ -39,17 +39,23 @@ private:
Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003); Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25); Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100); Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
//float headingDrift_rad;
//float headingSigma_rad;
float noiseLevel;
Distribution::Normal<float> dHeadErr;
std::vector<Listener*> listeners; std::vector<Listener*> listeners;
public: public:
/** ctor with the walker to follow */ /** ctor with the walker to follow */
SyntheticTurns(SyntheticWalker* walker) { SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) :
//headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad),
noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) {
walker->addListener(this); walker->addListener(this);
dAccX.setSeed(1); dAccX.setSeed(1);
dAccY.setSeed(3); dAccY.setSeed(3);
@@ -57,6 +63,7 @@ public:
dGyroX.setSeed(7); dGyroX.setSeed(7);
dGyroY.setSeed(9); dGyroY.setSeed(9);
dGyroZ.setSeed(11); dGyroZ.setSeed(11);
} }
/** attach a listener to this provider */ /** attach a listener to this provider */
@@ -116,14 +123,14 @@ protected:
// convert to gyro's radians-per-second // convert to gyro's radians-per-second
const double radPerSec = change * 1000 / deltaTs.ms();; const double radPerSec = change * 1000 / deltaTs.ms();;
const float accX = 0.00 + dAccX.draw(); const float accX = 0.00 + dAccX.draw() * (noiseLevel);
const float accY = 0.00 + dAccY.draw(); const float accY = 0.00 + dAccY.draw() * (noiseLevel);
const float accZ = 9.81 + dAccZ.draw(); const float accZ = 9.81 + dAccZ.draw() * (noiseLevel);
AccelerometerData acc(accX, accY, accZ); AccelerometerData acc(accX, accY, accZ);
const float gyroX = dGyroX.draw(); const float gyroX = dGyroX.draw() * (noiseLevel);
const float gyroY = dGyroY.draw(); const float gyroY = dGyroY.draw() * (noiseLevel);
const float gyroZ = dGyroZ.draw() + radPerSec; const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec;
GyroscopeData gyro(gyroX, gyroY, gyroZ); GyroscopeData gyro(gyroX, gyroY, gyroZ);
for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);} for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}