added code for advanced sample impoverishment using the mesh

navMesh/NavMeshFactory.h

@@ -140,13 +140,13 @@ namespace NM {
 						// line-obstacles
 						Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
 						if (line != nullptr) {
-							nmPoly.remove(getPolygon(line));
+                            nmPoly.remove(getPolygon(line));
 						}
 
 						// object-obstacles
 						Floorplan::FloorObstacleObject* obj = dynamic_cast<Floorplan::FloorObstacleObject*>(obs);
 						if (obj != nullptr) {
-							nmPoly.remove(getPolygon(obj));
+                            nmPoly.remove(getPolygon(obj));
 						}
 
 					}

navMesh/walk/NavMeshWalkKLD.h

@@ -0,0 +1,176 @@
+#ifndef NAVMESHWALKKLD_H
+#define NAVMESHWALKKLD_H
+
+#include <vector>
+#include <algorithm>
+#include <random>
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+    /**
+     * simple walker that gives particles a slight chance to walk
+     * thru walls depending upon a given delta
+     */
+    template <typename Tria> class NavMeshWalkKLD {
+
+    private:
+
+        const NavMesh<Tria>& mesh;
+
+        std::vector<NavMeshWalkEval<Tria>*> evals;
+        Distribution::Uniform<float> distNewOne = Distribution::Uniform<float>(0,1);
+
+        int hits = 0;
+        int misses = 0;
+        int walls = 0;
+        int noTria = 0;
+
+    public:
+
+
+        /** single result */
+        struct ResultEntry {
+            NavMeshLocation<Tria> location;
+            Heading heading;
+            double probability;
+            ResultEntry() : heading(0) {;}
+        };
+
+        /** list of results */
+        using ResultList = std::vector<ResultEntry>;
+
+    public:
+
+        /** ctor */
+        NavMeshWalkKLD(const NavMesh<Tria>& mesh) : mesh(mesh) {
+
+        }
+
+        /** add a new evaluator to the walker */
+        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+            this->evals.push_back(eval);
+        }
+
+        /**
+         * @brief getOne
+         * @param params - of the current sample
+         * @param kld - the kullback-leibler divergence
+         * @param lambda - some value to influence kld and wifi
+         * @param qualityWifi - current wifi quality between [0,1]
+         * @return a newly drawn sample based on the previous sample or random
+         */
+        ResultEntry getOne(const NavMeshWalkParams<Tria>& params, const double kld, const double lambda, const double qualityWifi) {
+
+            // sanity checks
+            params.check();
+
+            ResultEntry re;
+
+            // get kld between [0,1] the lower this value, the more we need to
+            // increase the reachable radius and number of particles walking tru walls
+            const double wallProb = 1 - std::exp(-lambda * (kld * qualityWifi));
+
+            // to-be-walked distance;
+            const float toBeWalkedDist = params.getToBeWalkedDistance();
+            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+            const float toBeWalkedDistKld = (kld * qualityWifi * 0.5);
+
+            // construct reachable region
+            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD...
+
+            // get the to-be-reached destination's position (using start+distance+heading)
+            const Point2 dir = params.heading.asVector();
+            const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+            //3D Destination for finding the location within the complete mesh
+            //TODO: better solution for z? seems to be a hack on stairs
+            const Point2 dsttt = params.start.pos.xy() + (dir * (toBeWalkedDistSafe + 1.0));
+            const Point3 dst3D = Point3(dsttt.x, dsttt.y, params.start.pos.z);
+
+            const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+            // is above destination reachable?
+            if (dstTria) {
+
+                re.heading = params.heading;                    // heading was OK -> keep
+                re.location.pos = dstTria->toPoint3(dst);       // new destination position
+                re.location.tria = dstTria;                     // new destination triangle
+                ++hits;
+            }
+            //give the particle a slight chance to walk thru the wall
+            else if(distNewOne.draw() < wallProb){
+
+                try{
+                    //check if there is a triangle for dst behind the wall
+                    const Tria* dstTriaBehindWall = mesh.getLocation(dst3D).tria;
+                    re.heading = params.heading;
+                    re.location.pos = dst3D;
+                    re.location.tria = dstTriaBehindWall;
+                    ++walls;
+                    ++misses;
+
+                } catch (...) {
+                    NavMeshRandom<Tria> rnd = reachable.getRandom();					// random-helper
+                    re.location = rnd.draw();											// get a random destination
+                    re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
+                    ++noTria;
+                    ++misses;
+                }
+
+                ++misses;
+            } else {
+
+                NavMeshRandom<Tria> rnd = reachable.getRandom();					// random-helper
+                re.location = rnd.draw();											// get a random destianation
+                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
+
+                ++misses;
+            }
+
+            const int total = (hits + misses);
+            const int totalWalls = (walls + noTria);
+            if (total % 10000 == 0) {
+                //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+                //std::cout << "walls: " << (walls*100/totalWalls) << "%" << std::endl;
+            }
+
+            // calculate probability
+            const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+            re.probability = 1.0;
+            for (const NavMeshWalkEval<Tria>* eval : evals) {
+                const double p1 = eval->getProbability(pwalk);
+                re.probability *= p1;
+            }
+
+            // done
+            return re;
+
+        }
+
+        ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            return {getOne(params)};
+
+        }
+
+        const NavMesh<Tria>& getMesh(){
+            return mesh;
+        }
+
+    };
+
+}
+
+
+#endif // NAVMESHWALKKLD_H

navMesh/walk/NavMeshWalkWifi.h

@@ -0,0 +1,152 @@
+#ifndef NAVMESHWALKWIFI_H
+#define NAVMESHWALKWIFI_H
+
+#include <vector>
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+    /** simple walker extended by a wifi anti impoverishment method
+     * draw 10000 random particles within the building equaly
+     * evaluate them with wifi and then draw cumulative from a list
+     * instead of killing the particles who walk against walls
+     * the number of new particles is restricted by ??? */
+    template <typename Tria> class NavMeshWalkWifi {
+
+    public:
+        /** single result */
+        struct ResultEntry {
+            NavMeshLocation<Tria> location;
+            Heading heading;
+            double probability;
+            ResultEntry() : heading(0) {;}
+        };
+
+    private:
+
+        const NavMesh<Tria>& mesh;
+
+        std::vector<NavMeshWalkEval<Tria>*> evals;
+
+        std::vector<ResultEntry> wifiSamples;
+        WiFiModel& wifiModel;
+        WiFiObserverFree wifiProbability;
+        double wifiCumWeight;
+        DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
+
+        int hits = 0;
+        int misses = 0;
+
+    public:
+
+        /** list of results */
+        using ResultList = std::vector<ResultEntry>;
+
+        /** ctor */
+        NavMeshWalkWifi(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
+            wifiModel(wifiModel),
+            wifiProbability(Settings::WiFiModel::sigma, wifiModel){
+
+        }
+
+        /** add a new evaluator to the walker */
+        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+            this->evals.push_back(eval);
+        }
+
+        /** update every transition step the WiFi */
+        void updateWiFi(const WiFiMeasurements& wifiObs, Timestamp currentTime){
+
+            this->wifiSamplesDrawList.reset();
+
+            //todo: restrict this to specific region
+            NavMeshRandom<Tria> rnd = mesh.getRandom();
+            wifiCumWeight = 0;
+            for(int i = 0; i < 10000; ++i){
+
+                NavMeshLocation<Tria> tmpLocation = rnd.draw();
+                double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
+
+                this->wifiSamplesDrawList.add(tmpLocation, weight);
+            }
+        }
+
+        ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            ResultEntry re;
+
+            // to-be-walked distance;
+            const float toBeWalkedDist = params.getToBeWalkedDistance();
+            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+
+            // construct reachable region
+            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+            // get the to-be-reached destination's position (using start+distance+heading)
+            const Point2 dir = params.heading.asVector();
+            const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+            const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+            // is above destination reachable?
+            if (dstTria) {
+
+                re.heading = params.heading;										// heading was OK -> keep
+                re.location.pos = dstTria->toPoint3(dst);							// new destination position
+                re.location.tria = dstTria;											// new destination triangle
+                re.probability = 1.0;
+                ++hits;
+
+            } else {
+                re.location = wifiSamplesDrawList.get();
+                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
+                re.probability = 0.1;
+                ++misses;
+            }
+
+            const int total = (hits + misses);
+            if (total % 10000 == 0) {
+                //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+            }
+
+            // calculate probability
+            /*
+            const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+            re.probability = 1.0;
+            for (const NavMeshWalkEval<Tria>* eval : evals) {
+                const double p1 = eval->getProbability(pwalk);
+                re.probability *= p1;
+            }
+            */
+
+            // done
+            return re;
+
+        }
+
+        ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            return {getOne(params)};
+        }
+    };
+
+}
+
+#endif // NAVMESHWALKWIFI_H

navMesh/walk/NavMeshWalkWifiRegional.h

@@ -0,0 +1,153 @@
+#ifndef NAVMESHWALKWIFIREGIONAL_H
+#define NAVMESHWALKWIFIREGIONAL_H
+
+#include <vector>
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+    /** simple walker extended by a wifi anti impoverishment method
+     * draw 10000 random particles within a specific region arround
+     * the last estimation equaly.
+     * evaluate them with wifi and then draw cumulative from a list
+     * instead of killing the particles who walk against walls
+     * the number of new particles is restricted by ??? */
+    template <typename Tria> class NavMeshWalkWifiRegional {
+
+    public:
+        /** single result */
+        struct ResultEntry {
+            NavMeshLocation<Tria> location;
+            Heading heading;
+            double probability;
+            ResultEntry() : heading(0) {;}
+        };
+
+    private:
+
+        const NavMesh<Tria>& mesh;
+
+        std::vector<NavMeshWalkEval<Tria>*> evals;
+
+        std::vector<ResultEntry> wifiSamples;
+        WiFiModel& wifiModel;
+        WiFiObserverFree wifiProbability;
+        double wifiCumWeight;
+        DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
+
+        int hits = 0;
+        int misses = 0;
+
+    public:
+
+        /** list of results */
+        using ResultList = std::vector<ResultEntry>;
+
+        /** ctor */
+        NavMeshWalkWifiRegional(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
+            wifiModel(wifiModel),
+            wifiProbability(Settings::WiFiModel::sigma, wifiModel){
+
+        }
+
+        /** add a new evaluator to the walker */
+        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+            this->evals.push_back(eval);
+        }
+
+        /** update every transition step the WiFi */
+        void updateWiFi(const WiFiMeasurements& wifiObs, const Timestamp currentTime, const Point3 lastEst){
+
+            this->wifiSamplesDrawList.reset();
+
+            //todo: restrict this to specific region
+            NavMeshRandom<Tria> rnd = mesh.getRandom();
+            wifiCumWeight = 0;
+            for(int i = 0; i < 10000; ++i){
+
+                NavMeshLocation<Tria> tmpLocation = rnd.drawWithin(lastEst, 10.0);
+                double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
+
+                this->wifiSamplesDrawList.add(tmpLocation, weight);
+            }
+        }
+
+        ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            ResultEntry re;
+
+            // to-be-walked distance;
+            const float toBeWalkedDist = params.getToBeWalkedDistance();
+            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+
+            // construct reachable region
+            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+            // get the to-be-reached destination's position (using start+distance+heading)
+            const Point2 dir = params.heading.asVector();
+            const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+            const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+            // is above destination reachable?
+            if (dstTria) {
+
+                re.heading = params.heading;										// heading was OK -> keep
+                re.location.pos = dstTria->toPoint3(dst);							// new destination position
+                re.location.tria = dstTria;											// new destination triangle
+                re.probability = 1.0;
+                ++hits;
+
+            } else {
+                re.location = wifiSamplesDrawList.get();
+                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
+                re.probability = 0.1;
+                ++misses;
+            }
+
+            const int total = (hits + misses);
+            if (total % 10000 == 0) {
+                //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+            }
+
+            // calculate probability
+            /*
+            const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+            re.probability = 1.0;
+            for (const NavMeshWalkEval<Tria>* eval : evals) {
+                const double p1 = eval->getProbability(pwalk);
+                re.probability *= p1;
+            }
+            */
+
+            // done
+            return re;
+
+        }
+
+        ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            return {getOne(params)};
+        }
+    };
+
+}
+
+#endif // NAVMESHWALKWIFIREGIONAL_H