From 716b004f3ce1c7a55dfbf1d9fdec24f199cf0368 Mon Sep 17 00:00:00 2001 From: FrankE Date: Fri, 5 Feb 2016 20:21:46 +0100 Subject: [PATCH] changed visualisation added new eval code for new walkers improved barometer (moving avg and median) floorplan-staircase-fixes disabled step-turn (now part of transition) added abs-orientation-reader (for testing) added beacons --- code/CMakeLists.txt | 2 +- code/Vis.h | 5 +- code/eval/Eval1.h | 38 +++++++++----- code/eval/EvalBase.h | 31 ++++++++--- code/frank/OrientationObservation.h | 7 +++ code/frank/OrientationSensorReader.h | 38 ++++++++++++++ code/frank/Settings.h | 5 +- code/frank/WiFiAP.h | 9 ++-- code/lukas/StepEvaluation.h | 49 +++++------------ code/lukas/TurnEvaluation.h | 51 +++++------------- code/main.cpp | 12 ++--- code/particles/MyObservation.h | 4 ++ code/plan_new.svg | 37 +++++++++---- code/toni/BarometerEvaluation.h | 1 - code/toni/BarometerSensorReader.h | 78 ++++++---------------------- 15 files changed, 188 insertions(+), 179 deletions(-) diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index 3413a8d..c5c9690 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,7 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O1 + -O2 -DWITH_TESTS -DWITH_ASSERTIONS diff --git a/code/Vis.h b/code/Vis.h index ca8d11c..9f14ff8 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -26,8 +26,9 @@ public: K::GnuplotSplotElementLines gridEdges; K::GnuplotSplotElementPoints particles; K::GnuplotSplotElementLines particleDir; - K::GnuplotSplotElementLines groundTruth; + K::GnuplotSplotElementLines estPath; + K::GnuplotSplotElementLines groundTruth; public: @@ -70,6 +71,8 @@ public: for (const Line2& l : f.getObstacles()) { const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm()); const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm()); + //if (l.p1.x > 3000) {continue;} + //if (l.p1.y > 3000) {continue;} floors.addSegment(p1, p2); } diff --git a/code/eval/Eval1.h b/code/eval/Eval1.h index f0e61bc..592a89c 100644 --- a/code/eval/Eval1.h +++ b/code/eval/Eval1.h @@ -7,7 +7,9 @@ #include #include #include + #include +#include #include #include @@ -25,7 +27,7 @@ public: pf = new K::ParticleFilter( MiscSettings::numParticles, std::unique_ptr(new MyInitializer(grid, 1120, 150, 3*350, 90)) ); - std::vector wp = path1; std::reverse(wp.begin(), wp.end()); + std::vector wp = path1;// std::reverse(wp.begin(), wp.end()); MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) ); MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) ); @@ -35,21 +37,22 @@ public: //GridWalkPushForward* walk = new GridWalkPushForward(); //GridWalkLightAtTheEndOfTheTunnel* walk = new GridWalkLightAtTheEndOfTheTunnel(grid, DijkstraMapper(grid), end); GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + //GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); // path1 - //sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward - //srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt"); - //srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt"); - sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward - srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt"); - srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt"); + sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward + srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt"); + srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt"); +// sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward +// srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt"); +// srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt"); // path2 - //sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward - //srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt"); - //srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt"); +// sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward +// srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt"); +// srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt"); //sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward //srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt"); //srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt"); @@ -62,11 +65,22 @@ public: // srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt"); // srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt"); + // path4 +// sr = new SensorReader("./measurements/path4/1454595382218.csv"); // forward +// srt = new SensorReaderTurn("./measurements/path4/Turns.txt"); +// srs = new SensorReaderStep("./measurements/path4/Steps2.txt"); + + gtw = getGroundTruthWay(*sr, floors.gtwp, wp); } + void setEvalFails() { + + } + + //wifi also uniform dist 0/1 fuer bereiche die OK sind? //steps hochzaehlen weil mehr als einer in einer transition?? //increase regional average region @@ -87,9 +101,9 @@ public: //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.10)) ); // state estimation step - //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); } diff --git a/code/eval/EvalBase.h b/code/eval/EvalBase.h index e816478..e36beff 100644 --- a/code/eval/EvalBase.h +++ b/code/eval/EvalBase.h @@ -27,6 +27,7 @@ #include "../frank/WiFiSensorReader.h" #include "../frank/BeaconSensorReader.h" +#include "../frank/OrientationSensorReader.h" class EvalBase { @@ -57,6 +58,7 @@ protected: std::vector path1 = {29, 28,27,26,255,25,24,23,22,21,20}; std::vector path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6}; std::vector path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31}; + std::vector path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; public: @@ -71,6 +73,12 @@ public: vis.addFloor(floors.f2, floors.h2); vis.addFloor(floors.f3, floors.h3); + vis.floors.setColorHex("#666666"); + vis.groundTruth.setCustomAttr("dashtype 3"); + vis.groundTruth.setColorHex("#009900"); + vis.gp << "unset cbrange\n"; + + } static GridPoint conv(const Point3& p) { @@ -110,7 +118,7 @@ public: void run() { // sensor numbers - const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; + const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6; const int s_linearAcceleration = 2; std::list turn_observations; @@ -198,8 +206,13 @@ public: } case s_linearAcceleration:{ - baroSensorReader.readVerticalAcceleration(se); - break; + baroSensorReader.readVerticalAcceleration(se); + break; + } + + case s_orientation: { + obs.orientation = OrientationSensorReader::read(se); + break; } } @@ -235,7 +248,7 @@ public: const Point3 curEst = est.pCur; // error calculation. compare ground-truth to estimation - const int offset = 0; // 750 + const int offset = 750; const Point3 curGT = gtw.getPosAtTime(se.ts - offset); const Point3 diff = curEst - curGT; @@ -258,11 +271,17 @@ public: vis.addEstPath(pathEst); vis.setEstAndShould(curEst, curGT); + if (obs.barometer != nullptr) { + vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n"; + } vis.gp << "set label 111 '" < + +class OrientationSensorReader { + +public: + + /** get wifi observation data from one CSV entry */ + static OrientationObservation read(const SensorEntry& se) { + + std::string tmp = se.data; + OrientationObservation obs; + + size_t pos1 = tmp.find(';'); + size_t pos2 = tmp.find(';', pos1+1); + size_t pos3 = tmp.find(';', pos2+1); + + assert(pos1 != std::string::npos); + assert(pos2 != std::string::npos); + assert(pos3 != std::string::npos); + + const std::string s1 = tmp.substr(0, pos1); + const std::string s2 = tmp.substr(pos1+1, pos2-pos1-1); + const std::string s3 = tmp.substr(pos2+1, pos3-pos2-1); + + obs.values[0] = std::stof(s1); + obs.values[1] = std::stof(s2); + obs.values[2] = std::stof(s3); + + return obs; + + } + +}; + #endif // ORIENTATIONSENSORREADER_H diff --git a/code/frank/Settings.h b/code/frank/Settings.h index f768e38..a212b08 100755 --- a/code/frank/Settings.h +++ b/code/frank/Settings.h @@ -22,6 +22,7 @@ public: const double pl = 2.7; // 2.7 const double tx = -46; + const float ibPLE = 1.9; addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl); addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl); @@ -52,7 +53,9 @@ public: addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl); addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!! - + addBeacon("48:EF:8D:77:66:DF", -81, ibPLE, 6984, 4526, Helper::getHeight(2)); + addBeacon("6F:5F:39:0C:51:E4", -81, ibPLE, 7829, 3916, 200); + addBeacon("49:23:D8:7F:E8:D2", -81, ibPLE, 6946, 4536, Helper::getHeight(1)); // OLD diff --git a/code/frank/WiFiAP.h b/code/frank/WiFiAP.h index 9337a4e..42cb6b5 100755 --- a/code/frank/WiFiAP.h +++ b/code/frank/WiFiAP.h @@ -15,15 +15,16 @@ class WiFiAP { public: /** the AP's MAC-Address */ - MACAddress mac; + const MACAddress mac; /** the AP's readable SSID */ - std::string ssid; + const std::string ssid; - double tx; + /** AP tx-power */ + const float tx; /** path loss for this ap. for testing */ - double pl; + const float pl; diff --git a/code/lukas/StepEvaluation.h b/code/lukas/StepEvaluation.h index 1671af2..8013ef1 100755 --- a/code/lukas/StepEvaluation.h +++ b/code/lukas/StepEvaluation.h @@ -21,48 +21,27 @@ public: return 1; - const float mdlWalked_m = state.walkState.distanceWalked_m; + // see: particle-filter-control-data +// const float mdlWalked_m = state.walkState.distanceWalked_m; - ((MyState&)state).walkState.distanceWalked_m = 0; +// ((MyState&)state).walkState.distanceWalked_m = 0; - const float stepSize_m = 0.71; - const float sensSigma_m = 0.05 + (0.05 * obs->steps); - const float sensWalked_m = obs->steps * stepSize_m; +// const float stepSize_m = 0.71; +// const float sensSigma_m = 0.05 + (0.05 * obs->steps); +// const float sensWalked_m = obs->steps * stepSize_m; - if (obs->steps > 1) { - int i = 0; - int j = i+1; ++j; - } - - const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m); - - if (prob != prob) { - throw 1; - } - - return prob; - -// float a = 1.0; -// float mu_distance = 0; -// float sigma_distance = 0; - -// if(obs->step) { -// a = 1.0; -// mu_distance = mu_walk; -// sigma_distance = sigma_walk; +// if (obs->steps > 1) { +// int i = 0; +// int j = i+1; ++j; // } -// else { -// a = 0.0; -// mu_distance = mu_stop; -// sigma_distance = sigma_stop; +// const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m); + +// if (prob != prob) { +// throw 1; // } -// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step -// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) + -// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance); - -// return p; +// return prob; } diff --git a/code/lukas/TurnEvaluation.h b/code/lukas/TurnEvaluation.h index b21e04b..3a690da 100755 --- a/code/lukas/TurnEvaluation.h +++ b/code/lukas/TurnEvaluation.h @@ -19,49 +19,22 @@ public: return 1; - // get the current heading-change - const float delta = Angle::radToDeg(state.walkState.headingChange_rad); + // see: particle-filter-control-data +// // get the current heading-change +// const float delta = Angle::radToDeg(state.walkState.headingChange_rad); - // and reset it as it was evaluated - ((MyState&) state).walkState.headingChange_rad = 0; +// // and reset it as it was evaluated +// ((MyState&) state).walkState.headingChange_rad = 0; - // proability? - const float sigma = 15.0; - const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta); +// // proability? +// const float sigma = 15.0; +// const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta); - if (prob != prob) { - throw 1; - } +// if (prob != prob) { +// throw 1; +// } - return prob; - - - -// //Particle's heading change -// double delta_heading_particle = state.heading - state.heading_old; - - -// //Correct offset of the heading change -// if (delta_heading_particle < -180) { -// delta_heading_particle += 360; -// } -// else if (delta_heading_particle > 180) { -// delta_heading_particle -= 360; -// } - - -// //Switch between simple and improved evaluation -// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using -// //normal distribution -// //if(simple) { - -// double sigma_delta_heading = sigma_heading; - -// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle); - - -// return p; -// // } +// return prob; } diff --git a/code/main.cpp b/code/main.cpp index 0a57558..26fe128 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -73,8 +73,6 @@ void testModelWalk() { vis.show(); } - - sleep(1000); } @@ -85,12 +83,12 @@ int main(void) { // testModelWalk(); -// Eval1 eval; -// eval.setEval1(); -// eval.run(); + Eval1 eval; + eval.setEval1(); + eval.run(); - PaperVisImportance::createImportance(); - PaperVisImportance::createPath(); +// PaperVisImportance::createImportance(); +// PaperVisImportance::createPath(); return 0; diff --git a/code/particles/MyObservation.h b/code/particles/MyObservation.h index 2b045c1..e9cca28 100755 --- a/code/particles/MyObservation.h +++ b/code/particles/MyObservation.h @@ -3,6 +3,8 @@ #include "../frank/WiFiObservation.h" #include "../frank/BeaconObservation.h" +#include "../frank/OrientationObservation.h" + #include "../toni/BarometerObservation.h" #include "../lukas/StepObservation.h" #include "../lukas/TurnObservation.h" @@ -15,6 +17,8 @@ struct MyObservation { /** wifi observation */ WiFiObservation wifi; + OrientationObservation orientation; + /** barometer observation data (if any) */ BarometerObservation* barometer = nullptr; diff --git a/code/plan_new.svg b/code/plan_new.svg index 8e50e36..a898c88 100644 --- a/code/plan_new.svg +++ b/code/plan_new.svg @@ -42,17 +42,17 @@ guidetolerance="10" inkscape:pageopacity="0" inkscape:pageshadow="2" - inkscape:window-width="3200" - inkscape:window-height="1671" + inkscape:window-width="1600" + inkscape:window-height="822" id="namedview3234" showgrid="true" - inkscape:zoom="1.6989779" - inkscape:cx="881.41659" - inkscape:cy="1487.0248" + inkscape:zoom="2.4027176" + inkscape:cx="2636.6172" + inkscape:cy="1428.0719" inkscape:window-x="0" - inkscape:window-y="55" + inkscape:window-y="0" inkscape:window-maximized="1" - inkscape:current-layer="layer26" + inkscape:current-layer="layer12" inkscape:object-nodes="true" units="px" showborder="true" @@ -2511,7 +2511,7 @@ inkscape:groupmode="layer" id="layer12" inkscape:label="floor_1" - style="display:none"> + style="display:inline"> + + +