diff --git a/code/eval/DebugShortestPath.h b/code/eval/DebugShortestPath.h index 527dcb3..bbc1bac 100644 --- a/code/eval/DebugShortestPath.h +++ b/code/eval/DebugShortestPath.h @@ -33,9 +33,9 @@ public: } - GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad, Activity act) { - GridWalkState s = GridWalkShortestPathControl::getDestination(grid, start, distance_m, headChange_rad); + GridWalkState s = GridWalkShortestPathControl::getDestination(grid, start, distance_m, headChange_rad, act); if (this->recalc == 0){ vis.estPath.clear(); diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index ab2a7fe..f99d7ff 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -27,9 +27,11 @@ #include "../reader/SensorReader.h" #include "../reader/SensorReaderStep.h" #include "../reader/SensorReaderTurn.h" +#include "../reader/SensorReaderAccel.h" #include "../lukas/TurnObservation.h" #include "../lukas/StepObservation.h" +#include "../lukas/ActivityDetection.h" #include "../toni/BarometerSensorReader.h" @@ -133,6 +135,8 @@ public: // sensor numbers const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6; + const int s_accel = 0; + //const int s_linearAcceleration = 2; std::list turn_observations; @@ -141,6 +145,9 @@ public: //Create an BarometerSensorReader BarometerSensorReader baroSensorReader; + // activity detection + ActivityDetection actDet; + //Read all turn Observations while(srt->hasNext()) { @@ -234,9 +241,17 @@ public: case s_barometer: { obs.barometer = baroSensorReader.readBarometer(se); + actDet.addBaro(baroSensorReader.getHPA(se)); break; } + case s_accel: { + float acc[3]; + SensorReaderAccel sre; sre.read(se, acc); + actDet.addAccel(acc); + break; + } + // case s_linearAcceleration:{ // baroSensorReader.readVerticalAcceleration(se); // break; @@ -267,6 +282,11 @@ public: } + // currently detected activity + // TODO: feed sensor values! + ctrl.currentActivitiy = actDet.getCurrentActivity(); + + // time for a transition? if (se.ts - lastTransitionTS > MiscSettings::timeSteps) { @@ -366,6 +386,7 @@ public: vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n"; } vis.gp << "set label 111 '" << ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n"; + vis.gp << "set label 112 '" << "Act: " << actDet.toString() << "' at screen 0.1,0.15\n"; //double avgAngleRad = estBF.avgAngle * 180/3.14159265359; diff --git a/code/eval/PaperVisImportance.h b/code/eval/PaperVisImportance.h index 46a7951..cc60942 100644 --- a/code/eval/PaperVisImportance.h +++ b/code/eval/PaperVisImportance.h @@ -178,7 +178,7 @@ public: if (i % 250 == 0) {std::cout << i << std::endl;} const MyGridNode& nStart = gnEnd; GridWalkState sStart(&nStart, Heading::rnd()); - GridWalkState sEnd = walk.getDestination(grid, sStart, 135, 0); + GridWalkState sEnd = walk.getDestination(grid, sStart, 135, 0, Activity::UNKNOWN); (void) sEnd; } diff --git a/code/eval/SmoothingEvalBase.h b/code/eval/SmoothingEvalBase.h index 57e2db5..ef480ee 100644 --- a/code/eval/SmoothingEvalBase.h +++ b/code/eval/SmoothingEvalBase.h @@ -394,9 +394,10 @@ public: oSError.close(); // plot-data - std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); - std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close(); - std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close(); + std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); // ground truth + std::ofstream oEstN("/tmp/est_norm" + runName + ".dat"); vis.estPath.addDataTo(oEstN); oEstN.close(); // estimation via filter itself + std::ofstream oEstS("/tmp/est_smooth" + runName + ".dat"); vis.smoothPath.addDataTo(oEstS); oEstS.close(); // estimation via smoothing + std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close(); std::ofstream oPlot("/tmp/plot_" + runName + ".gp"); @@ -416,9 +417,10 @@ public: oPlot << "unset ztics\n"; oPlot << "splot \\\n"; - oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n"; - oPlot << "'path_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n"; - oPlot << "'est_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' "; + oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n"; + oPlot << "'path_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n"; + oPlot << "'est_norm_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099', "; + oPlot << "'est_smooth_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000000' "; oPlot.close(); } diff --git a/code/lukas/Activities.h b/code/lukas/Activities.h new file mode 100644 index 0000000..6cbe154 --- /dev/null +++ b/code/lukas/Activities.h @@ -0,0 +1,6 @@ +#ifndef ACTIVITIES_H +#define ACTIVITIES_H + + + +#endif // ACTIVITIES_H diff --git a/code/lukas/ActivityDetection.h b/code/lukas/ActivityDetection.h new file mode 100644 index 0000000..bbc5255 --- /dev/null +++ b/code/lukas/ActivityDetection.h @@ -0,0 +1,121 @@ +#ifndef ACTIVITYDETECTION_H +#define ACTIVITYDETECTION_H + +#include +#include "Activities.h" +#include + +#include +#include + +#include + + +#include + +/** + * classification of pedestrian activities using sensor inputs. + * sensor inputs are passed into this element + * recognized acitivities are the output + * + */ +class ActivityDetection { + +private: + + /** the currently detected activity */ + Activity current = Activity::UNKNOWN; + + K::Statistics mag; + K::Statistics hpa; + + MovingMedian hpaAvg; + +public: + + ActivityDetection() : hpaAvg(3) {;} + + /** add accelerometer values */ + void addAccel(float accel[3]) { + + float magnitude = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]); + mag.add(magnitude - 9.81f); + + } + + /** add barometer values */ + void addBaro(float hpa) { + hpaAvg.add(hpa); + float smoothed = hpaAvg.get(); + this->hpa.add(smoothed); + if (this->hpa.getCount() > 50) {analyze();} + } + + struct ActClass { + Activity act; + Distribution::Normal barometer; + Distribution::Normal magnitude; + ActClass(const Activity act, float muMag, float muBaro, float varMag, float varBaro) : + act(act), barometer(muBaro, std::sqrt(varBaro)), magnitude(muMag, std::sqrt(varMag)) { + ; + } + float getProbability(const float hpaRange, const float varMag) const { + return barometer.getProbability(hpaRange) * + magnitude.getProbability(varMag); + } + }; + + void analyze() { + +// std::vector classes = { +// ActClass(Activity::STANDING, 0.042118, 0.01, 0.002322, 0.001), +// ActClass(Activity::STAIRS, 3.561667, 0.035, 1.392631, 0.002), // stair up +// ActClass(Activity::STAIRS, 5.529414, -0.035, 3.416315, 0.002), // stair down +// ActClass(Activity::WALKING, 1.669506, 0.01, 0.273880, 0.001), +// }; + +// auto comp = [&] (const ActClass& a, const ActClass& b) { +// return a.getProbability(hpa.getRange(), mag.getStdDev()) < b.getProbability(hpa.getRange(), mag.getStdDev()); +// }; + +// auto it = std::max_element(classes.begin(), classes.end(), comp); + +// current = (*it).act; + + + if (mag.getStdDev() < 0.3) { + current = Activity::STANDING; + } else { + if (hpa.getRange() > 0.035) { + current = Activity::STAIRS; + } else { + current = Activity::WALKING; + } + } + +// current = (Activity) idx; + + mag.reset(); + hpa.reset(); + + } + + /** get the currently detected activity */ + Activity getCurrentActivity() const { + return current; + } + + std::string toString() const { + switch (current) { + case Activity::UNKNOWN: return "unknown"; + case Activity::STANDING: return "standing"; + case Activity::WALKING: return "walking"; + case Activity::STAIRS: return "stairs"; + case Activity::ELEVATOR: return "elevator"; + } + throw "should not happen"; + } + +}; + +#endif // ACTIVITYDETECTION_H diff --git a/code/particles/MyControl.h b/code/particles/MyControl.h index cfe5598..bcb141b 100755 --- a/code/particles/MyControl.h +++ b/code/particles/MyControl.h @@ -1,12 +1,17 @@ #ifndef MYCONTROL_H #define MYCONTROL_H +#include "../lukas/Activities.h" + +/** current control data for the transition step */ struct MyControl { float walked_m = 0; float headingChange_rad = 0; + Activity currentActivitiy = Activity::UNKNOWN; + }; #endif // MYCONTROL_H diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h index 7126f62..a327fbc 100755 --- a/code/particles/MyEvaluation.h +++ b/code/particles/MyEvaluation.h @@ -71,16 +71,18 @@ public: weight *= beaconEval.getProbability(p.state, observation); } - if (useStep) { - weight *= stepEval.getProbability(p.state, observation.step); - } + // CONTROL! +// if (useStep) { +// weight *= stepEval.getProbability(p.state, observation.step); +// } - if (useTurn) { - weight *= turnEval.getProbability(p.state, observation.turn, true); + // CONTROL! +// if (useTurn) { +// weight *= turnEval.getProbability(p.state, observation.turn, true); - //set - p.state.angularHeadingChange = observation.turn->delta_heading; - } +// //set +// p.state.angularHeadingChange = observation.turn->delta_heading; +// } // set and accumulate p.weight = weight; diff --git a/code/particles/MyTransition.h b/code/particles/MyTransition.h index 687ef90..cf3cd3d 100755 --- a/code/particles/MyTransition.h +++ b/code/particles/MyTransition.h @@ -103,7 +103,7 @@ public: // get new destination //const Node3* dst = choice->getTarget(src, p.state, dist_m); - p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad ); + p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad, control->currentActivitiy ); // randomly move the particle within its target grid (box) // (z remains unchanged!) diff --git a/code/reader/SensorReaderAccel.h b/code/reader/SensorReaderAccel.h new file mode 100644 index 0000000..9f997c8 --- /dev/null +++ b/code/reader/SensorReaderAccel.h @@ -0,0 +1,35 @@ +#ifndef SENSORREADERACCEL_H +#define SENSORREADERACCEL_H + +#include "../reader/SensorReader.h" + +#include + +class SensorReaderAccel { + +public: + + /** get wifi observation data from one CSV entry */ + static void read(const SensorEntry& se, float dst[3]) { + + std::string tmp = se.data; + + size_t pos1 = tmp.find(';'); + size_t pos2 = tmp.find(';', pos1+1); + size_t pos3 = tmp.find(';', pos2+1); + + if (pos1 == std::string::npos) {throw 1;} + if (pos2 == std::string::npos) {throw 1;} + + dst[0] = std::stof( tmp.substr(0, pos1) ); + dst[1] = std::stof( tmp.substr(pos1+1, pos2-pos1-1) ); + dst[2] = std::stof( tmp.substr(pos2+1, pos3-pos2-1) ); + + int j = 0; (void) j; + + } + +}; + + +#endif // SENSORREADERACCEL_H