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;