diff --git a/Controller.cpp b/Controller.cpp index 3882def..537f505 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -250,10 +250,52 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi BeaconModel* buildBLEModelOnce(Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& bleModelFile){ // ask questions - const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No); + const QMessageBox::StandardButton replyBLEFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No); BeaconModel* bleModel = new BeaconModelLogDistCeiling(map); + // WiFi setup + if (replyBLEFP == QMessageBox::Yes) { + + std::ifstream inp(bleModelFile, std::ifstream::binary); + + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + + Log::add("Controller", "Create new BLEModel"); + BLECalibrationDataModel mdl(fpFile); + Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!"); + WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); + for (const BeaconFingerprint& fp : mdl.getFingerprints()) { + opt.addFingerprint(fp); //we added a function to add bluetooth fingerprints to the wifi optimizer + } + + const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.MIN_10_FPS); + for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { + const BeaconModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); + ((BeaconModelLogDistCeiling*) bleModel)->addBeacon(ap.mac, entry); + } + + ((BeaconModelLogDistCeiling*) bleModel)->saveXML(bleModelFile); + + } else { + + Log::add("Controller", "Use existing BLEModel"); + XMLDoc doc; + XMLserialize::assertOK(doc.LoadFile(bleModelFile.c_str()), doc, "error while loading file"); + XMLElem* root = doc.FirstChildElement("root"); + + //if(bleModel){delete bleModel;} + bleModel->readFromXML(&doc, root); + } + + } else { + + Log::add("Controller", "Read Beacons from File"); + // load all APs from the floorplan and use same TXP/EXP/WAF for all of them + ((BeaconModelLogDistCeiling*)bleModel)->loadBeaconsFromMap(map, Settings::BLEModel::TXP, Settings::BLEModel::EXP, Settings::BLEModel::WAF); + Assert::isFalse(bleModel->getAllBeacons().empty(), "no AccessPoints stored within the map.xml"); + } + return bleModel; } @@ -340,6 +382,7 @@ void Controller::loadNavMesh(QDir dir) { QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiModel(dir.path() + "/wifimodel.dat"); QFile fBLEFP(dir.path() + "/ble_fp.dat"); + QFile fBLEModel(dir.path() + "/blemodel.dat"); Assert::isTrue(fMap.exists(), "map.xml missing"); @@ -347,12 +390,16 @@ void Controller::loadNavMesh(QDir dir) { QString str = QString(fMap.readAll()); im = Floorplan::Reader::readFromString(str.toStdString()); + //build the wifi model const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); const std::string sWiFiModel = fWiFiModel.fileName().toStdString(); - const std::string sBLEFP = fBLEFP.fileName().toStdString(); - wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel); + //build the ble model + const std::string sBLEFP = fBLEFP.fileName().toStdString(); + const std::string sBLEModel = fBLEModel.fileName().toStdString(); + bleModel = buildBLEModelOnce(im, sBLEFP, sBLEModel); + // create navmesh if (navMesh) {delete navMesh; navMesh = nullptr;} NM::NavMeshSettings settings; @@ -362,7 +409,7 @@ void Controller::loadNavMesh(QDir dir) { // create a new navigator if (nav) {delete nav; nav = nullptr;} - nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel); + nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel, bleModel); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); BLECalibrationDataModel* bleCalib = new BLECalibrationDataModel(sBLEFP); diff --git a/Controller.h b/Controller.h index 9f33693..2f07a75 100644 --- a/Controller.h +++ b/Controller.h @@ -22,32 +22,33 @@ class InfoWidget; class MapView3D; class MapView2D; class WiFiModel; +class BeaconModel; namespace NM { - template class NavMesh; - class NavMeshTriangle; + template class NavMesh; + class NavMeshTriangle; } namespace Floorplan { - class IndoorMap; + class IndoorMap; } class Controller : public QObject { - Q_OBJECT + Q_OBJECT public: - /** ctor */ - explicit Controller(); + /** ctor */ + explicit Controller(); public: - MapView3D* getMapView3D() const; - MapView2D* getMapView2D() const; + MapView3D* getMapView3D() const; + MapView2D* getMapView2D() const; - MainMenu* getMainMenu() const; - InfoWidget* getInfoWidget() const; + MainMenu* getMainMenu() const; + InfoWidget* getInfoWidget() const; protected: @@ -60,26 +61,27 @@ protected: private slots: - void onLoadButton(); - void onDebugButton(); - void onStartButton(); + void onLoadButton(); + void onDebugButton(); + void onStartButton(); - void onTransparentButton(); - void onCameraButton(); - void on3DButton(); + void onTransparentButton(); + void onCameraButton(); + void on3DButton(); private: - void loadGrid(QDir dir); - void loadNavMesh(QDir dir); + void loadGrid(QDir dir); + void loadNavMesh(QDir dir); - MainWindow* mainWindow; + MainWindow* mainWindow; - Grid* grid = nullptr; - NM::NavMesh* navMesh = nullptr; - NavController* nav = nullptr; - Floorplan::IndoorMap* im = nullptr; + Grid* grid = nullptr; + NM::NavMesh* navMesh = nullptr; + NavController* nav = nullptr; + Floorplan::IndoorMap* im = nullptr; WiFiModel* wifiModel = nullptr; + BeaconModel* bleModel = nullptr; }; diff --git a/Settings.h b/Settings.h index e09bff2..8daa168 100644 --- a/Settings.h +++ b/Settings.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -12,114 +13,123 @@ namespace Settings { const int numParticles = 5000; - namespace IMU { - const float turnSigma = 1.5; // 3.5 - const float stepLength = 0.80; + namespace IMU { + const float turnSigma = 1.5; // 3.5 + const float stepLength = 0.80; const float stepSigma = 0.40; //toni changed - } + } - const float smartphoneAboveGround = 1.3; + const float smartphoneAboveGround = 1.3; - const float offlineSensorSpeedup = 2; + const float offlineSensorSpeedup = 2; - namespace Grid { - constexpr int gridSize_cm = 20; - } + namespace Grid { + constexpr int gridSize_cm = 20; + } - //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination - const GridPoint destination = GridPoint(0,0,0); // do not use destination + //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination + const GridPoint destination = GridPoint(0,0,0); // do not use destination - namespace SensorDebug { - const Timestamp updateEvery = Timestamp::fromMS(200); - } + namespace SensorDebug { + const Timestamp updateEvery = Timestamp::fromMS(200); + } - namespace WiFiModel { + namespace WiFiModel { constexpr float sigma = 10.0; //TODO: im Museum hatten wir 8.0 - /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ + /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ constexpr float TXP = -45; constexpr float EXP = 2.3; constexpr float WAF = -5.0; - // 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::AVERAGE); - const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); + // 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::AVERAGE); + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); // const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE); // const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE); - } + } + namespace BLEModel { + constexpr float sigma = 8.0; - namespace MapView3D { - const int maxColorPoints = 10000; - constexpr int fps = 15; - const Timestamp msPerFrame = Timestamp::fromMS(1000/fps); - } + constexpr float TXP = -59; + constexpr float EXP = 2.3; + constexpr float WAF = -9.0; - namespace Filter { - const Timestamp updateEvery = Timestamp::fromMS(500); - constexpr bool useMainThread = false; // perform filtering in the main thread - } + const BeaconMeasurementGrouper vg_eval = BeaconMeasurementGrouper(BeaconMeasurementGrouper::Mode::DISABLED, BeaconMeasurementGrouper::Aggregation::AVERAGE); + } - namespace Data { + namespace MapView3D { + const int maxColorPoints = 10000; + constexpr int fps = 15; + const Timestamp msPerFrame = Timestamp::fromMS(1000/fps); + } - // notes - // copy: scp -P 2222 /tmp/grid.dat kazu@192.168.24.11:/storage/sdcard1/YASMIN/maps/car/ - // all: scp -P 2222 -r /apps/android/workspace/YASMIN_DATA/* kazu@192.168.24.11:/storage/sdcard1/YASMIN/ + namespace Filter { + const Timestamp updateEvery = Timestamp::fromMS(500); + constexpr bool useMainThread = false; // perform filtering in the main thread + } - static inline std::string getRoot() { - #ifdef ANDROID - // const std::string folder = getenv("EXTERNAL_STORAGE") + std::string("/YASMIN/maps/"); // this is NOT the sdcard?! - // qDebug(folder.c_str()); - // return folder; - //return "/storage/sdcard1/YASMIN/"; - //const std::string f1 = "/storage/sdcard1/YASMIN/"; - //const std::string f2 = "/sdcard/YASMIN/"; - //if (QFile(f1.c_str()).exists()) {return f1;} - //if (QFile(f2.c_str()).exists()) {return f2;} - std::string tried; - QDir dStorage("/storage"); - QStringList lst = dStorage.entryList(); + namespace Data { + + // notes + // copy: scp -P 2222 /tmp/grid.dat kazu@192.168.24.11:/storage/sdcard1/YASMIN/maps/car/ + // all: scp -P 2222 -r /apps/android/workspace/YASMIN_DATA/* kazu@192.168.24.11:/storage/sdcard1/YASMIN/ + + static inline std::string getRoot() { + #ifdef ANDROID + // const std::string folder = getenv("EXTERNAL_STORAGE") + std::string("/YASMIN/maps/"); // this is NOT the sdcard?! + // qDebug(folder.c_str()); + // return folder; + //return "/storage/sdcard1/YASMIN/"; + //const std::string f1 = "/storage/sdcard1/YASMIN/"; + //const std::string f2 = "/sdcard/YASMIN/"; + //if (QFile(f1.c_str()).exists()) {return f1;} + //if (QFile(f2.c_str()).exists()) {return f2;} + std::string tried; + QDir dStorage("/storage"); + QStringList lst = dStorage.entryList(); lst.append("sdcard/YASMIN/"); lst.append("emulated/0"); //TODO: THIS IS A MESS, PURE CHAOS xD - // try each potential folder - for (const QString subfolder : lst) { - QString path = dStorage.path() + "/" + subfolder + "/YASMIN/"; - if (QFile(path).exists()) {return path.toStdString();} - tried += path.toStdString() + "\n"; - } - throw Exception("data folder missing. tried:\n" + tried); - #else - //return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/"; - return "/apps/android/workspace/YASMIN_DATA/"; - #endif - } + // try each potential folder + for (const QString subfolder : lst) { + QString path = dStorage.path() + "/" + subfolder + "/YASMIN/"; + if (QFile(path).exists()) {return path.toStdString();} + tried += path.toStdString() + "\n"; + } + throw Exception("data folder missing. tried:\n" + tried); + #else + //return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/"; + return "/apps/android/workspace/YASMIN_DATA/"; + #endif + } - /** get the directory where maps are stored */ - static inline std::string getMapDir() { - return getRoot() + "maps/"; - } + /** get the directory where maps are stored */ + static inline std::string getMapDir() { + return getRoot() + "maps/"; + } - static inline std::string getOfflineDir() { - return getRoot() + "offline/"; - } + static inline std::string getOfflineDir() { + return getRoot() + "offline/"; + } - static inline std::string getRecordsDir() { - return getRoot() + "records/"; - } + static inline std::string getRecordsDir() { + return getRoot() + "records/"; + } - } + } } diff --git a/_android/src/BLE.java b/_android/src/BLE.java index 32ddf66..fbc40c2 100644 --- a/_android/src/BLE.java +++ b/_android/src/BLE.java @@ -29,6 +29,8 @@ public class BLE { /** called when a scan is completed successfully */ public static native void onScanComplete(final byte[] result); + public static int dataCounter = 0; + /** * start bluetooth scanning in the background. * will call onScanComplete() once a scan is available @@ -52,7 +54,7 @@ public class BLE { final BluetoothManager mgr = (BluetoothManager) act.getSystemService(Context.BLUETOOTH_SERVICE); bt = mgr.getAdapter(); - Log.d("ble", "BLE Mangaer Start"); + Log.d("ble", "BLE Manager Start"); // create the scanner scanner = bt.getBluetoothLeScanner(); @@ -69,7 +71,12 @@ public class BLE { // and attach the callback mLeScanCallback = new ScanCallback() { @Override public void onScanResult(int callbackType, android.bluetooth.le.ScanResult result) { - //Log.d("ble", result.getDevice() + " " + result.getRssi()); + + if(++dataCounter > 50){ + Log.d("ble", result.getDevice() + " " + result.getRssi()); + dataCounter = 0; + } + final byte[] rst = serialize(result); BLE.onScanComplete(rst); diff --git a/_android/src/WiFi.java b/_android/src/WiFi.java index f41e2b9..2a7bf9e 100644 --- a/_android/src/WiFi.java +++ b/_android/src/WiFi.java @@ -122,7 +122,7 @@ public class WiFi { /** try to trigger one scan process */ private static void triggerOneScan() { - //Log.d("wifi", "triggerOneScan()"); + Log.d("wifi", "triggerOneScan()"); try { if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");} diff --git a/nav/NavController.h b/nav/NavController.h index b612153..7121b3a 100644 --- a/nav/NavController.h +++ b/nav/NavController.h @@ -5,6 +5,7 @@ #include "../sensors/GyroscopeSensor.h" #include "../sensors/BarometerSensor.h" #include "../sensors/WiFiSensor.h" +#include "../sensors/BLESensor.h" #include "../sensors/SensorFactory.h" #include "../sensors/StepSensor.h" #include "../sensors/TurnSensor.h" diff --git a/nav/mesh/FilterMesh.h b/nav/mesh/FilterMesh.h index 5a2ca44..fe4d686 100644 --- a/nav/mesh/FilterMesh.h +++ b/nav/mesh/FilterMesh.h @@ -14,11 +14,14 @@ #include #include -#include #include #include #include +#include +#include +#include + #include #include #include @@ -32,185 +35,192 @@ namespace MeshBased { - class PFInit : public SMC::ParticleFilterInitializer { + class PFInit : public SMC::ParticleFilterInitializer { - private: + private: - const NM::NavMesh* mesh; + const NM::NavMesh* mesh; - public: + public: - PFInit(const 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); + std::minstd_rand gen; + std::uniform_real_distribution distHead(0, 2*M_PI); - NM::NavMeshRandom rnd = mesh->getRandom(); + NM::NavMeshRandom rnd = mesh->getRandom(); - for (SMC::Particle& p : particles) { - p.state.pos = rnd.draw(); - p.state.heading = Heading(distHead(gen)); // random heading - p.weight = 1.0 / particles.size(); // equal weight - } + for (SMC::Particle& p : particles) { + p.state.pos = rnd.draw(); + p.state.heading = Heading(distHead(gen)); // random heading + p.weight = 1.0 / particles.size(); // equal weight + } - // // fix position + heading - // 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 - // p.state.position = node; - // p.state.heading.direction = Heading(0); - // } + // // fix position + heading + // 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 + // p.state.position = node; + // p.state.heading.direction = Heading(0); + // } - } + } - }; + }; - class PFTrans : public SMC::ParticleFilterTransition { + class PFTrans : public SMC::ParticleFilterTransition { - public: + public: - //using MyNavMeshWalk = NM::NavMeshWalkSimple; - using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim; - //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; - //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; - MyNavMeshWalk walker; + //using MyNavMeshWalk = NM::NavMeshWalkSimple; + using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim; + //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional; + //using MyNavMeshWalk = NM::NavMeshWalkUnblockable; + MyNavMeshWalk walker; - // local, static control-data COPY - MyControl ctrl; + // local, static control-data COPY + MyControl ctrl; - public: + public: - PFTrans(NM::NavMesh* mesh) : walker(*mesh){ + PFTrans(NM::NavMesh* mesh) : walker(*mesh){ - // 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 + // 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(); + // local copy!! observation might be changed async outside!! (will really produces crashes!) + this->ctrl = *_ctrl; + ((MyControl*)_ctrl)->resetAfterTransition(); - // 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); + // 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); - #pragma omp parallel for num_threads(3) - for (int i = 0; i < particles.size(); ++i) { - SMC::Particle& p = particles[i]; + #pragma omp parallel for num_threads(3) + for (int i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[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.pos; + // how to walk + NM::NavMeshWalkParams params; + params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw(); + params.numSteps = ctrl.numStepsSinceLastTransition; + params.start = p.state.pos; - params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); - params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); + params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); + params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); - 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; - } + 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; + } - // walk - MyNavMeshWalk::ResultEntry res = walker.getOne(params); + // walk + MyNavMeshWalk::ResultEntry res = walker.getOne(params); - // assign back to particle's state - p.weight *= res.probability; - p.state.pos = res.location; - p.state.heading = res.heading; - } + // assign back to particle's state + p.weight *= res.probability; + p.state.pos = res.location; + p.state.heading = res.heading; + } - } + } - }; + }; - class PFEval : public SMC::ParticleFilterEvaluation { + class PFEval : public SMC::ParticleFilterEvaluation { - WiFiModel& wifiModel; - WiFiObserverFree wifiProbability; + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; - double getStairProb(const SMC::Particle& p, const Activity act) { + BeaconModel& bleModel; + BeaconObserverFree bleProbability; - const float kappa = 0.9; + double getStairProb(const SMC::Particle& p, const Activity act) { - switch (act) { + const float kappa = 0.9; - case Activity::WALKING: - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} - {return 1-kappa;} + switch (act) { - case Activity::WALKING_UP: - case Activity::WALKING_DOWN: - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} - {return 1-kappa;} - } + case Activity::WALKING: + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + {return 1-kappa;} - return 1.0; - } + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} + if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} + {return 1-kappa;} + } - public: + return 1.0; + } - //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){} + public: - double evaluation(std::vector>& particles, const MyObservation& _observation) override { + //TODO: Was ist hier besser? Im Museum hatten wir das unterste. + PFEval(WiFiModel* wifiModel, BeaconModel* bleModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel), bleModel(*bleModel), bleProbability(Settings::BLEModel::sigma, *bleModel){} + //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 sum = 0; + double evaluation(std::vector>& particles, const MyObservation& _observation) override { - // local copy!! observation might be changed async outside!! (will really produces crashes!) - const MyObservation observation = _observation; + double sum = 0; - // vap-grouping - const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + // local copy!! observation might be changed async outside!! (will really produces crashes!) + const MyObservation observation = _observation; - // sanity check - //Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + // vap-grouping + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble); - // assign weights - #pragma omp parallel for num_threads(3) - for (size_t i = 0; i < particles.size(); ++i) { - SMC::Particle& p = particles[i]; + //TODO: do i need a vap grouping for beacons? - const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); - const double pStair = getStairProb(p, observation.activity); - const double pGPS = 1; + // sanity check + //Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); - //TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind. + // assign weights + #pragma omp parallel for num_threads(3) + for (size_t i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; - const double prob = pWifi * pStair * pGPS; + const double pBLE = bleProbability.getProbability(p.state.pos.pos, observation.currentTime, beaconObs); + const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); + const double pStair = getStairProb(p, observation.activity); + const double pGPS = 1; - p.weight *= prob; - if (p.weight != p.weight) {throw Exception("nan");} + //TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind. - #pragma omp atomic - sum += p.weight; - } + const double prob = pBLE * pStair * pGPS * pWifi; - return sum; + p.weight *= prob; + if (p.weight != p.weight) {throw Exception("nan");} - } + #pragma omp atomic + sum += p.weight; + } - }; + return sum; + + } + + }; } diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp index b9da1bf..a364cea 100644 --- a/nav/mesh/NavControllerMesh.cpp +++ b/nav/mesh/NavControllerMesh.cpp @@ -31,11 +31,13 @@ #include "Settings.h" +#include "../Controller.h" + Q_DECLARE_METATYPE(const void*) /** ctor */ -MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel) : - NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) { +MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel) : + NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel), bleModel(bleModel) { // filter init std::unique_ptr> init(new MeshBased::PFInit(navMesh)); @@ -54,7 +56,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); // eval and transition - std::unique_ptr> eval(new MeshBased::PFEval(wifiModel)); + std::unique_ptr> eval(new MeshBased::PFEval(wifiModel, bleModel)); std::unique_ptr> transition(new MeshBased::PFTrans(navMesh)); // setup the filter diff --git a/nav/mesh/NavControllerMesh.h b/nav/mesh/NavControllerMesh.h index 7021bb3..e4c2069 100644 --- a/nav/mesh/NavControllerMesh.h +++ b/nav/mesh/NavControllerMesh.h @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -19,7 +20,6 @@ //#include "State.h" #include "FilterMesh.h" -#include "../Controller.h" #include "../NavController.h" namespace MeshBased { @@ -30,6 +30,7 @@ namespace MeshBased { NM::NavMesh* navMesh; WiFiModel* wifiModel; + BeaconModel* bleModel; std::unique_ptr> pf; @@ -38,7 +39,7 @@ namespace MeshBased { public: - NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel); + NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel); void start() override;