strange bug... a lot of zero weight particles

CMakeLists.txt

@@ -41,8 +41,8 @@ FILE(GLOB SOURCES
 	./*/*.cpp
 	./*/*/*.cpp
 	./*/*/*/*.cpp
-	../Indoor/lib/tinyxml/tinyxml2.cpp
+	../../Indoor/lib/tinyxml/tinyxml2.cpp
-	../Indoor/lib/Recast/*.cpp
+        ../../Indoor/lib/Recast/*.cpp
 )
 
 

Settings.h

@@ -51,9 +51,11 @@ namespace Settings {
     namespace WiFiModel {
         constexpr float sigma = 8.0;
 		/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
-        constexpr float TXP = -40;
+        constexpr float TXP = -45;
-        constexpr float EXP = 2.2;
+        constexpr float EXP = 2.3;
-        constexpr float WAF = -8.0;
+        constexpr float WAF = -11.0;
+
+        const bool optimize = true;
 
         // how to perform VAP grouping. see
         // - calibration in Controller.cpp

main.cpp

@@ -1,5 +1,4 @@
 
-
 #include "navMesh/main.h"
 
 int main(int argc, char** argv) {

navMesh/filter.h

@@ -2,20 +2,26 @@
 #define NAV_MESH_FILTER_H
 
 #include "mesh.h"
+#include "../Settings.h"
 
 #include <Indoor/geo/Heading.h>
 #include <Indoor/math/Distributions.h>
 
-#include <KLib/math/filter/particles/Particle.h>
+#include <Indoor/smc/Particle.h>
-#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <Indoor/smc/filtering/ParticleFilter.h>
-#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
+#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
-#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
+#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
-#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
-#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
 
 #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
 #include <Indoor/navMesh/walk/NavMeshWalkEval.h>
 
+#include <Indoor/sensors/radio/WiFiMeasurements.h>
+#include <Indoor/data/Timestamp.h>
+
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+
 struct MyState {
 
 	/** the state's position (within the mesh) */
@@ -44,6 +50,16 @@ struct MyState {
 		return res;
 	}
 
+    float getBinValue(const int dim) const {
+        switch (dim) {
+            case 0: return this->pos.pos.x;
+            case 1: return this->pos.pos.y;
+            case 2: return this->pos.pos.z;
+            case 3: return this->heading.getRAD();
+        }
+        throw "cant find this value within the bin";
+    }
+
 };
 
 struct MyControl {
@@ -61,11 +77,19 @@ struct MyControl {
 
 struct MyObservation {
 
+    // pressure
+    float sigmaPressure = 0.10f;
+    float relativePressure = 0;
 
+    //wifi
+    WiFiMeasurements wifi;
+
+    //time
+    Timestamp currentTime;
 
 };
 
-class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {
+class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
 
 	const MyNavMesh* mesh;
 
@@ -75,12 +99,12 @@ public:
 		;
 	}
 
-	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+    virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
 
 		/** random position and heading within the mesh */
 		Distribution::Uniform<float> dHead(0, 2*M_PI);
 		MyNavMeshRandom rnd = mesh->getRandom();
-		for (K::Particle<MyState>& p : particles) {
+        for (SMC::Particle<MyState>& p : particles) {
 			p.state.pos = rnd.draw();
 			p.state.heading = dHead.draw();
 			p.weight = 1.0 / particles.size();
@@ -89,7 +113,7 @@ public:
 
 };
 
-class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {
+class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
 
 	const MyNavMesh* mesh;
 	const Point3 pos;
@@ -100,11 +124,11 @@ public:
 		;
 	}
 
-	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+    virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
 
 		/** random position and heading within the mesh */
 		Distribution::Uniform<float> dHead(0, 2*M_PI);
-		for (K::Particle<MyState>& p : particles) {
+        for (SMC::Particle<MyState>& p : particles) {
 			p.state.pos = mesh->getLocation(pos);
 			p.state.heading = dHead.draw();
 			p.weight = 1.0 / particles.size();
@@ -114,7 +138,7 @@ public:
 };
 
 
-class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
 
 	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
 	MyNavMeshWalk walker;
@@ -124,20 +148,20 @@ public:
 	MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
 
 		// 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
+        //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target
 
 	}
 
-	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+    void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
 
 		Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
 		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
 		Distribution::Normal<float> dHeading(0.0, 0.10);
 
 
-		for (K::Particle<MyState>& p : particles) {
+        for (SMC::Particle<MyState>& p : particles) {
 
 			// how to walk
 			MyNavMeshWalkParams params;
@@ -165,20 +189,38 @@ public:
 
 };
 
-class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
+
+    WiFiModel& wifiModel;
+    WiFiObserverFree wifiProbability;
 
 public:
 
-	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
+    MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
 
-		return 1.0;
+    virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
+
+        double sum = 0;
+        const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
+
+        for (SMC::Particle<MyState>& p : particles) {
+
+            double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
+
+            const double prob = pWifi;
+
+            p.weight *= prob;
+            sum += prob;
+        }
+
+        return sum;
 
 	}
 
 };
 
 
-using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;
+using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
 
 
 #endif

navMesh/main.h

@@ -3,27 +3,80 @@
 
 #include "mesh.h"
 #include "filter.h"
+#include "../Settings.h"
+
 #include <memory>
 #include <thread>
+
 #include <Indoor/floorplan/v2/FloorplanReader.h>
+#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
+#include <Indoor/sensors/offline/FileReader.h>
+#include <Indoor/geo/Heading.h>
+#include <Indoor/geo/Point2.h>
+#include <Indoor/sensors/imu/TurnDetection.h>
+#include <Indoor/sensors/imu/StepDetection.h>
+#include <Indoor/sensors/imu/MotionDetection.h>
+#include <Indoor/sensors/pressure/RelativePressure.h>
+#include <Indoor/data/Timestamp.h>
+#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
+
 
 void navMeshMain() {
 
-	std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
+    //std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
+    std::string mapFile = "../map/map42_ap.xml";
 
+    // reading file
 	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
+    Offline::FileReader fr("../measurements/museum/Pixel/Path1_2468.csv");
+    WiFiFingerprints fingerprints("../measurements/museum/Nexus/fingerprints/wifi_fp.dat");
+    const std::string wifiModelFile = "../measurements/museum/wifimodel.dat";
+    std::ifstream inp(wifiModelFile, std::ifstream::binary);
 
+    // wifi
+    WiFiModelLogDistCeiling WiFiModel(map);
+
+    // with optimization
+    if(Settings::WiFiModel::optimize){
+
+        if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
+            Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
+            WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
+            for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
+                opt.addFingerprint(fp);
+            }
+            const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
+            for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
+                const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
+                WiFiModel.addAP(ap.mac, entry);
+            }
+
+            WiFiModel.saveXML(wifiModelFile);
+        } else {
+            WiFiModel.loadXML(wifiModelFile);
+        }
+
+    } else {
+        // without optimization
+        WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
+        Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
+    }
+
+
+
+    // mesh
 	NM::NavMeshSettings set;
 	MyNavMesh mesh;
 	MyNavMeshFactory fac(&mesh, set);
 	fac.build(map);
 
-	const Point3 src(26, 43, 7.5);
+    const Point3 srcPath0(26, 43, 7.5);
+    const Point3 srcPath1(62, 38, 1.8);
 
 	// add shortest-path to destination
 	//const Point3 dst(51, 45, 1.7);
-	const Point3 dst(25, 45, 0);
+    //const Point3 dst(25, 45, 0);
-	NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
+    //NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
 
 	// debug show
 	NM::NavMeshDebug dbg;
@@ -33,12 +86,12 @@ void navMeshMain() {
 
 	// particle-filter
     const int numParticles = 1000;
-	auto init = std::make_unique<MyPFInitFixed>(&mesh, src);	// 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>();
+    auto eval = std::make_unique<MyPFEval>(WiFiModel);
 	auto trans = std::make_unique<MyPFTrans>(mesh);
-	auto resample = std::make_unique<K::ParticleFilterResamplingSimple<MyState>>();
+    auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
-	auto estimate = std::make_unique<K::ParticleFilterEstimationWeightedAverage<MyState>>();
+    auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
 
 	// setup
 	MyFilter pf(numParticles, std::move(init));
@@ -48,47 +101,83 @@ void navMeshMain() {
 	pf.setEstimation(std::move(estimate));
     pf.setNEffThreshold(1);
 
-
+    // sensors
 	MyControl ctrl;
 	MyObservation obs;
 
-	//Distribution::Uniform<float> dHead(0, 2*M_PI);
+    StepDetection sd;
-	Distribution::Normal<float> dHead(0, 0.1);
+    PoseDetection pd;
+    TurnDetection td(&pd);
+    RelativePressure relBaro;
+    relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
+    Timestamp lastTimestamp = Timestamp::fromMS(0);
+
+    // parse each sensor-value within the offline data
+    for (const Offline::Entry& e : fr.getEntries()) {
+
+        const Timestamp ts = Timestamp::fromMS(e.ts);
+
+        if (e.type == Offline::Sensor::WIFI) {
+            obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
+
+        } else if (e.type == Offline::Sensor::ACC) {
+            if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
+                ++ctrl.numStepsSinceLastEval;
+            }
+            const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
+            pd.addAccelerometer(ts, _acc.data);
+
+        } else if (e.type == Offline::Sensor::GYRO) {
+            const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
+            const float delta_gyro = td.addGyroscope(ts, _gyr.data);
+
+            ctrl.headingChangeSinceLastEval += delta_gyro;
+
+        } else if (e.type == Offline::Sensor::BARO) {
+            relBaro.add(ts, fr.getBarometer()[e.idx].data);
+            obs.relativePressure = relBaro.getPressureRealtiveToStart();
+            obs.sigmaPressure = relBaro.getSigma();
+        }
+
+        if (ts.ms() - lastTimestamp.ms() > 500 && ctrl.numStepsSinceLastEval > 0) {
+
+            obs.currentTime = ts;
+//            if(ctrl.numStepsSinceLastEval > 0){
+//                pf.updateTransitionOnly(&ctrl);
+//                ctrl.afterEval();
+//            }
+//            MyState est = pf.updateEvaluationOnly(obs);
+//            lastTimestamp = ts;
 
-	for (int i = 0; i < 10000; ++i) {
 
-		ctrl.numStepsSinceLastEval = 1;
-		ctrl.headingChangeSinceLastEval = dHead.draw();
 
             MyState est = pf.update(&ctrl, obs);
-
             ctrl.afterEval();
+            lastTimestamp = ts;
 
-		try {
+    //		try {
-			MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
+    //			MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
-			auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
+    //			auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
-			dbg.addDijkstra(path);
+    //			dbg.addDijkstra(path);
-		} catch (...) {;}
+    //		} catch (...) {;}
 
-		const int d = (i * 1) % 360;
+    //		const int d = (i * 1) % 360;
-		dbg.plot.getView().setCamera(60, d);
+    //		dbg.plot.getView().setCamera(60, d);
             dbg.showParticles(pf.getParticles());
             dbg.setCurPos(est.pos.pos);
 
             //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
             //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
 
-		std::cout << i << std::endl;
+    //		std::cout << i << std::endl;
 
             dbg.draw();
 
-
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
-
-		std::this_thread::sleep_for(std::chrono::milliseconds(5));
 
         }
 
-
+    }
 }
 
 #endif