From 9eb774f7b978136546eaf849006f3d614949d557 Mon Sep 17 00:00:00 2001 From: kazu Date: Tue, 26 Apr 2016 11:55:13 +0200 Subject: [PATCH] removed Heading::rnd() changed particle init fixed uninitialized values new ground-truth helper methods added new stats to the eval (distance from ground-truth) --- code/eval/FixedLagEvalBase.h | 26 +++++++++++++++++--------- code/eval/GroundTruthWay.h | 23 +++++++++++++++++++++++ code/eval/PaperVisImportance.h | 2 ++ code/particles/MyInitializer.h | 3 ++- code/particles/MyState.h | 10 +++++----- code/particles/MyTransition.h | 2 +- 6 files changed, 50 insertions(+), 16 deletions(-) diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index d8d67d1..698edeb 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -196,6 +196,8 @@ public: K::Statistics statsFiltering; K::Statistics statsSmoothing; + K::Statistics statsDistFiltering; + K::Statistics statsDistSmoothing; int cnt = 0; // error per update-step @@ -245,12 +247,12 @@ public: break; } - case s_accel: { - float acc[3]; - SensorReaderAccel sre; sre.read(se, acc); - actDet.addAccel(acc); - break; - } + case s_accel: { + float acc[3]; + SensorReaderAccel sre; sre.read(se, acc); + actDet.addAccel(acc); + break; + } // case s_linearAcceleration:{ // baroSensorReader.readVerticalAcceleration(se); @@ -297,7 +299,6 @@ public: // timed updates ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); - // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path const MyState est = pf->update(&ctrl, obs); const Point3 curEst = est.pCur; @@ -315,6 +316,7 @@ public: pathEst.push_back(curEst); const float err = diff.length(); + const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth @@ -323,8 +325,10 @@ public: if (cnt > 35) { statsFiltering.add(err); + statsDistFiltering.add(errDist); errorsNorm.push_back(err); - std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl; + std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl; + std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl; } if(cnt > skip){ @@ -363,10 +367,14 @@ public: const float errSmoothed = diffSmoothed.length(); + const float errDistSmoothed = gtw.getMinDist(curSmoothedEst); // minimum distance between smoothed-estimation and ground-truth + statsSmoothing.add(errSmoothed); + statsDistSmoothing.add(errDistSmoothed); errorsSmooth.push_back(errSmoothed); - std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; + std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; + std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl; //plot vis.clearStates(); diff --git a/code/eval/GroundTruthWay.h b/code/eval/GroundTruthWay.h index 2e1387c..3fccacb 100644 --- a/code/eval/GroundTruthWay.h +++ b/code/eval/GroundTruthWay.h @@ -18,6 +18,29 @@ public: /** get the ground truth way */ const std::vector& getWay() const {return entries;} + /** get the time where the ground-truth is nearest to p */ + uint64_t getNearestTime(const Point3 p) const { + + // TODO only an approximation (100ms) -> minor errors possible + + float minD = INFINITY; + uint64_t minTS = 0; + for (uint64_t ts = getMinKey(); ts <= getMaxKey(); ts += 100) { + const Point3 cur = getPosAtTime(ts); + const float curD = cur.getDistance(p); + if (curD < minD) {minD = curD; minTS = ts;} + } + return minTS; + + } + + /** get the minimum distance between the given point and the ground-truth (at any time) */ + float getMinDist(const Point3 p1) const { + const uint64_t minTS = getNearestTime(p1); + const Point3 p2 = getPosAtTime(minTS); + return p1.getDistance(p2); + } + }; diff --git a/code/eval/PaperVisImportance.h b/code/eval/PaperVisImportance.h index cc60942..9d70aa9 100644 --- a/code/eval/PaperVisImportance.h +++ b/code/eval/PaperVisImportance.h @@ -1,6 +1,7 @@ #ifndef PAPERVISIMPORTANCE_H #define PAPERVISIMPORTANCE_H +/* #include #include #include @@ -275,5 +276,6 @@ public: }; +*/ #endif // PAPERVISIMPORTANCE_H diff --git a/code/particles/MyInitializer.h b/code/particles/MyInitializer.h index 765e971..79ea62e 100755 --- a/code/particles/MyInitializer.h +++ b/code/particles/MyInitializer.h @@ -37,6 +37,7 @@ public: std::minstd_rand gen; std::uniform_int_distribution<> dist(0, grid.getNumNodes()); + std::uniform_real_distribution distHead(0, M_PI*2); for (K::Particle& p : particles) { @@ -50,7 +51,7 @@ public: p.state.walkState.node = &n; p.state.pOld = p.state.pCur; - p.state.walkState.heading = Heading::rnd(); + p.state.walkState.heading = distHead(gen); p.state.hPa = 0; } diff --git a/code/particles/MyState.h b/code/particles/MyState.h index 832c4cf..b2835f4 100755 --- a/code/particles/MyState.h +++ b/code/particles/MyState.h @@ -25,18 +25,18 @@ struct MyState { GridWalkState walkState; // cumulative heading - double cumulativeHeading; + double cumulativeHeading = 0; // save last hPa measurement for the smoothing process - double measurement_pressure; + double measurement_pressure = 0; // save last angularHeadingChangefor the smoothing process in Degree - double angularHeadingChange; + double angularHeadingChange = 0; - double avgAngle; + double avgAngle = 0; //the current Activity - Activity currentActivity; + Activity currentActivity = Activity::UNKNOWN; //int distanceWalkedCM; diff --git a/code/particles/MyTransition.h b/code/particles/MyTransition.h index cf3cd3d..3507092 100755 --- a/code/particles/MyTransition.h +++ b/code/particles/MyTransition.h @@ -88,7 +88,7 @@ public: // update the old heading and the other old values //p.state.walkState.heading = p.state.heading; if(!(p.state.pOld == p.state.pCur)){ - p.state.cumulativeHeading = Angle::getDEG_360(p.state.pOld.x, p.state.pOld.y, p.state.pCur.x, p.state.pCur.y); + p.state.cumulativeHeading = Angle::getDEG_360(p.state.pOld.x, p.state.pOld.y, p.state.pCur.x, p.state.pCur.y); } p.state.pOld = p.state.pCur;