diff --git a/math/dsp/Convolution2D.h b/math/dsp/Convolution2D.h index e9832f2..7ce8ece 100644 --- a/math/dsp/Convolution2D.h +++ b/math/dsp/Convolution2D.h @@ -38,6 +38,11 @@ public: } + /** TODO: + * Image2D Funktion make powertwo schreiben + * die Bild zeilenweise durchgeht und mit nullen füllt und in neuen vektor speichert. + * Ggf. Nen konstruktor dafür keine Funktion? Dann pfusch ich net im original rum. + */ //calculate FFT for both diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index b434f5d..107ac5d 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -140,13 +140,13 @@ namespace NM { // line-obstacles Floorplan::FloorObstacleLine* line = dynamic_cast(obs); if (line != nullptr) { - nmPoly.remove(getPolygon(line)); + nmPoly.remove(getPolygon(line)); } // object-obstacles Floorplan::FloorObstacleObject* obj = dynamic_cast(obs); if (obj != nullptr) { - nmPoly.remove(getPolygon(obj)); + nmPoly.remove(getPolygon(obj)); } } diff --git a/navMesh/walk/NavMeshWalkKLD.h b/navMesh/walk/NavMeshWalkKLD.h new file mode 100644 index 0000000..2490e9b --- /dev/null +++ b/navMesh/walk/NavMeshWalkKLD.h @@ -0,0 +1,176 @@ +#ifndef NAVMESHWALKKLD_H +#define NAVMESHWALKKLD_H + +#include +#include +#include + +#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 class NavMeshWalkKLD { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + Distribution::Uniform distNewOne = Distribution::Uniform(0,1); + + int hits = 0; + int misses = 0; + int walls = 0; + int noTria = 0; + + public: + + + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + /** list of results */ + using ResultList = std::vector; + + public: + + /** ctor */ + NavMeshWalkKLD(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* 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& 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 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 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 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 pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + return {getOne(params)}; + + } + + const NavMesh& getMesh(){ + return mesh; + } + + }; + +} + + +#endif // NAVMESHWALKKLD_H diff --git a/navMesh/walk/NavMeshWalkWifi.h b/navMesh/walk/NavMeshWalkWifi.h new file mode 100644 index 0000000..7a2f63f --- /dev/null +++ b/navMesh/walk/NavMeshWalkWifi.h @@ -0,0 +1,152 @@ +#ifndef NAVMESHWALKWIFI_H +#define NAVMESHWALKWIFI_H + +#include + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "../../sensors/radio/WiFiMeasurements.h" +#include +#include + +#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 class NavMeshWalkWifi { + + public: + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + std::vector wifiSamples; + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; + double wifiCumWeight; + DrawList> wifiSamplesDrawList; + + int hits = 0; + int misses = 0; + + public: + + /** list of results */ + using ResultList = std::vector; + + /** ctor */ + NavMeshWalkWifi(const NavMesh& mesh, WiFiModel& wifiModel) : mesh(mesh), + wifiModel(wifiModel), + wifiProbability(Settings::WiFiModel::sigma, wifiModel){ + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* 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 rnd = mesh.getRandom(); + wifiCumWeight = 0; + for(int i = 0; i < 10000; ++i){ + + NavMeshLocation tmpLocation = rnd.draw(); + double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs); + + this->wifiSamplesDrawList.add(tmpLocation, weight); + } + } + + ResultEntry getOne(const NavMeshWalkParams& 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 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 pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + */ + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + return {getOne(params)}; + } + }; + +} + +#endif // NAVMESHWALKWIFI_H diff --git a/navMesh/walk/NavMeshWalkWifiRegional.h b/navMesh/walk/NavMeshWalkWifiRegional.h new file mode 100644 index 0000000..a148ca5 --- /dev/null +++ b/navMesh/walk/NavMeshWalkWifiRegional.h @@ -0,0 +1,153 @@ +#ifndef NAVMESHWALKWIFIREGIONAL_H +#define NAVMESHWALKWIFIREGIONAL_H + +#include + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "../../sensors/radio/WiFiMeasurements.h" +#include +#include + +#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 class NavMeshWalkWifiRegional { + + public: + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + std::vector wifiSamples; + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; + double wifiCumWeight; + DrawList> wifiSamplesDrawList; + + int hits = 0; + int misses = 0; + + public: + + /** list of results */ + using ResultList = std::vector; + + /** ctor */ + NavMeshWalkWifiRegional(const NavMesh& mesh, WiFiModel& wifiModel) : mesh(mesh), + wifiModel(wifiModel), + wifiProbability(Settings::WiFiModel::sigma, wifiModel){ + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* 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 rnd = mesh.getRandom(); + wifiCumWeight = 0; + for(int i = 0; i < 10000; ++i){ + + NavMeshLocation tmpLocation = rnd.drawWithin(lastEst, 10.0); + double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs); + + this->wifiSamplesDrawList.add(tmpLocation, weight); + } + } + + ResultEntry getOne(const NavMeshWalkParams& 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 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 pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + */ + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + return {getOne(params)}; + } + }; + +} + +#endif // NAVMESHWALKWIFIREGIONAL_H diff --git a/sensors/activity/ActivityDetector.h b/sensors/activity/ActivityDetector.h index 6c066de..e5d4ea2 100644 --- a/sensors/activity/ActivityDetector.h +++ b/sensors/activity/ActivityDetector.h @@ -141,7 +141,7 @@ private: #endif if (std::abs(delta_hPa) < 0.042) { - current = Activity::WALKING; + current = Activity::WALKING; return; } else if (delta_hPa > 0) { current = Activity::WALKING_DOWN; diff --git a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h index e115656..f247403 100644 --- a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h +++ b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h @@ -12,8 +12,10 @@ #include "../../../math/boxkde/Image2D.h" #include "../../../math/boxkde/BoxGaus.h" #include "../../../math/boxkde/Grid2D.h" -#include "../../../grid/Grid.h"; -#include "../../../floorplan/v2/FloorplanHelper.h"; +#include "../../../grid/Grid.h" +#include "../../../floorplan/v2/FloorplanHelper.h" + +#include "../../../navMesh/NavMesh.h" namespace SMC { @@ -36,6 +38,10 @@ namespace SMC { public: + ParticleFilterEstimationBoxKDE(){ + //fuck off + } + ParticleFilterEstimationBoxKDE(const Floorplan::IndoorMap* map, const float gridsize_m, const Point2 bandwith){ const Point3 maxBB = FloorplanHelper::getBBox(map).getMax(); @@ -50,6 +56,20 @@ namespace SMC { this->bandwith = bandwith; } + template ParticleFilterEstimationBoxKDE(const NM::NavMesh* mesh, const float gridsize_m, const Point2 bandwith){ + + const Point3 maxBB = mesh->getBBox().getMax(); + const Point3 minBB = mesh->getBBox().getMin(); + this->bb = BoundingBox(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10); + + // Create histogram + size_t nBinsX = static_cast((maxBB.x - minBB.x) / gridsize_m); + size_t nBinsY = static_cast((maxBB.y - minBB.y) / gridsize_m); + this->grid = Grid2D(bb, nBinsX, nBinsY); + + this->bandwith = bandwith; + } + State estimate(const std::vector>& particles) override { // compile-time sanity checks diff --git a/smc/filtering/resampling/ParticleFilterResamplingSignalStrengthOnMesh.h b/smc/filtering/resampling/ParticleFilterResamplingSignalStrengthOnMesh.h new file mode 100644 index 0000000..b96e500 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingSignalStrengthOnMesh.h @@ -0,0 +1,109 @@ +#ifndef PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H +#define PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include "../../../navMesh/NavMeshRandom.h" +#include "../../../navMesh/walk/NavMeshSub.h" + +namespace SMC { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight. + * O(log(n)) per particle + */ + template + class ParticleFilterResamplingSignalStrengthOnMesh: public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + public: + + /** ctor */ + ParticleFilterResamplingSimpleImpoverishment() { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // randomness for drawing particles + std::uniform_real_distribution distNewOne(0.0, 1.0); + + // now draw from the copy vector and fill the original one + // with the resampled particle-set + for (uint32_t i = 0; i < cnt; ++i) { + + // slight chance to get a truely particle in range X m + if (distNewOne(gen) < 0.001) { + const NM::NavMeshSub reachable(particlesCopy[i].state.pos, 10.0); + particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0); + particles[i].weight = equalWeight; + continue; + } + + particles[i] = draw(cumWeight); + particles[i].weight = equalWeight; + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + + }; + + +} + +#endif // PARTICLEFILTERRESAMPLINGSIGNALSTRENGTHONMESH_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimple.h b/smc/filtering/resampling/ParticleFilterResamplingSimple.h index 4a2a9bf..476a6a0 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingSimple.h +++ b/smc/filtering/resampling/ParticleFilterResamplingSimple.h @@ -89,9 +89,6 @@ namespace SMC { return *it; } - - - }; diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h index cf901f0..aec9329 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h +++ b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h @@ -74,7 +74,7 @@ namespace SMC { // with the resampled particle-set for (uint32_t i = 0; i < cnt; ++i) { - // slight chance to get a truely particle in range 25m + // slight chance to get a truely particle in range X m if (distNewOne(gen) < 0.001) { const NM::NavMeshSub reachable(particlesCopy[i].state.pos, 10.0); particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0); diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index 4ccc10d..2f43454 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/wifi/estimate/ray3/ModelFactory.h @@ -22,7 +22,7 @@ namespace Ray3D { public: bool exportCeilings = true; - bool exportObstacles = true; + bool exportObstacles = true; bool exportStairs = true; bool fancyStairs = true; bool exportHandrails = true;