code for journal presentation

CMakeLists.txt

@@ -59,7 +59,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-        #-O2
+        -O2
 	-march=native
 
 	-DWITH_TESTS

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<Point3> 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() {
 

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"

main.cpp

@@ -93,8 +93,8 @@ Stats::Statistics<float> 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 srcPath2(62, 38, 1.8);
-    //const Point3 srcPath3(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<float> run(Settings::DataSetup setup, int numFile, std::string
 
     // particle-filter
     const int numParticles = 5000;
-    auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1);	// known position
+    //auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1);	// known position
-    //auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
+    auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
     auto eval = std::make_unique<MyPFEval>(WiFiModel);
-    auto trans = std::make_unique<MyPFTrans>(mesh);
+    auto trans = std::make_unique<MyPFTrans>(mesh, WiFiModel);
 
     //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
-    auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
+    //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
+    auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
 
-    //auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
+    auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
     //auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
-    auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
+    //auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
 
     // setup
     MyFilter pf(numParticles, std::move(init));
@@ -154,6 +155,7 @@ Stats::Statistics<float> 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<float> 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<float> run(Settings::DataSetup setup, int numFile, std::string
             Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(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<float> 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<float> 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
+    /* plot in gp file */
-//    std::ofstream plotFile;
+    std::ofstream plotFile;
-//    plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
+    plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
-//    dbg.saveToFile(plotFile);
+    plot.saveToFile(plotFile);
-//    plotFile.close();
+    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<float> statsQuantil;
     Stats::Statistics<float> tmp;
 
-    Settings::DataSetup set = Settings::data.Path1;
+    std::string evaluationName = "museum/Path3_Museum_KLD_SimpleDistance";
-    std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
 
-    for(int i = 0; i < 1; ++i){
+    for(int i = 0; i < 100; ++i){
 
-        for(int j = 0; j < 1; ++j){
+        //TODO: in transition die distance über KLD noch einkommentieren als Test
-            tmp = run(set, j, evaluationName);
+
+//        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));
 
 }

main.h

@@ -0,0 +1,6 @@
+#ifndef NAV_MESH_MAIN_H
+#define NAV_MESH_MAIN_H
+
+
+
+#endif

navMesh/filter.h

@@ -16,13 +16,23 @@
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
+
 #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
 #include <Indoor/navMesh/walk/NavMeshWalkEval.h>
+#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
+#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
+#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
+#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
+#include <Indoor/navMesh/NavMeshRandom.h>
+
 #include <Indoor/sensors/radio/WiFiMeasurements.h>
 #include <Indoor/data/Timestamp.h>
 #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
 #include <Indoor/sensors/activity/ActivityDetector.h>
 
+#include <Indoor/math/divergence/KullbackLeibler.h>
+#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
+
 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<MyState, MyControl> {
 
-	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+    //using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+    //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
+    //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
+    using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
 	MyNavMeshWalk walker;
 
+    WiFiQualityAnalyzer analyzer;
+    WiFiObserverFree wifiProbability;
+    const double lambda =  0.0003;
+
+    SMC::ParticleFilterEstimationBoxKDE<MyState> 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<MyNavMeshTriangle>(0.04));
+        //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
-        walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
+        //walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
         //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target
 
-	}
+        estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
+
+    }
 
     void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
 
+        // walking and heading random
         Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
 		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
         Distribution::Normal<float> 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<SMC::Particle<MyState>> wifiParticles;
+        NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
+        for(int i = 0; i < 10000; ++i){
+
+            NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
+            double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
+            SMC::Particle<MyState> 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<double>::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<MyState>& 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;
 

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) {
-		;
 	}
 
 };