diff --git a/Settings.h b/Settings.h index 7f32e41..1379e91 100644 --- a/Settings.h +++ b/Settings.h @@ -56,13 +56,14 @@ namespace Settings { constexpr float WAF = -11.0; const bool optimize = true; + const bool useRegionalOpt = true; // how to perform VAP grouping. see // - calibration in Controller.cpp // - eval in Filter.h // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! - const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); - const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); + const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM } namespace BeaconModel { @@ -83,9 +84,13 @@ namespace Settings { constexpr bool useMainThread = false; // perform filtering in the main thread } - const std::string mapDir = "../map/"; - const std::string dataDir = "../measurements/"; - const std::string errorDir = dataDir + "results/"; +// const std::string mapDir = "../map/"; +// const std::string dataDir = "../measurements/"; +// const std::string errorDir = dataDir + "results/"; + + const std::string mapDir = "/apps/museum/maps/"; + const std::string dataDir = "/apps/"; + const std::string errorDir = dataDir + "museum/results/"; /** describes one dataset (map, training, parameter-estimation, ...) */ struct DataSetup { diff --git a/main.cpp b/main.cpp index 2f939eb..a54f4e5 100755 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,11 @@ #include #include #include +#include #include +#include + + #include #include @@ -55,32 +59,67 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string // wifi - WiFiModelLogDistCeiling WiFiModel(map); +// WiFiModelLogDistCeiling WiFiModel(map); +// WiFiModelPerFloor WiFiModelPerFloor(map); +// WiFiModelPerBBox WiFiModelPerBBox(map); + + WiFiModel* WiFiModel = nullptr; // with optimization if(Settings::WiFiModel::optimize){ if (!inp.good() || (inp.peek()&&0) || inp.eof()) { - Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!"); - WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); - for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { - opt.addFingerprint(fp); - } - const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); - for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { - const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); - WiFiModel.addAP(ap.mac, entry); - } - WiFiModel.saveXML(setup.wifiModel); + Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!"); + + if (Settings::WiFiModel::useRegionalOpt) { + + // use a regional optimization scheme (one per floor) + + WiFiOptimizerPerFloor opt(map); + + // add all fingerprints to the optimizer (optimizer will add them to the correct floor/model) + for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { + opt.addFingerprint(fp); + } + + WiFiModel = opt.optimizeAll(); + + WiFiModel->saveXML(setup.wifiModel); + + } else { + + // use one model per AP for the whole map + + WiFiModel = new WiFiModelLogDistCeiling(map); + WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); + for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { + opt.addFingerprint(fp); + } + const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); + for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { + const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); + ((WiFiModelLogDistCeiling*)WiFiModel)->addAP(ap.mac, entry); + } + + WiFiModel->saveXML(setup.wifiModel); + + } + } else { - WiFiModel.loadXML(setup.wifiModel); + + // load WiFiModel from file. The factory will create the correct instance + //WiFiModel->loadXML(setup.wifiModel); + WiFiModelFactory fac(map); + WiFiModel = fac.loadXML(setup.wifiModel); + } } else { // without optimization - WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + WiFiModel = new WiFiModelLogDistCeiling(map); + ((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"); } @@ -117,8 +156,8 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string const int numParticles = 5000; //auto init = std::make_unique(&mesh, srcPath1); // known position auto init = std::make_unique(&mesh); // uniform distribution - auto eval = std::make_unique(WiFiModel); - auto trans = std::make_unique(mesh, WiFiModel); + auto eval = std::make_unique(*WiFiModel); + auto trans = std::make_unique(mesh, *WiFiModel); //auto resample = std::make_unique>(); //auto resample = std::make_unique>(); @@ -212,7 +251,8 @@ Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string plot.setGroundTruth(gtPos); plot.addEstimationNode(est.pos.pos); plot.setActivity((int) act.get()); - //plot.plot(); + + plot.plot(); // error calc float err_m = gtPos.getDistance(est.pos.pos); diff --git a/navMesh/filter.h b/navMesh/filter.h index 415f000..883df55 100644 --- a/navMesh/filter.h +++ b/navMesh/filter.h @@ -172,10 +172,10 @@ public: class MyPFTrans : public SMC::ParticleFilterTransition { - //using MyNavMeshWalk = NM::NavMeshWalkSimple; + //using MyNavMeshWalk = NM::NavMeshWalkSimple; //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; - using MyNavMeshWalk = NM::NavMeshWalkKLD; + using MyNavMeshWalk = NM::NavMeshWalkKLD; MyNavMeshWalk walker; WiFiQualityAnalyzer analyzer; @@ -219,20 +219,20 @@ public: } // divergence between eval and transition - std::vector> wifiParticles; - NM::NavMeshRandom rnd = walker.getMesh().getRandom(); - for(int i = 0; i < 10000; ++i){ + std::vector> wifiParticles; + NM::NavMeshRandom rnd = walker.getMesh().getRandom(); + for(int i = 0; i < 10000; ++i){ - NM::NavMeshLocation tmpLocation = rnd.draw(); - double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs); - SMC::Particle tmpParticle(MyState(tmpLocation.pos), weight); - wifiParticles.push_back(tmpParticle); - } + NM::NavMeshLocation tmpLocation = rnd.draw(); + double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs); + SMC::Particle tmpParticle(MyState(tmpLocation.pos), weight); + wifiParticles.push_back(tmpParticle); + } - MyState wifiEstimate = estimator.estimate(wifiParticles); + MyState wifiEstimate = estimator.estimate(wifiParticles); - // fake kld - const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos); + // fake kld + const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos); //const double kld = Divergence::KullbackLeibler::getMultivariateGauss(normParticle, normWifi);; //std::cout << "KLD: " << kld << std::endl; @@ -262,8 +262,8 @@ public: double deltaUnblockable = 0.01; // walk - //MyNavMeshWalk::ResultEntry res = walker.getOne(params); - MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi); + //MyNavMeshWalk::ResultEntry res = walker.getOne(params); + MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi); // assign back to particle's state p.weight *= res.probability; @@ -310,7 +310,10 @@ class MyPFEval : public SMC::ParticleFilterEvaluation { public: - MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} + // FRANK + //MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} + //MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){} + MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){} virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { @@ -318,8 +321,8 @@ public: const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); - #pragma omp parallel for num_threads(3) - for (int i = 0; i < particles.size(); ++i) { + #pragma omp parallel for num_threads(3) + for (size_t i = 0; i < particles.size(); ++i) { SMC::Particle& p = particles[i]; double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);