From 625f5fe04dc8b2aac20775e712bbcd6e5b5bf536 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 12 Jul 2018 18:39:27 +0200 Subject: [PATCH] updated sensors and filter to current code version removed KLib stuff added new activity filter is uncommand! at the moment, the app is not able to load new maps and breaks using old maps --- Controller.cpp | 84 ++++++++++--- Controller.h | 2 + Settings.h | 2 +- misc/Debug.h | 2 +- nav/NavController.h | 4 +- nav/grid/Filter.h | 39 +++--- nav/grid/NavControllerGrid.cpp | 29 +++-- nav/grid/NavControllerGrid.h | 4 +- nav/grid/NodeResampling.h | 12 +- nav/grid/RegionalResampling.h | 20 +-- nav/mesh/FilterMesh.h | 207 +++++++++++++++---------------- nav/mesh/NavControllerMesh.cpp | 84 ++++++------- nav/mesh/NavControllerMesh.h | 9 +- nav/mesh/State.h | 28 +++-- sensors/ActivitySensor.h | 20 +-- sensors/SensorFactory.h | 12 +- ui/map/2D/ColorPoints2D.h | 6 +- ui/map/2D/HasSelectableNodes.h | 2 +- ui/map/2D/MapView2D.cpp | 2 +- ui/map/2D/MapView2D.h | 6 +- ui/map/3D/MapView3D.h | 4 +- ui/map/3D/elements/ColorPoints.h | 8 +- 22 files changed, 325 insertions(+), 261 deletions(-) diff --git a/Controller.cpp b/Controller.cpp index f5ff4aa..c974e83 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -20,6 +20,7 @@ #include #include +#include #include @@ -73,12 +74,12 @@ Controller::Controller() { //writer->start(file); // start the sensors - SensorFactory::get().getAccelerometer().start(); - SensorFactory::get().getGyroscope().start(); - SensorFactory::get().getBarometer().start(); - SensorFactory::get().getWiFi().start(); - SensorFactory::get().getGPS().start(); - SensorFactory::get().getCompass().start(); + SensorFactory::get().getAccelerometer().start(); + SensorFactory::get().getGyroscope().start(); + SensorFactory::get().getBarometer().start(); + SensorFactory::get().getWiFi().start(); + SensorFactory::get().getGPS().start(); + SensorFactory::get().getCompass().start(); } @@ -189,6 +190,55 @@ void buildGridOnce(Grid* grid, Floorplan::IndoorMap* map, const std: } +void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& wifiModelFile){ + + // ask questions + const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "WiFi", "Use Fingerprints for WiFiCalibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use APs from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No); + + // WiFi setup + wifiModel = new WiFiModelLogDistCeiling(map); + if (replyWiFiFP == QMessageBox::Yes) { + + std::ifstream inp(wifiModelFile, std::ifstream::binary); + + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + + Log::add("Controller", "Create new WiFiModel"); + WiFiCalibrationDataModel mdl(fpFile); + Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!"); + WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); + for (const WiFiFingerprint& fp : mdl.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); + ((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry); + } + + wifiModel->saveXML(wifiModelFile); + + } else { + + Log::add("Controller", "Use existing WifiModel"); + // load WiFiModel from file. The factory will create the correct instance + //WiFiModel->loadXML(setup.wifiModel); + WiFiModelFactory fac(map); + wifiModel = fac.loadXML(wifiModelFile); + } + + } else { + + Log::add("Controller", "Read AP from File"); + // load all APs from the floorplan and use same TXP/EXP/WAF for all of them + ((WiFiModelLogDistCeiling*)wifiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + Assert::isFalse(wifiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + } + + +} + void Controller::on3DButton() { static bool use3D = false; @@ -264,8 +314,9 @@ void Controller::loadGrid(QDir dir) { void Controller::loadNavMesh(QDir dir) { QFile fMap(dir.path() + "/map.xml"); - QFile fGrid(dir.path() + "/grid.dat"); + QFile fGrid(dir.path() + "/grid.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); + QFile fWiFiModel(dir.path() + "/wifimodel.dat"); Assert::isTrue(fMap.exists(), "map.xml missing"); @@ -274,24 +325,27 @@ void Controller::loadNavMesh(QDir dir) { im = Floorplan::Reader::readFromString(str.toStdString()); const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); + const std::string sWiFiModel = fWiFiModel.fileName().toStdString(); - // create navmesh - if (navMesh) {delete navMesh; navMesh = nullptr;} - NM::NavMeshSettings settings; - NM::NavMeshFactory fac(navMesh, settings); - fac.build(im); + buildWiFiModelOnce(wifiModel, im, sWiFiFP, sWiFiModel); + + // create navmesh + if (navMesh) {delete navMesh; navMesh = nullptr;} + NM::NavMeshSettings settings; + NM::NavMeshFactory fac(navMesh, settings); + fac.build(im); // create a new navigator if (nav) {delete nav; nav = nullptr;} - nav = new MeshBased::NavControllerMesh(this, im, navMesh); + nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); getMapView3D()->setMap(im); getMapView2D()->setMap(wifiCalib, im); - getMapView3D()->showGridImportance(grid); - getMapView2D()->showGridImportance(grid); + getMapView3D()->showGridImportance(grid); + getMapView2D()->showGridImportance(grid); getMapView3D()->setVisible(false); diff --git a/Controller.h b/Controller.h index 2833cac..9f33693 100644 --- a/Controller.h +++ b/Controller.h @@ -21,6 +21,7 @@ class MyGridNode; class InfoWidget; class MapView3D; class MapView2D; +class WiFiModel; namespace NM { template class NavMesh; @@ -78,6 +79,7 @@ private: NM::NavMesh* navMesh = nullptr; NavController* nav = nullptr; Floorplan::IndoorMap* im = nullptr; + WiFiModel* wifiModel = nullptr; }; diff --git a/Settings.h b/Settings.h index 6708529..3dd5ded 100644 --- a/Settings.h +++ b/Settings.h @@ -39,7 +39,7 @@ namespace Settings { namespace WiFiModel { - constexpr float sigma = 13.0; + constexpr float sigma = 13.0; //TODO: im Museum hatten wir 8.0 /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ constexpr float TXP = -48; diff --git a/misc/Debug.h b/misc/Debug.h index b36aa55..4283c65 100644 --- a/misc/Debug.h +++ b/misc/Debug.h @@ -9,7 +9,7 @@ public: static void error(const std::string& err) { qDebug(err.c_str()); - throw err; + throw err; } }; diff --git a/nav/NavController.h b/nav/NavController.h index 19eabcd..c831524 100644 --- a/nav/NavController.h +++ b/nav/NavController.h @@ -48,8 +48,8 @@ class NavController : public SensorListener, public SensorListener, public SensorListener, - public SensorListener - //public SensorListener + public SensorListener, + public SensorListener { diff --git a/nav/grid/Filter.h b/nav/grid/Filter.h index ab43b50..3c3caf9 100644 --- a/nav/grid/Filter.h +++ b/nav/grid/Filter.h @@ -1,13 +1,18 @@ #ifndef FILTER_H #define FILTER_H -#include +#include +#include +#include -#include -#include +#include +#include +#include -#include -#include +#include +#include +#include +#include #include #include @@ -31,7 +36,7 @@ namespace GridBased { - class PFInit : public K::ParticleFilterInitializer { + class PFInit : public SMC::ParticleFilterInitializer { private: @@ -43,14 +48,14 @@ namespace GridBased { } - virtual void initialize(std::vector>& particles) override { + virtual void initialize(std::vector>& particles) override { std::minstd_rand gen; std::uniform_int_distribution distIdx(0, grid->getNumNodes()-1); std::uniform_real_distribution distHead(0, 2*M_PI); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { const int idx = distIdx(gen); const MyGridNode& node = (*grid)[idx]; p.state.position = node; // random position @@ -59,7 +64,7 @@ namespace GridBased { } // // fix position + heading - // for (K::Particle& p : particles) { + // for (SMC::Particle& p : particles) { //// const int idx = 9000; //// const MyGridNode& node = (*grid)[idx]; // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap @@ -71,7 +76,7 @@ namespace GridBased { }; - class PFTrans : public K::ParticleFilterTransition { + class PFTrans : public SMC::ParticleFilterTransition { public: @@ -110,7 +115,7 @@ namespace GridBased { - void transition(std::vector>& particles, const MyControl* _ctrl) override { + void transition(std::vector>& particles, const MyControl* _ctrl) override { // local copy!! observation might be changed async outside!! (will really produces crashes!) this->ctrl = *_ctrl; @@ -121,14 +126,14 @@ namespace GridBased { // sanity check Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); - //for (K::Particle& p : particles) { + //for (SMC::Particle& p : particles) { #pragma omp parallel for num_threads(3) for (int i = 0; i < Settings::numParticles; ++i) { //#pragma omp atomic const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); - K::Particle& p = particles[i]; + SMC::Particle& p = particles[i]; double prob; p.state = walker.getDestination(*grid, p.state, dist_m, prob); @@ -147,7 +152,7 @@ namespace GridBased { }; - class PFEval : public K::ParticleFilterEvaluation { + class PFEval : public SMC::ParticleFilterEvaluation { Grid* grid; @@ -170,7 +175,7 @@ namespace GridBased { } - double getStairProb(const K::Particle& p, const Activity act) { + double getStairProb(const SMC::Particle& p, const Activity act) { const float kappa = 0.75; @@ -195,7 +200,7 @@ namespace GridBased { } - double evaluation(std::vector>& particles, const MyObservation& _observation) override { + double evaluation(std::vector>& particles, const MyObservation& _observation) override { double sum = 0; @@ -215,7 +220,7 @@ namespace GridBased { #pragma omp parallel for num_threads(3) for (int i = 0; i < Settings::numParticles; ++i) { - K::Particle& p = particles[i]; + SMC::Particle& p = particles[i]; // WiFi free //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); diff --git a/nav/grid/NavControllerGrid.cpp b/nav/grid/NavControllerGrid.cpp index 59a33a1..4724fb7 100644 --- a/nav/grid/NavControllerGrid.cpp +++ b/nav/grid/NavControllerGrid.cpp @@ -21,25 +21,25 @@ Q_DECLARE_METATYPE(const void*) GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { // filter init - std::unique_ptr> init(new PFInit(grid)); + std::unique_ptr> init(new PFInit(grid)); - // estimation - //std::unique_ptr> estimation(new K::ParticleFilterEstimationWeightedAverage()); - std::unique_ptr> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5)); + // estimation + //std::unique_ptr> estimation(new SMC::ParticleFilterEstimationWeightedAverage()); + std::unique_ptr> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5)); // resampling std::unique_ptr> resample(new NodeResampling(*grid)); - //std::unique_ptr> resample(new K::ParticleFilterResamplingSimple()); - //std::unique_ptr> resample(new K::ParticleFilterResamplingPercent(0.05)); + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.05)); //std::unique_ptr resample(new RegionalResampling()); // eval and transition wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - std::unique_ptr> eval(new PFEval(grid, wifiModel)); - std::unique_ptr> transition(new PFTrans(grid)); + std::unique_ptr> eval(new PFEval(grid, wifiModel)); + std::unique_ptr> transition(new PFTrans(grid)); // setup the filter - pf = std::unique_ptr>(new K::ParticleFilter(Settings::numParticles, std::move(init))); + pf = std::unique_ptr>(new SMC::ParticleFilter(Settings::numParticles, std::move(init))); pf->setTransition(std::move(transition)); pf->setEvaluation(std::move(eval)); pf->setEstimation(std::move(estimation)); @@ -56,7 +56,7 @@ GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floo SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getTurns().addListener(this); - //SensorFactory::get().getActivity().addListener(this); + SensorFactory::get().getActivity().addListener(this); } @@ -141,6 +141,15 @@ void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const gotSensorData(ts); } +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { + (void) sensor; + (void) ts; + curCtrl.activity = data.curActivity; + curObs.activity = data.curActivity; + //debugActivity(data.curActivity); + gotSensorData(ts); +} + /** called when any sensor has received new data */ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { curObs.currentTime = ts; diff --git a/nav/grid/NavControllerGrid.h b/nav/grid/NavControllerGrid.h index 5be5778..ed985c4 100644 --- a/nav/grid/NavControllerGrid.h +++ b/nav/grid/NavControllerGrid.h @@ -28,7 +28,7 @@ namespace GridBased { Grid* grid; WiFiModelLogDistCeiling wifiModel; - std::unique_ptr> pf; + std::unique_ptr> pf; DijkstraPath pathToDest; @@ -60,7 +60,7 @@ namespace GridBased { void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; - // void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; + void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; private: diff --git a/nav/grid/NodeResampling.h b/nav/grid/NodeResampling.h index fd48535..cef3ffd 100644 --- a/nav/grid/NodeResampling.h +++ b/nav/grid/NodeResampling.h @@ -6,7 +6,7 @@ #include #include -#include +#include /** @@ -16,12 +16,12 @@ * O(log(n)) per particle */ template - class NodeResampling : public K::ParticleFilterResampling { + class NodeResampling : public SMC::ParticleFilterResampling { private: /** this is a copy of the particle-set to draw from it */ - std::vector> particlesCopy; + std::vector> particlesCopy; /** random number generator */ std::minstd_rand gen; @@ -35,7 +35,7 @@ gen.seed(1234); } - void resample(std::vector>& particles) override { + void resample(std::vector>& particles) override { // compile-time sanity checks // TODO: this solution requires EXPLICIT overloading which is bad... @@ -99,7 +99,7 @@ private: /** draw one particle according to its weight from the copy vector */ - const K::Particle& draw(const double cumWeight) { + const SMC::Particle& draw(const double cumWeight) { // generate random values between [0:cumWeight] std::uniform_real_distribution dist(0, cumWeight); @@ -108,7 +108,7 @@ const float rand = dist(gen); // search comparator (cumWeight is ordered -> use binary search) - auto comp = [] (const K::Particle& s, const float d) {return s.weight < d;}; + auto comp = [] (const SMC::Particle& s, const float d) {return s.weight < d;}; auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); return *it; diff --git a/nav/grid/RegionalResampling.h b/nav/grid/RegionalResampling.h index 21a0994..12216d1 100644 --- a/nav/grid/RegionalResampling.h +++ b/nav/grid/RegionalResampling.h @@ -1,12 +1,12 @@ #ifndef REGIONALRESAMPLING_H #define REGIONALRESAMPLING_H -#include +#include #include "State.h" namespace GridBased { - class RegionalResampling : public K::ParticleFilterResampling { + class RegionalResampling : public SMC::ParticleFilterResampling { public: @@ -14,25 +14,25 @@ namespace GridBased { RegionalResampling() {;} - void resample(std::vector>& particles) override { + void resample(std::vector>& particles) override { Point3 sum; - for (const K::Particle& p : particles) { + for (const SMC::Particle& p : particles) { sum += p.state.position.inMeter(); } const Point3 avg = sum / particles.size(); - std::vector> next; - for (const K::Particle& p : particles) { + std::vector> next; + for (const SMC::Particle& p : particles) { const float dist = p.state.position.inMeter().getDistance(avg); if (rand() % 6 != 0) {continue;} if (dist < maxDist) {next.push_back(p);} } // cumulate - std::vector> copy = particles; + std::vector> copy = particles; double cumWeight = 0; - for ( K::Particle& p : copy) { + for ( SMC::Particle& p : copy) { cumWeight += p.weight; p.weight = cumWeight; } @@ -50,7 +50,7 @@ namespace GridBased { std::minstd_rand gen; /** draw one particle according to its weight from the copy vector */ - const K::Particle& draw(std::vector>& copy, const double cumWeight) { + const SMC::Particle& draw(std::vector>& copy, const double cumWeight) { // generate random values between [0:cumWeight] std::uniform_real_distribution dist(0, cumWeight); @@ -59,7 +59,7 @@ namespace GridBased { const float rand = dist(gen); // search comparator (cumWeight is ordered -> use binary search) - auto comp = [] (const K::Particle& s, const float d) {return s.weight < d;}; + auto comp = [] (const SMC::Particle& s, const float d) {return s.weight < d;}; auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); return *it; diff --git a/nav/mesh/FilterMesh.h b/nav/mesh/FilterMesh.h index fdfabbf..d181793 100644 --- a/nav/mesh/FilterMesh.h +++ b/nav/mesh/FilterMesh.h @@ -1,13 +1,18 @@ #ifndef FILTERMESH_ #define FILTERMESH_ -#include +#include +#include +#include -#include -#include +#include +#include +#include -#include -#include +#include +#include +#include +#include #include #include @@ -18,6 +23,7 @@ #include #include "State.h" +#include "../Observation.h" #include "../../Settings.h" #include @@ -25,33 +31,33 @@ namespace MeshBased { - class PFInit : public K::ParticleFilterInitializer { + class PFInit : public SMC::ParticleFilterInitializer { private: - NM::NavMesh* mesh; + const NM::NavMesh* mesh; public: - PFInit(NM::NavMesh* mesh) : mesh(mesh) { - + PFInit(const NM::NavMesh* mesh) : mesh(mesh) { + ; } - virtual void initialize(std::vector>& particles) override { + virtual void initialize(std::vector>& particles) override { std::minstd_rand gen; std::uniform_real_distribution distHead(0, 2*M_PI); NM::NavMeshRandom rnd = mesh->getRandom(); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { p.state.loc = rnd.draw(); p.state.heading = Heading(distHead(gen)); // random heading p.weight = 1.0 / particles.size(); // equal weight } // // fix position + heading - // for (K::Particle& p : particles) { + // for (SMC::Particle& p : particles) { //// const int idx = 9000; //// const MyGridNode& node = (*grid)[idx]; // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap @@ -63,107 +69,109 @@ namespace MeshBased { }; - /* - class PFTrans : public K::ParticleFilterTransition { + + class PFTrans : public SMC::ParticleFilterTransition { public: - // local, static control-data COPY + using MyNavMeshWalk = NM::NavMeshWalkSimple; + //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; + //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; + MyNavMeshWalk walker; + + // local, static control-data COPY MyControl ctrl; - Grid* grid; - GridWalker walker; - - WalkModuleFavorZ modFavorZ; - WalkModuleHeadingControl modHeading; - WalkModuleNodeImportance modImportance; - WalkModuleFollowDestination modDestination; - WalkModuleActivityControl modActivity; - - NodeResampling resampler; - - std::minstd_rand gen; - public: - PFTrans(Grid* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) { + PFTrans(NM::NavMesh* mesh) : walker(*mesh){ - //walker.addModule(&modFavorZ); - walker.addModule(&modHeading); - //walker.addModule(&modImportance); - walker.addModule(&modActivity); - - - if (Settings::destination != GridPoint(0,0,0)) { - //walker.addModule(&modDestination); - modDestination.setDestination(grid->getNodeFor(Settings::destination)); - } + // how to evaluate drawn points + //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); + //walker.addEvaluator(new NM::WalkEvalDistance(0.1)); + //walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target } - - - void transition(std::vector>& particles, const MyControl* _ctrl) override { + void transition(std::vector>& particles, const MyControl* _ctrl) override { // local copy!! observation might be changed async outside!! (will really produces crashes!) this->ctrl = *_ctrl; ((MyControl*)_ctrl)->resetAfterTransition(); - std::normal_distribution noise(0, Settings::IMU::stepSigma); + // walking and heading random + Distribution::Normal dStepSizeFloor(0.70, 0.1); + Distribution::Normal dStepSizeStair(0.35, 0.1); + Distribution::Normal dHeading(0.0, 0.1); - // sanity check - Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + #pragma omp parallel for num_threads(3) + for (int i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; - //for (K::Particle& p : particles) { - #pragma omp parallel for num_threads(3) - for (int i = 0; i < Settings::numParticles; ++i) { + // how to walk + NM::NavMeshWalkParams params; + params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw(); + params.numSteps = ctrl.numStepsSinceLastTransition; + params.start = p.state.loc; - //#pragma omp atomic - const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); + params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); + params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); - K::Particle& p = particles[i]; + if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){ + params.stepSizes.stepSizeFloor_m = 0.1; + params.stepSizes.stepSizeStair_m = 0.1; + } - double prob; - p.state = walker.getDestination(*grid, p.state, dist_m, prob); - //p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15); - //p.weight = (prob > 0.01) ? (1.0) : (0.15); - //p.weight = prob; - //p.weight = 1.0; // reset - //p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict] - //p.weight *= std::pow(prob, 0.1); // add grid-walk-probability - p.weight = prob; // grid-walk-probability - if (p.weight != p.weight) {throw Exception("nan");} + // walk + MyNavMeshWalk::ResultEntry res = walker.getOne(params); - } + // assign back to particle's state + p.weight *= res.probability; + p.state.loc = res.location; + p.state.heading = res.heading; + } } }; - class PFEval : public K::ParticleFilterEvaluation { - Grid* grid; + class PFEval : public SMC::ParticleFilterEvaluation { - WiFiModelLogDistCeiling& wifiModel; + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; + double getStairProb(const SMC::Particle& p, const Activity act) { - //WiFiObserverFree wiFiProbability; // free-calculation - WiFiObserverGrid wiFiProbability; // grid-calculation + const float kappa = 0.75; - // smartphone is 1.3 meter above ground - const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); + switch (act) { + + case Activity::WALKING: + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + {return 1-kappa;} + + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} + {return 1-kappa;} + } + + return 1.0; + } public: - PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : - grid(grid), wifiModel(wifiModel), - //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free - wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid + //TODO: Was ist hier besser? Im Museum hatten wir das unterste. + //PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){} + //PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){} + PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){} - - } - - double evaluation(std::vector>& particles, const MyObservation& _observation) override { + double evaluation(std::vector>& particles, const MyObservation& _observation) override { double sum = 0; @@ -171,50 +179,35 @@ namespace MeshBased { const MyObservation observation = _observation; // vap-grouping - const int numAP1 = observation.wifi.entries.size(); - const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi); - const int numAP2 = wifiObs.entries.size(); - - Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2)); + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); // sanity check Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); - #pragma omp parallel for num_threads(3) - for (int i = 0; i < Settings::numParticles; ++i) { + // assign weights + #pragma omp parallel for num_threads(3) + for (size_t i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; - K::Particle& p = particles[i]; + const double pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs); + const double pStair = getStairProb(p, observation.activity); + const double pGPS = 1; - // WiFi free - //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); + const double prob = pWifi * pStair * pGPS; - // WiFi grid - const MyGridNode& node = grid->getNodeFor(p.state.position); - const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs); + p.weight *= prob; + if (p.weight != p.weight) {throw Exception("nan");} + #pragma omp atomic + sum += p.weight; + } - //Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms())); - - const double pStair = getStairProb(p, observation.activity); - const double pGPS = 1; - const double prob = pWiFi * pGPS * pStair; - - p.weight *= prob; // NOTE: keeps the weight returned by the transition step! - //p.weight = prob; // does NOT keep the weights returned by the transition step - if (p.weight != p.weight) {throw Exception("nan");} - - #pragma omp atomic - sum += p.weight; - - } - - return sum; + return sum; } }; - */ } #endif // FILTERMESH_ diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp index 2f9e61b..9bb5347 100644 --- a/nav/mesh/NavControllerMesh.cpp +++ b/nav/mesh/NavControllerMesh.cpp @@ -31,37 +31,35 @@ Q_DECLARE_METATYPE(const void*) /** ctor */ -MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh) : - NavController(mainController, im), navMesh(navMesh), wifiModel(im) { +MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel) : + NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) { // filter init - std::unique_ptr> init(new MeshBased::PFInit(navMesh)); + std::unique_ptr> init(new MeshBased::PFInit(navMesh)); -// // estimation -// //std::unique_ptr> estimation(new K::ParticleFilterEstimationWeightedAverage()); -// std::unique_ptr> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5)); + // estimation + std::unique_ptr> estimation(new SMC::ParticleFilterEstimationWeightedAverage()); + //std::unique_ptr> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5)); -// // resampling -// std::unique_ptr> resample(new NodeResampling(*grid)); -// //std::unique_ptr> resample(new K::ParticleFilterResamplingSimple()); -// //std::unique_ptr> resample(new K::ParticleFilterResamplingPercent(0.05)); -// //std::unique_ptr resample(new RegionalResampling()); + // resampling + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.05)); + std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); -// // eval and transition -// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); -// std::unique_ptr> eval(new PFEval(grid, wifiModel)); -// std::unique_ptr> transition(new PFTrans(grid)); + // eval and transition + std::unique_ptr> eval(new MeshBased::PFEval(wifiModel)); + std::unique_ptr> transition(new MeshBased::PFTrans(navMesh)); -// // setup the filter -// pf = std::unique_ptr>(new K::ParticleFilter(Settings::numParticles, std::move(init))); -// pf->setTransition(std::move(transition)); -// pf->setEvaluation(std::move(eval)); -// pf->setEstimation(std::move(estimation)); -// pf->setResampling(std::move(resample)); + // setup the filter + pf = std::unique_ptr>(new SMC::ParticleFilter(Settings::numParticles, std::move(init))); + pf->setTransition(std::move(transition)); + pf->setEvaluation(std::move(eval)); + pf->setEstimation(std::move(estimation)); + pf->setResampling(std::move(resample)); -// pf->setNEffThreshold(0.85); //before 0.75, edit by toni -// //pf->setNEffThreshold(0.65); // still too low? -// //pf->setNEffThreshold(0.25); // too low + pf->setNEffThreshold(0.85); //before 0.75, edit by toni + //pf->setNEffThreshold(0.65); // still too low? + //pf->setNEffThreshold(0.25); // too low // attach as listener to all sensors SensorFactory::get().getAccelerometer().addListener(this); @@ -70,7 +68,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getTurns().addListener(this); - //SensorFactory::get().getActivity().addListener(this); + SensorFactory::get().getActivity().addListener(this); // hacky.. but we need to call this one from the main thread! //mainController->getMapView()->showParticles(pf->getParticles()); @@ -161,14 +159,14 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const gotSensorData(ts); } -// void NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { -// (void) sensor; -// (void) ts; -// curCtrl.activity = data.curActivity; -// curObs.activity = data.curActivity; -// debugActivity(data.curActivity); -// gotSensorData(ts); -// } +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { + (void) sensor; + (void) ts; + curCtrl.activity = data.curActivity; + curObs.activity = data.curActivity; + //debugActivity(data.curActivity); + gotSensorData(ts); +} /** called when any sensor has received new data */ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { @@ -176,16 +174,16 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} } -// void debugActivity(const ActivityData& activity) { -// QString act; -// switch(activity.curActivity) { -// case ActivityButterPressure::Activity::STAY: act = "STAY"; break; -// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; -// case ActivityButterPressure::Activity::UP: act = "UP"; break; -// default: act = "???"; break; -// } -// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); -// } +// void debugActivity(const ActivityData& activity) { +// QString act; +// switch(activity.curActivity) { +// case Activity::STANDING: act = "STAY"; break; +// case Activity::WALKING_DOWN: act = "DOWN"; break; +// case Activity::WALKING_UP: act = "UP"; break; +// default: act = "???"; break; +// } +// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); +// } /** particle-filter update loop */ void MeshBased::NavControllerMesh::filterUpdateLoop() { diff --git a/nav/mesh/NavControllerMesh.h b/nav/mesh/NavControllerMesh.h index 3d7c6ec..4692891 100644 --- a/nav/mesh/NavControllerMesh.h +++ b/nav/mesh/NavControllerMesh.h @@ -8,6 +8,7 @@ #include "../sensors/SensorFactory.h" #include "../sensors/StepSensor.h" #include "../sensors/TurnSensor.h" +#include "../sensors/ActivitySensor.h" #include #include @@ -28,16 +29,16 @@ namespace MeshBased { private: NM::NavMesh* navMesh; - WiFiModelLogDistCeiling wifiModel; + WiFiModel* wifiModel; - std::unique_ptr> pf; + std::unique_ptr> pf; MyObservation curObs; MyControl curCtrl; public: - NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh); + NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel); void start() override; @@ -59,7 +60,7 @@ namespace MeshBased { void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; - // void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; + void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; private: diff --git a/nav/mesh/State.h b/nav/mesh/State.h index e46bbf9..fa1d2ca 100644 --- a/nav/mesh/State.h +++ b/nav/mesh/State.h @@ -22,24 +22,26 @@ namespace MeshBased { ; } -// MyState& operator += (const MyState& o) { -// position += o.position; -// return *this; -// } + MyState& operator += (const MyState& o) { + loc.pos += o.loc.pos; + return *this; + } -// MyState& operator /= (const float val) { -// position /= val; -// return *this; -// } + MyState& operator /= (const float val) { + loc.pos /= val; + return *this; + } -// MyState operator * (const float val) const { -// MyState copy = *this; -// copy.position = copy.position * val; -// return copy; -// } + MyState operator * (const float val) const { + MyState copy = *this; + copy.loc.pos = copy.loc.pos * val; + return copy; + } }; + + } #endif // MESH_STATE_H diff --git a/sensors/ActivitySensor.h b/sensors/ActivitySensor.h index 4df4839..aa1dcc2 100644 --- a/sensors/ActivitySensor.h +++ b/sensors/ActivitySensor.h @@ -1,16 +1,16 @@ #ifndef BAROMETERACTIVITYSENSOR_H #define BAROMETERACTIVITYSENSOR_H -#include +#include #include #include "BarometerSensor.h" #include "AccelerometerSensor.h" #include struct ActivityData { - ActivityButterPressure::Activity curActivity; - ActivityData(const ActivityButterPressure::Activity act) : curActivity(act) {;} - ActivityData() : curActivity(ActivityButterPressure::Activity::STAY) {;} + Activity curActivity; + ActivityData(const Activity act) : curActivity(act) {;} + ActivityData() : curActivity(Activity::STANDING) {;} }; /** @@ -23,7 +23,7 @@ class ActivitySensor : private: - ActivityButterPressure act; + ActivityDetector act; ActivityData data; BarometerSensor& baro; @@ -60,16 +60,16 @@ public: return; } - this->data.curActivity = act.add(ts, data); - informListeners(ts, this->data); + act.add(ts, data); + this->data.curActivity = act.get(); + informListeners(ts, this->data); } virtual void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override { (void) sensor; - (void) ts; - (void) data; - // TODO! + + act.add(ts, data); } }; diff --git a/sensors/SensorFactory.h b/sensors/SensorFactory.h index b613844..6f5989d 100644 --- a/sensors/SensorFactory.h +++ b/sensors/SensorFactory.h @@ -30,7 +30,7 @@ #include "StepSensor.h" #include "TurnSensor.h" -//#include "ActivitySensor.h" +#include "ActivitySensor.h" class SensorFactory { @@ -90,11 +90,11 @@ public: return turns; } -// /** get the Activity sensor */ -// ActivitySensor& getActivity() { -// static ActivitySensor activity(getBarometer(), getAccelerometer()); -// return activity; -// } + /** get the Activity sensor */ + ActivitySensor& getActivity() { + static ActivitySensor activity(getBarometer(), getAccelerometer()); + return activity; + } }; diff --git a/ui/map/2D/ColorPoints2D.h b/ui/map/2D/ColorPoints2D.h index ca0ec30..7bd5fb9 100644 --- a/ui/map/2D/ColorPoints2D.h +++ b/ui/map/2D/ColorPoints2D.h @@ -7,7 +7,7 @@ #include -#include +#include #include "../nav/grid/Node.h" #include "../nav/Observation.h" @@ -59,13 +59,13 @@ public: } /** NOTE: must be called from Qt's main thread! */ - template void setFromParticles(const std::vector>& particles) { + template void setFromParticles(const std::vector>& particles) { points.clear(); // group particles by grid-point std::unordered_map weights; - for (const K::Particle& p : particles) { + for (const SMC::Particle& p : particles) { const GridPoint gp = p.state.position; if (weights.find(gp) != weights.end()) {continue;} weights[gp] += p.weight; diff --git a/ui/map/2D/HasSelectableNodes.h b/ui/map/2D/HasSelectableNodes.h index bf8c138..1abaf92 100644 --- a/ui/map/2D/HasSelectableNodes.h +++ b/ui/map/2D/HasSelectableNodes.h @@ -2,7 +2,7 @@ #define HASSELECTABLENODES_H #include -#include +#include class HasSelectableNodes { diff --git a/ui/map/2D/MapView2D.cpp b/ui/map/2D/MapView2D.cpp index 5a096b4..6ae40c5 100644 --- a/ui/map/2D/MapView2D.cpp +++ b/ui/map/2D/MapView2D.cpp @@ -111,7 +111,7 @@ void MapView2D::setMap(WiFiCalibrationDataModel* mdl, Floorplan::IndoorMap* map) } -void MapView2D::showParticles(const std::vector>* particles) { +void MapView2D::showParticles(const std::vector>* particles) { this->colorPoints->setFromParticles(*particles); } diff --git a/ui/map/2D/MapView2D.h b/ui/map/2D/MapView2D.h index 56b09e3..3fccf8f 100644 --- a/ui/map/2D/MapView2D.h +++ b/ui/map/2D/MapView2D.h @@ -28,7 +28,7 @@ class ColorPoints2D; class Path2D; template class DijkstraPath; -namespace K { +namespace SMC { template class Particle; } class MyState; @@ -101,11 +101,11 @@ public: /** NOTE: must be called from Qt's main thread! */ Q_INVOKABLE void showParticles(const void* particles) { - showParticles((const std::vector>*) particles); + showParticles((const std::vector>*) particles); } /** NOTE: must be called from Qt's main thread! */ - void showParticles(const std::vector>* particles); + void showParticles(const std::vector>* particles); diff --git a/ui/map/3D/MapView3D.h b/ui/map/3D/MapView3D.h index da3847b..7777edf 100644 --- a/ui/map/3D/MapView3D.h +++ b/ui/map/3D/MapView3D.h @@ -124,11 +124,11 @@ public: /** NOTE: must be called from Qt's main thread! */ Q_INVOKABLE void showParticles(const void* particles) { - showParticles((const std::vector>*) particles); + showParticles((const std::vector>*) particles); } /** NOTE: must be called from Qt's main thread! */ - void showParticles(const std::vector>* particles) { + void showParticles(const std::vector>* particles) { this->colorPoints->setFromParticles(*particles); } diff --git a/ui/map/3D/elements/ColorPoints.h b/ui/map/3D/elements/ColorPoints.h index bfdc681..cf55ecd 100644 --- a/ui/map/3D/elements/ColorPoints.h +++ b/ui/map/3D/elements/ColorPoints.h @@ -3,7 +3,7 @@ #include -#include +#include #include "../gl/GLHelper.h" #include "../gl/GLPoints.h" @@ -62,7 +62,7 @@ public: } /** NOTE: must be called from Qt's main thread! */ - template void setFromParticles(const std::vector>& particles) { + template void setFromParticles(const std::vector>& particles) { points.clear(); @@ -71,7 +71,7 @@ public: // group particles by grid-point std::unordered_map weights; - for (const K::Particle& p : particles) { + for (const SMC::Particle& p : particles) { const GridPoint gp = p.state.position; if (weights.find(gp) != weights.end()) {continue;} weights[gp] += p.weight; @@ -97,7 +97,7 @@ public: } -// for (const K::Particle& p : particles) { +// for (const SMC::Particle& p : particles) { // const GridPoint gp = p.state.position; // const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f); // swap z and y // const QColor color = Qt::blue;