diff --git a/CMakeLists.txt b/CMakeLists.txt index 90091d4..a081e79 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - #-O2 + -O2 -march=native -DWITH_TESTS diff --git a/Plotty.h b/Plotty.h index b6c60ae..b08b30c 100755 --- a/Plotty.h +++ b/Plotty.h @@ -167,6 +167,8 @@ public: cpoints.setPointSize(2); cpoints.setPointType(7); + splot.getView().setEnabled(false); + } void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { @@ -313,6 +315,22 @@ public: splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n"; } + void setActivity(const int act) { + + std::string activity = "Unkown"; + if(act == 0){ + activity = "Standing"; + } else if(act == 1) { + activity = "Walking"; + } else if(act == 2) { + activity = "Up"; + } else if(act == 3) { + activity = "Down"; + } + + gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\n"; + } + void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) { std::vector points = { Point3(p1.x, p1.y, p1.z), @@ -512,10 +530,17 @@ public: gp.draw(splot); stream << "set terminal x11 size 2000,1500\n"; stream << gp.getBuffer(); - stream << "pause -1\n"; + stream << "pause -1\n"; gp.flush(); } + void printOverview(const std::string& path) { + gp << "set terminal png size 2000,1500\n"; + gp << "set output '" << path << "_overview" << ".png'\n"; + gp << "set view 75,60\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + } void buildFloorplan() { diff --git a/Settings.h b/Settings.h index 20a8468..7f32e41 100644 --- a/Settings.h +++ b/Settings.h @@ -165,7 +165,7 @@ namespace Settings { dataDir + "museum/Nexus/Path3_4519.csv", dataDir + "museum/Nexus/Path3_5929.csv", dataDir + "museum/Pixel/Path3_6307.csv", - dataDir + "museum/Pixel/Path3_5451.csv", + //dataDir + "museum/Pixel/Path3_5451.csv", //geht nur bis gt-punkt 31 dataDir + "museum/Pixel/Path3_9243.csv", //dataDir + "museum/Samsung/Path3_7610.csv", //dataDir + "museum/Samsung/Path3_7819.csv" diff --git a/main.cpp b/main.cpp index 8d08200..b4731c5 100755 --- a/main.cpp +++ b/main.cpp @@ -93,8 +93,8 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string const Point3 srcPath0(26, 43, 7.5); const Point3 srcPath1(62, 38, 1.7); - //const Point3 srcPath2(62, 38, 1.8); - //const Point3 srcPath3(62, 38, 1.8); + const Point3 srcPath2(62, 38, 1.8); + const Point3 srcPath3(62, 38, 1.8); // add shortest-path to destination //const Point3 dst(51, 45, 1.7); @@ -115,17 +115,18 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string // particle-filter const int numParticles = 5000; - auto init = std::make_unique(&mesh, srcPath1); // known position - //auto init = std::make_unique(&mesh); // uniform distribution + //auto init = std::make_unique(&mesh, srcPath1); // known position + auto init = std::make_unique(&mesh); // uniform distribution auto eval = std::make_unique(WiFiModel); - auto trans = std::make_unique(mesh); + auto trans = std::make_unique(mesh, WiFiModel); //auto resample = std::make_unique>(); - auto resample = std::make_unique>(); + //auto resample = std::make_unique>(); + auto resample = std::make_unique>(); - //auto estimate = std::make_unique>(map, 0.2, Point2(1,1)); + auto estimate = std::make_unique>(map, 0.2, Point2(1,1)); //auto estimate = std::make_unique>(); - auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); @@ -154,6 +155,7 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string if (e.type == Offline::Sensor::WIFI) { obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; + ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data; } else if (e.type == Offline::Sensor::ACC) { if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { @@ -184,6 +186,8 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string if (ctrl.numStepsSinceLastEval > 0) { obs.currentTime = ts; + ctrl.currentTime = ts; + // if(ctrl.numStepsSinceLastEval > 0){ // pf.updateTransitionOnly(&ctrl); // } @@ -192,7 +196,7 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1); lastTimestamp = ts; - + ctrl.lastEstimate = est.pos.pos; //plot //dbg.showParticles(pf.getParticles()); @@ -207,22 +211,16 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string plot.setCurEst(est.pos.pos); plot.setGroundTruth(gtPos); plot.addEstimationNode(est.pos.pos); - plot.plot(); + plot.setActivity((int) act.get()); + //plot.plot(); // error calc float err_m = gtPos.getDistance(est.pos.pos); errorStats.add(err_m); - errorFile << err_m << "\n"; + errorFile << ts.ms() << " " << err_m << "\n"; //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png"); //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); - - if(ts.ms() == 13410 || ts.ms() == 20802){ - std::ofstream plotFile; - plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp"); - plot.saveToFile(plotFile); - plotFile.close(); - } } } @@ -241,14 +239,15 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n"; errorFile.close(); - //save the .gp buffer into a file -// std::ofstream plotFile; -// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp"); -// dbg.saveToFile(plotFile); -// plotFile.close(); + /* plot in gp file */ + std::ofstream plotFile; + plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); //save also a png image, just for a better overview -// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.plot(); return errorStats; } @@ -261,13 +260,38 @@ int main(int argc, char** argv) { Stats::Statistics statsQuantil; Stats::Statistics tmp; - Settings::DataSetup set = Settings::data.Path1; - std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper"; + std::string evaluationName = "museum/Path3_Museum_KLD_SimpleDistance"; - for(int i = 0; i < 1; ++i){ + for(int i = 0; i < 100; ++i){ - for(int j = 0; j < 1; ++j){ - tmp = run(set, j, evaluationName); + //TODO: in transition die distance über KLD noch einkommentieren als Test + +// for(int j = 0; j < Settings::data.Path0.training.size(); ++j){ +// tmp = run(Settings::data.Path0, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + +// for(int j = 0; j < Settings::data.Path1.training.size(); ++j){ +// tmp = run(Settings::data.Path1, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + +// for(int j = 0; j < Settings::data.Path2.training.size(); ++j){ +// tmp = run(Settings::data.Path2, j, evaluationName); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); +// } + + for(int j = 0; j < Settings::data.Path3.training.size(); ++j){ + tmp = run(Settings::data.Path3, j, evaluationName); statsMedian.add(tmp.getMedian()); statsAVG.add(tmp.getAvg()); statsSTD.add(tmp.getStdDev()); @@ -299,6 +323,6 @@ int main(int argc, char** argv) { finalStatisticFile.close(); - std::this_thread::sleep_for(std::chrono::seconds(60)); + //std::this_thread::sleep_for(std::chrono::seconds(60)); } diff --git a/main.h b/main.h new file mode 100644 index 0000000..e62a9c5 --- /dev/null +++ b/main.h @@ -0,0 +1,6 @@ +#ifndef NAV_MESH_MAIN_H +#define NAV_MESH_MAIN_H + + + +#endif diff --git a/navMesh/filter.h b/navMesh/filter.h index f7b65fe..415f000 100644 --- a/navMesh/filter.h +++ b/navMesh/filter.h @@ -16,13 +16,23 @@ #include #include #include + #include #include +#include +#include +#include +#include +#include + #include #include #include #include +#include +#include + struct MyState { /** the state's position (within the mesh) */ @@ -83,6 +93,14 @@ struct MyControl { headingChangeSinceLastEval = 0; } + //wifi + WiFiMeasurements wifi; + + //time + Timestamp currentTime; + + //last estimation + Point3 lastEstimate = Point3(26, 43, 7.5); }; @@ -154,26 +172,75 @@ public: class MyPFTrans : public SMC::ParticleFilterTransition { - using MyNavMeshWalk = NM::NavMeshWalkSimple; + //using MyNavMeshWalk = NM::NavMeshWalkSimple; + //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; + //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; + using MyNavMeshWalk = NM::NavMeshWalkKLD; MyNavMeshWalk walker; + WiFiQualityAnalyzer analyzer; + WiFiObserverFree wifiProbability; + const double lambda = 0.0003; + + SMC::ParticleFilterEstimationBoxKDE estimator; + public: - MyPFTrans(MyNavMesh& mesh) : walker(mesh) { + MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) : + walker(mesh), + wifiProbability(Settings::WiFiModel::sigma, wifiModel){ // how to evaluate drawn points - walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); - walker.addEvaluator(new NM::WalkEvalDistance(0.1)); + //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); + //walker.addEvaluator(new NM::WalkEvalDistance(0.1)); //walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target - } + estimator = SMC::ParticleFilterEstimationBoxKDE(&mesh, 0.2, Point2(1,1)); + + } void transition(std::vector>& particles, const MyControl* control) override { + // walking and heading random Distribution::Normal dStepSizeFloor(0.70, 0.1); Distribution::Normal dStepSizeStair(0.35, 0.1); Distribution::Normal dHeading(0.0, 0.1); + // wifi and quality of wifi + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi); + if(!wifiObs.entries.empty()){ + analyzer.add(wifiObs); + } + float qualityWifi = analyzer.getQuality(); + if(std::isnan(qualityWifi)){ + qualityWifi = 1.0; + } else if(qualityWifi == 0) { + qualityWifi = 0.00000001; + } + + // divergence between eval and transition + std::vector> wifiParticles; + NM::NavMeshRandom rnd = walker.getMesh().getRandom(); + for(int i = 0; i < 10000; ++i){ + + NM::NavMeshLocation tmpLocation = rnd.draw(); + double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs); + SMC::Particle tmpParticle(MyState(tmpLocation.pos), weight); + wifiParticles.push_back(tmpParticle); + } + + MyState wifiEstimate = estimator.estimate(wifiParticles); + + // fake kld + const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos); + //const double kld = Divergence::KullbackLeibler::getMultivariateGauss(normParticle, normWifi);; + + //std::cout << "KLD: " << kld << std::endl; + //std::cout << "Quality: " << qualityWifi << std::endl; + + //update wifi + //walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate); + #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { SMC::Particle& p = particles[i]; @@ -192,8 +259,11 @@ public: params.stepSizes.stepSizeStair_m = 0.1; } + double deltaUnblockable = 0.01; + // walk - MyNavMeshWalk::ResultEntry res = walker.getOne(params); + //MyNavMeshWalk::ResultEntry res = walker.getOne(params); + MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi); // assign back to particle's state p.weight *= res.probability; @@ -257,9 +327,7 @@ public: //HACK HACK HACK HACK double prob = 1.0; - if(observation.currentTime.ms() > 20801){ - prob = pWifi * pStair; - } + prob = pWifi * pStair; p.weight *= prob; diff --git a/navMesh/mesh.h b/navMesh/mesh.h index be6447e..1652081 100644 --- a/navMesh/mesh.h +++ b/navMesh/mesh.h @@ -18,7 +18,6 @@ class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangle public: MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) { - ; } };