From 6171582b4085bab0d0a67275d658032b24ba68f4 Mon Sep 17 00:00:00 2001 From: kazu Date: Sat, 1 Apr 2017 09:13:45 +0200 Subject: [PATCH] many changes --- PlotErrFunc.h | 18 +++++++++---- Plotty.h | 13 ++++++--- Settings.h | 14 +++++++--- main.cpp | 2 +- EvalWalk.h => pf/EvalWalk.h | 54 +++++++++++++++++++++++++++---------- pf/PF.h | 14 +++++++--- 6 files changed, 85 insertions(+), 30 deletions(-) rename EvalWalk.h => pf/EvalWalk.h (83%) diff --git a/PlotErrFunc.h b/PlotErrFunc.h index 2eaa807..4a926e4 100644 --- a/PlotErrFunc.h +++ b/PlotErrFunc.h @@ -19,6 +19,7 @@ class PlotErrFunc { }; std::vector entries; + std::vector lines; K::Gnuplot gp; K::GnuplotPlot gplot; @@ -39,6 +40,15 @@ public: /** add one curve */ void add(const std::string name, const K::Statistics stats) { entries.push_back(Entry(name, stats)); + K::GnuplotPlotElementLines* gpel = new K::GnuplotPlotElementLines(); + gpel->setTitle(name); + gpel->setLineWidth(2); + lines.push_back(gpel); + gplot.add(gpel); + } + + void clear() { + entries.clear(); } K::Gnuplot& getGP() { @@ -57,12 +67,11 @@ public: for (size_t i = 0; i < entries.size(); ++i) { const Entry e = entries[i]; + K::GnuplotPlotElementLines* line = lines[i]; - K::GnuplotPlotElementLines* line = new K::GnuplotPlotElementLines(); - line->setTitle(e.name); + line->clear(); + //line.setTitle(e.name); line->setColorHex(colors[i]); - line->setLineWidth(2); - gplot.add(line); // 0 - 80% for (int i = 0; i <= 85; i+= 5) { @@ -83,7 +92,6 @@ public: } gp.flush(); - gp.close(); } diff --git a/Plotty.h b/Plotty.h index 98e1529..4478011 100644 --- a/Plotty.h +++ b/Plotty.h @@ -133,10 +133,10 @@ public: splot.add(&mapOutlineDrywall); splot.add(&mapOutlineGlass); - splot.add(&particles); particles.setPointSize(0.33); particles.setColorHex("#0000ff"); + splot.add(&particles); particles.setPointSize(0.20); particles.setColorHex("#777777"); - splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#0000ff"); - splot.add(&pathEst); + splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#000000"); + splot.add(&pathEst); pathEst.setLineWidth(2); pathEst.setColorHex("#0000ff"); splot.add(&points); points.setPointType(7); @@ -148,6 +148,13 @@ public: } + void setGroundTruth(const Point3 pos_m) { + gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n"; + } + void setCurEst(const Point3 pos_m) { + gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n"; + } + void setPaletteRedBlue() { diff --git a/Settings.h b/Settings.h index bef3f1f..8776682 100644 --- a/Settings.h +++ b/Settings.h @@ -8,6 +8,7 @@ namespace Settings { const std::string pathData = "/apps/android/workspace/OTHER2017/data/"; const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/"; + const std::string pathWiFi = "/apps/android/workspace/OTHER2017/data/wifi/"; const std::string path1a = pathWalks + "path1/1490208103510.csv"; const std::string path1b = pathWalks + "path1/1490208519267.csv"; @@ -15,8 +16,14 @@ namespace Settings { const std::string path2a = pathWalks + "path2/1490208940015.csv"; const std::string path2b = pathWalks + "path2/1490209320343.csv"; + const std::string wifiAllFixed = pathWiFi + "allFixed.xml"; + const std::string wifiAllOptPar = pathWiFi + "allOptPar.xml"; + const std::string wifiEachOptPar = pathWiFi + "eachOptPar.xml"; + const std::string wifiEachOptParPos = pathWiFi + "eachOptParPos.xml"; - const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL37.xml"; + + + const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38.xml"; const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat"; const std::string fPathGFX = "/apps/android/workspace/OTHER2017/tex/gfx/"; @@ -26,9 +33,10 @@ namespace Settings { float smartphoneAboveGround = 1.3; namespace IMU { - const float turnSigma = 1.5 + 1.5;//1.5; - const float stepLength = 0.80; + const float turnSigma = 1.5 + 0.5;//1.5; + const float stepLength = 0.85; const float stepSigma = 0.40; + const float absHeadSigma = 90; } namespace Grid { diff --git a/main.cpp b/main.cpp index 0b0ccc9..e264848 100644 --- a/main.cpp +++ b/main.cpp @@ -22,10 +22,10 @@ #include "EvalCompareOpt.h" #include "EvalApOpt.h" -#include "EvalWalk.h" #include "EvalData.h" #include "EvalWiFi.h" #include "EvalWiFiSigStrength.h" +#include "pf/EvalWalk.h" #include "PlotErrFunc.h" diff --git a/EvalWalk.h b/pf/EvalWalk.h similarity index 83% rename from EvalWalk.h rename to pf/EvalWalk.h index eff861b..8848e55 100644 --- a/EvalWalk.h +++ b/pf/EvalWalk.h @@ -9,6 +9,8 @@ #include #include +#include "../PlotErrFunc.h" + #include #include "Indoor/sensors/radio/setup/WiFiOptimizer.h" @@ -30,9 +32,10 @@ #include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h" #include "Indoor/sensors/offline/FileReader.h" -#include "Helper.h" -#include "pf/PF.h" +#include "../Helper.h" +#include "PF.h" +#include #include #include @@ -47,6 +50,8 @@ class EvalWalk : public Offline::Listener { Plotty plotty; Offline::FileReader reader; + Offline::FilePlayer player; + Offline::FileReader::GroundTruth groundTruthLive; Timestamp lastTransition; @@ -79,6 +84,7 @@ public: // build the grid GridFactory gf(*grid); gf.build(map); + Importance::addImportance(*grid); std::ofstream out(saveFile, std::ofstream::binary); grid->write(out); out.close(); @@ -91,6 +97,9 @@ public: std::unique_ptr eval = std::unique_ptr( new PFEval(grid, wifiModel) ); pf->setEvaluation( std::move(eval) ); + wifiModel.saveXML("/tmp/test.xml"); + wifiModel.loadXML("/tmp/test.xml"); + // resampling step? pf->setNEffThreshold(0.5); //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); @@ -115,9 +124,14 @@ public: //GridWalkSimpleControl* walk = new GridWalkSimpleControl(); pf->setTransition( std::unique_ptr( new PFTrans(grid)) ); - reader.open(path, this); - + reader.open(path); + groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1); + player.setReader(&reader); + player.setListener(this); + player.start(); + // wait for completion + player.join(); } @@ -158,8 +172,12 @@ public: } virtual void onCompass(const Timestamp ts, const CompassData data) override { - const float newAzimuth = - data.azimuth + M_PI/2; // oriented towards north for our map - curCtrl.compass_azimuth = curCtrl.compass_azimuth * 0.99 + newAzimuth * 0.01; + const float newAzimuth =- data.azimuth + M_PI/2; // oriented towards north for our map + const float newAzimuth_safe = Angle::makeSafe_2PI(newAzimuth); + const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe); + curCtrl.compassAzimuth_rad += diff * 0.01; + curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad); + //curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01; } @@ -206,9 +224,14 @@ private: + K::Statistics statsErr; + + /** perform a filter-update (called from a background-loop) */ void filterUpdate() { + static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}"); + std::cout << "update" << std::endl; MyControl ctrlCopy = curCtrl; @@ -216,17 +239,20 @@ private: //lastEst = curEst; curEst = pf->update(&curCtrl, curObs); - //Log::add("Nav", "cur est: " + curEst.position.asString()); - // inform listeners about the new estimation -// for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());} + const Point3 curGT = groundTruthLive.get(lastTransition); - // update estimated path - //estPath.push_back(curEst.position.inMeter()); - -// PFTrans* trans = (PFTrans*)pf->getTransition(); + plotty.setCurEst(curEst.position.inMeter()); + plotty.setGroundTruth(curGT); + // error between ground-truth and estimation + const float estRealErr = curEst.position.inMeter().getDistance(curGT); + statsErr.add(estRealErr); + pef.clear(); + pef.add("", statsErr); + pef.plot(); + std::cout << statsErr.asString() << std::endl; const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm); plotty.pathEst.add(p3/100); @@ -254,7 +280,7 @@ private: Point2 dir(std::cos(absHead), std::sin(absHead)); Point2 arr = cen + dir * 0.1; plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n"; - dir = Point2(std::cos(ctrlCopy.compass_azimuth), std::sin(ctrlCopy.compass_azimuth)); + dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad)); arr = cen + dir * 0.05; plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n"; } diff --git a/pf/PF.h b/pf/PF.h index 59b40f2..43fae3b 100644 --- a/pf/PF.h +++ b/pf/PF.h @@ -21,6 +21,7 @@ #include #include #include +#include #include "../Settings.h" @@ -108,7 +109,7 @@ struct MyControl { int numStepsSinceLastTransition = 0; /** absolute heading in radians */ - float compass_azimuth = 0; + float compassAzimuth_rad = 0; // TODO: switch to a general activity enum/detector using barometer + accelerometer? /** currently detected activity */ @@ -163,19 +164,24 @@ public: GridWalker walker; WalkModuleFavorZ modFavorZ; - WalkModuleHeadingControl modHeading; WalkModuleNodeImportance modImportance; WalkModuleFollowDestination modDestination; WalkModuleActivityControl modActivity; + WalkModuleHeadingControl modRelHead; + WalkModuleAbsoluteHeadingControl modAbsHead; + std::minstd_rand gen; public: - PFTrans(Grid* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl) { + PFTrans(Grid* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) { + + + walker.addModule(&modRelHead); + walker.addModule(&modAbsHead); //walker.addModule(&modFavorZ); - walker.addModule(&modHeading); //walker.addModule(&modImportance); //walker.addModule(&modActivity);