fixed some minor bugs using ble on android

added BLE to the evaluation of the particle filter
made first evaluation inside fhws

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);

Controller.h

@@ -22,6 +22,7 @@ class InfoWidget;
 class MapView3D;
 class MapView2D;
 class WiFiModel;
+class BeaconModel;
 
 namespace NM {
     template <typename T> class NavMesh;
@@ -80,6 +81,7 @@ private:
     NavController* nav = nullptr;
     Floorplan::IndoorMap* im = nullptr;
     WiFiModel* wifiModel = nullptr;
+    BeaconModel* bleModel = nullptr;
 
 };
 

Settings.h

@@ -4,6 +4,7 @@
 #include <Indoor/grid/GridPoint.h>
 #include <Indoor/data/Timestamp.h>
 #include <Indoor/sensors/radio/VAPGrouper.h>
+#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
 
 #include <QFile>
 #include <QDir>
@@ -56,7 +57,16 @@ namespace Settings {
 //		const VAPGrouper vg_eval  = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
     }
 
+    namespace BLEModel {
 
+        constexpr float sigma = 8.0;
+
+        constexpr float TXP = -59;
+        constexpr float EXP = 2.3;
+        constexpr float WAF = -9.0;
+
+        const BeaconMeasurementGrouper vg_eval  = BeaconMeasurementGrouper(BeaconMeasurementGrouper::Mode::DISABLED, BeaconMeasurementGrouper::Aggregation::AVERAGE);
+    }
 
     namespace MapView3D {
         const int maxColorPoints = 10000;

_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);

_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!");}

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"

nav/mesh/FilterMesh.h

@@ -14,11 +14,14 @@
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
 
-#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
 #include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
 #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
 #include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
 
+#include <Indoor/sensors/beacon/model/BeaconModel.h>
+#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
+#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
+
 #include <Indoor/navMesh/NavMesh.h>
 #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
 #include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
@@ -143,6 +146,9 @@ namespace MeshBased {
         WiFiModel& wifiModel;
         WiFiObserverFree wifiProbability;
 
+        BeaconModel& bleModel;
+        BeaconObserverFree bleProbability;
+
         double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
 
             const float kappa = 0.9;
@@ -169,7 +175,7 @@ namespace MeshBased {
     public:
 
         //TODO: Was ist hier besser? Im Museum hatten wir das unterste.
-		PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
+        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){}
 
@@ -182,6 +188,9 @@ namespace MeshBased {
 
             // vap-grouping
             const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
+            const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
+
+            //TODO: do i need a vap grouping for beacons?
 
             // sanity check
             //Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
@@ -191,13 +200,14 @@ namespace MeshBased {
             for (size_t i = 0; i < particles.size(); ++i) {
                 SMC::Particle<MyState>& p = particles[i];
 
+                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;
 
                 //TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
 
-				const double prob = pWifi * pStair * pGPS;
+                const double prob = pBLE * pStair * pGPS * pWifi;
 
                 p.weight *= prob;
                 if (p.weight != p.weight) {throw Exception("nan");}

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<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
+MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel) :
-    NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
+    NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel), bleModel(bleModel) {
 
     // filter init
     std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
@@ -54,7 +56,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
     std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
 
     // eval and transition
-    std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
+    std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel, bleModel));
     std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
 
     // setup the filter

nav/mesh/NavControllerMesh.h

@@ -12,6 +12,7 @@
 
 #include <Indoor/navMesh/NavMeshLocation.h>
 #include <Indoor/navMesh/NavMesh.h>
+#include <Indoor/sensors/beacon/model/BeaconModel.h>
 
 #include <Indoor/Assertions.h>
 #include <thread>
@@ -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<NM::NavMeshTriangle>* navMesh;
         WiFiModel* wifiModel;
+        BeaconModel* bleModel;
 
         std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
 
@@ -38,7 +39,7 @@ namespace MeshBased {
 
     public:
 
-        NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
+        NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel);
 
 
         void start() override;