refactoring, easy debugging

code/Settings.h

@@ -7,6 +7,8 @@
 
 namespace Settings {
 
+    const bool useKLB = true;
+
     const int numParticles = 10000;
 
 	namespace IMU {

code/filter/Logic.h

@@ -14,6 +14,7 @@
 #include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
 #include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
 #include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
 
 #include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
 #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
@@ -46,6 +47,7 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
 			p.state.heading.direction = (rand() % 360) / 180.0 * M_PI;		// random heading
 			p.state.heading.error = 0;
             p.state.relativePressure = 0;                                   // start with a relative pressure of 0
+            p.weight = 1.0 / particles.size();                              // equal weight
 
 		}
 	}
@@ -75,7 +77,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
             p.state.heading.direction = headingDeg / 180.0 * M_PI;		// fixed heading
             p.state.heading.error = 0;
             p.state.relativePressure = 0;                               // start with a relative pressure of 0
-
+            p.weight = 1.0 / particles.size();						// equal weight
         }
     }
 
@@ -96,6 +98,8 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
 	WalkModuleFavorZ<MyNode, MyState> modFavorZ;
     //WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
 
+    //WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
+
     std::minstd_rand gen;
 
 
@@ -104,7 +108,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
         walker.addModule(&modHead);
         //walker.addModule(&modHeadMises);
         //walker.addModule(&modSpread);				// might help in some situations! keep in mind!
-
+        //walker.addModule(&modActivity);
         //walker.addModule(&modHeadUgly);
         //walker.addModule(&modImportance);
         //walker.addModule(&modFavorZ);
@@ -178,18 +182,45 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
         return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
     }
 
+    double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
+
+        const float kappa = 0.75;
+
+        const MyNode& gn = grid.getNodeFor(p.state.position);
+        switch (act) {
+
+        case ActivityButterPressure::Activity::STAY:
+            if (gn.getType() == GridNode::TYPE_FLOOR)		{return kappa;}
+            if (gn.getType() == GridNode::TYPE_DOOR)		{return kappa;}
+                                                            {return 1-kappa;}
+
+        case ActivityButterPressure::Activity::UP:
+        case ActivityButterPressure::Activity::DOWN:
+            if (gn.getType() == GridNode::TYPE_STAIR)		{return kappa;}
+            if (gn.getType() == GridNode::TYPE_ELEVATOR)	{return kappa;}
+                                                            {return 1-kappa;}
+
+        }
+
+        return 1.0;
+
+    }
+
 	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
 
 		double sum = 0;
         const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
 
-		for (K::Particle<MyState>& p : particles) {
+        #pragma omp parallel for num_threads(3)
+        for (int i = 0; i < Settings::numParticles; ++i) {
+            K::Particle<MyState>& p = particles[i];
 
             // Point3 pos_m = p.state.position.inMeter();
             // Point3 posOld_m = p.state.positionOld.inMeter();
 
             const double pWifi = getWIFI(observation, wifiObs, p.state.position);
-            const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
+            const double pBaroPressure = getStairProb(p, observation.activity);
+            //const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
             //const double pBeacon = getBEACON(observation, p.state.position);
 
             //small checks
@@ -199,6 +230,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
             const double prob = pBaroPressure * pWifi;
 
             p.weight = prob;
+
+            #pragma omp atomic
             sum += (prob);
 
 		}

code/filter/Structs.h

@@ -18,6 +18,8 @@
 #include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
 #include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
 
+#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
+
 struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
 
 	static Floorplan::IndoorMap* map;
@@ -77,6 +79,9 @@ struct MyObs {
     /** wifi measurements */
     WiFiMeasurements wifi;
 
+    /** detected activity */
+    ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
+
     /** beacon measurements */
     BeaconMeasurements beacons;
 

code/main.cpp

@@ -170,11 +170,13 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
 
     //resampling
-    //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
+    if(Settings::useKLB){
-    //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
+        pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
-    //pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
+    } else {
-    pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
+        //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
-
+        pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
+        //pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
+    }
 
     pf.setNEffThreshold(0.95);
 
@@ -190,6 +192,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     StepDetection sd;
     TurnDetection td;
     MotionDetection md;
+    ActivityButterPressure act;
 
     RelativePressure relBaro;	relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
 
@@ -236,6 +239,10 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             obs.relativePressure = relBaro.getPressureRealtiveToStart();
             obs.sigmaPressure = relBaro.getSigma();
 
+            //activity recognition
+            obs.activity = act.add(ts, fr.getBarometer()[e.idx].data);
+            //activity for transition
+
         } else if (e.type == Offline::Sensor::LIN_ACC) {
             md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
 
@@ -249,31 +256,39 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
 
             obs.currentTime = ts;
 
-            const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
+            MyState est;
+            if(Settings::useKLB){
 
-            std::vector<MyNode> allNodes = grid.getNodes();
+                const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
-            std::vector<K::Particle<MyState>> particleWifi;
+
-            for(MyNode node : allNodes){
+                std::vector<MyNode> allNodes = grid.getNodes();
-                double prob = wiFiProbability.getProbability(node, ts, wifiObs);
+                std::vector<K::Particle<MyState>> particleWifi;
-                K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
+                for(MyNode node : allNodes){
-                particleWifi.push_back(tmp);
+                    double prob = wiFiProbability.getProbability(node, ts, wifiObs);
+                    K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
+                    particleWifi.push_back(tmp);
+                }
+
+                if(kld_data.empty()){
+                    kld_data.push_back(0.0);
+                }
+
+                double kld = 0.0;
+
+                //set probability distributions.
+                //std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
+                std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
+
+                //update filter
+                est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
+
+                kld_data.push_back(kld);
+            } else {
+                est = pf.update(&ctrl, obs);
             }
 
-            if(kld_data.empty()){
-                kld_data.push_back(0.0);
-            }
-
-            std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
-            //std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
-
-            double kld = 0.0;
-            MyState est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
             Point3 estPos = est.position.inMeter();
 
-            //double kld = getKernelDensityProbability(pf, WiFiModel, obs, grid, ts, plot);
-            //double kld = kldFromMultivariatNormal(pf, estPos, particleWifi, plot);
-            kld_data.push_back(kld);
-
             //current ground truth position
             Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
 
@@ -298,8 +313,10 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             //plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
             int minutes = static_cast<int>(ts.sec()) / 60;
             plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(ts.sec())%60) << "'\n";
-            plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld << "'\n";
+            if(Settings::useKLB){
-            //plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" <<  ctrl.barometer.act << "'\n";
+                plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n";
+            }
+            plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" <<  obs.activity << "'\n";
 
             // error between GT and estimation
             float err_m = gtPos.getDistance(estPos);
@@ -309,7 +326,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             plot.show();
             usleep(10*10);
 
-
             lastTimestamp = ts;
 
             // reset control
@@ -350,23 +366,25 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     K::GnuplotPlot plotkld;
     K::GnuplotPlotElementLines lines;
 
-    //save as screenshot
+    //save as screenshot for klb
-    std::string path = "/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
+    if(Settings::useKLB){
-    gp << "set terminal png size 1280,720\n";
+        std::string path = "/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
-    gp << "set output '" << path << "_shennendistance.png'\n";
+        gp << "set terminal png size 1280,720\n";
+        gp << "set output '" << path << "_shennendistance.png'\n";
 
-    for(int i=0; i < kld_data.size()-1; ++i){
+        for(int i=0; i < kld_data.size()-1; ++i){
 
-        K::GnuplotPoint2 p1(i, kld_data[i]);
+            K::GnuplotPoint2 p1(i, kld_data[i]);
-        K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
+            K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
 
-        lines.addSegment(p1, p2);
+            lines.addSegment(p1, p2);
+        }
+
+        plotkld.add(&lines);
+        gp.draw(plotkld);
+        gp.flush();
     }
 
-    plotkld.add(&lines);
-    gp.draw(plotkld);
-    gp.flush();
-
     std::cout << "finished" << std::endl;
     sleep(1);
 
@@ -380,7 +398,7 @@ int main(int argc, char** argv) {
     //for(int i = 0; i < 5; ++i){
         //run(data.IPIN2017, 0, "ipin2017");    // Nexus Path2
         //run(data.IPIN2017, 1, "ipin2017");
-        run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
+        //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
         run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
         run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
         run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);