From 20ae2f5c2ae9cfa757b39474454ee32fc4ba5a6f Mon Sep 17 00:00:00 2001 From: "mail@toni-fetzer.de" <9ak5gcoueo> Date: Wed, 5 Jun 2019 18:04:31 +0200 Subject: [PATCH] started to add ble functions added ble as sensor to java and c++ added sensorlistener for ble added ble to observation and onDataSensor in filter started to work on ble fingerprints for optimization --- Controller.cpp | 285 ++++++++-------- Settings.h | 2 +- _android/src/BLE.java | 111 +++++++ _android/src/WiFi.java | 74 ++--- nav/NavController.h | 77 ++--- nav/Observation.h | 46 +-- nav/grid/Filter.h | 255 ++++++++------- nav/grid/NavControllerGrid.cpp | 291 +++++++++-------- nav/grid/NavControllerGrid.h | 64 ++-- nav/mesh/FilterMesh.h | 176 +++++----- nav/mesh/NavControllerMesh.cpp | 207 ++++++------ nav/mesh/NavControllerMesh.h | 56 ++-- nav/mesh/State.h | 74 ++--- sensors/SensorFactory.h | 80 ++--- sensors/SensorWriter.h | 101 +++--- sensors/TurnSensor.h | 1 + sensors/android/BLESensorAndroid.cpp | 18 + sensors/android/SensorFactoryAndroid.h | 41 ++- tools/calibration/BLECalibrationDataModel.h | 4 + tools/calibration/WiFiCalibrationScanDialog.h | 124 +++---- ui/map/2D/ColorPoints2D.h | 86 ++--- ui/map/2D/Floor2D.h | 11 +- yasmin.pro | 309 +++++++++--------- 23 files changed, 1341 insertions(+), 1152 deletions(-) create mode 100644 _android/src/BLE.java create mode 100644 sensors/android/BLESensorAndroid.cpp create mode 100644 tools/calibration/BLECalibrationDataModel.h diff --git a/Controller.cpp b/Controller.cpp index ae7df74..9c2ac64 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -44,71 +44,72 @@ Controller::Controller() { - // OpenGL setup - // MUST happen before anything gets visible (= gets initialized) - QSurfaceFormat format; - format.setDepthBufferSize(16); - QSurfaceFormat::setDefaultFormat(format); + // OpenGL setup + // MUST happen before anything gets visible (= gets initialized) + QSurfaceFormat format; + format.setDepthBufferSize(16); + QSurfaceFormat::setDefaultFormat(format); - // configure the to-be-used sensor factory - //SensorFactory::set(new SensorFactoryDummy()); - //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/nexus6/kleinerKreis_216steps_6runden_telefongerade.csv")); - //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s3mini/kleinerKreis_225steps_6runden_telefongerade.csv")); - //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s4/kleinerKreis_220steps_6runden_telefongeneigt.csv")); + // configure the to-be-used sensor factory + //SensorFactory::set(new SensorFactoryDummy()); + //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/nexus6/kleinerKreis_216steps_6runden_telefongerade.csv")); + //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s3mini/kleinerKreis_225steps_6runden_telefongerade.csv")); + //SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s4/kleinerKreis_220steps_6runden_telefongeneigt.csv")); - //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "bergwerk/path3/nexus/vor/1454782562231.csv")); - //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/rueck/1454776724285_rueck.csv")); - //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv")); + //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "bergwerk/path3/nexus/vor/1454782562231.csv")); + //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/rueck/1454776724285_rueck.csv")); + //SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv")); - // live data on the smartphone - if (1 == 1) { + // live data on the smartphone + if (1 == 1) { - // use android's sensors - SensorFactory::set(new SensorFactoryAndroid()); + // use android's sensors + SensorFactory::set(new SensorFactoryAndroid()); - // write them to file?? - //const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv"; - //SensorWriter* writer = new SensorWriter(); - //writer->start(file); + // write them to file?? + //const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv"; + //SensorWriter* writer = new SensorWriter(); + //writer->start(file); - // start the sensors + // 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().getBLE().start(); - } + } - // create the main window - mainWindow = new MainWindow(); - //sl = new StepLoggerWrapperAndroid(scaler); - //sl = new StepLoggerWrapper(scaler, mainWindow); + // create the main window + mainWindow = new MainWindow(); + //sl = new StepLoggerWrapperAndroid(scaler); + //sl = new StepLoggerWrapper(scaler, mainWindow); - // attach logger - LoggerComposite* log = new LoggerComposite(); - log->addLogger(new LoggerCOUT()); - log->addLogger(new LoggerAndroid()); - log->addLogger(new LoggerUI(mainWindow->getInfoWidget())); - Log::setLogger(log); + // attach logger + LoggerComposite* log = new LoggerComposite(); + log->addLogger(new LoggerCOUT()); + log->addLogger(new LoggerAndroid()); + log->addLogger(new LoggerUI(mainWindow->getInfoWidget())); + Log::setLogger(log); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onLoadButton, this, &Controller::onLoadButton), "connect() failed"); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onDebugButton, this, &Controller::onDebugButton), "connect() failed"); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onStartButton, this, &Controller::onStartButton), "connect() failed"); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onTransparentButton, this, &Controller::onTransparentButton), "connect() failed"); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onCameraButton, this, &Controller::onCameraButton), "connect() failed"); - Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::on3DButton, this, &Controller::on3DButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onLoadButton, this, &Controller::onLoadButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onDebugButton, this, &Controller::onDebugButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onStartButton, this, &Controller::onStartButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onTransparentButton, this, &Controller::onTransparentButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onCameraButton, this, &Controller::onCameraButton), "connect() failed"); + Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::on3DButton, this, &Controller::on3DButton), "connect() failed"); - // order is important! otherwise OpenGL fails! - mainWindow->show(); + // order is important! otherwise OpenGL fails! + mainWindow->show(); // // start all sensors // SensorFactory::get().getAccelerometer().start(); @@ -120,73 +121,73 @@ Controller::Controller() { } MapView3D* Controller::getMapView3D() const { - return mainWindow->getMapView3D(); + return mainWindow->getMapView3D(); } MapView2D* Controller::getMapView2D() const { - return mainWindow->getMapView2D(); + return mainWindow->getMapView2D(); } MainMenu* Controller::getMainMenu() const { - return mainWindow->getMainMenu(); + return mainWindow->getMainMenu(); } InfoWidget* Controller::getInfoWidget() const { - return mainWindow->getInfoWidget(); + return mainWindow->getInfoWidget(); } #include #include void buildGridOnce(Grid* grid, Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& saveFile) { - //FloorplanHelper::align(map, grid->getGridSize_cm()); + //FloorplanHelper::align(map, grid->getGridSize_cm()); - // 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); + // 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 - WiFiModelLogDistCeiling wifiModel(map); - if (replyWiFiFP == QMessageBox::Yes) { + // WiFi setup + WiFiModelLogDistCeiling wifiModel(map); + if (replyWiFiFP == QMessageBox::Yes) { - 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); - WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); - 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); - } - } else { + 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); + WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); + 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); + } + } else { - // NOTE IPIN UAH map does not make sense.. APs look like VAPs but are not - // -> disable VAP-grouping in Settings.h [this is for eval and here] + // NOTE IPIN UAH map does not make sense.. APs look like VAPs but are not + // -> disable VAP-grouping in Settings.h [this is for eval and here] - // load all APs from the floorplan and use same TXP/EXP/WAF for all of them - //wifiModel.loadAPs(map, Settings::WiFiModel::vg_calib, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - Assert::isFalse(wifiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + // load all APs from the floorplan and use same TXP/EXP/WAF for all of them + //wifiModel.loadAPs(map, Settings::WiFiModel::vg_calib, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + Assert::isFalse(wifiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); - } + } - // build the grid - GridFactory gf(*grid); - gf.build(map); + // build the grid + GridFactory gf(*grid); + gf.build(map); - // add node-importance - Importance::addImportance(*grid); + // add node-importance + Importance::addImportance(*grid); - // stamp WiFi signal-strengths onto the grid - WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); + // stamp WiFi signal-strengths onto the grid + WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); - // serialize the grid - std::ofstream out(saveFile, std::ofstream::binary); - grid->write(out); - out.close(); + // serialize the grid + std::ofstream out(saveFile, std::ofstream::binary); + grid->write(out); + out.close(); } @@ -245,90 +246,90 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi void Controller::on3DButton() { - static bool use3D = false; - use3D = !use3D; + static bool use3D = false; + use3D = !use3D; - getMapView2D()->setVisible(!use3D); - getMapView3D()->setVisible( use3D); + getMapView2D()->setVisible(!use3D); + getMapView3D()->setVisible( use3D); } void Controller::onLoadButton() { - // pick a map to load - QDir dir = LoadSetupDialog::pickSetupFolder(); + // pick a map to load + QDir dir = LoadSetupDialog::pickSetupFolder(); - // cancelled? - if (dir.path() == ".") { return; } + // cancelled? + if (dir.path() == ".") { return; } - loadNavMesh(dir); + loadNavMesh(dir); } void Controller::loadGrid(QDir dir) { - QFile fMap(dir.path() + "/map.xml"); - QFile fGrid(dir.path() + "/grid.dat"); - QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); + QFile fMap(dir.path() + "/map.xml"); + QFile fGrid(dir.path() + "/grid.dat"); + QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); - Assert::isTrue(fMap.exists(), "map.xml missing"); - //Assert::isTrue(fGrid.exists(), "grid.dat missing"); + Assert::isTrue(fMap.exists(), "map.xml missing"); + //Assert::isTrue(fGrid.exists(), "grid.dat missing"); - fMap.open(QIODevice::ReadOnly); - QString str = QString(fMap.readAll()); - im = Floorplan::Reader::readFromString(str.toStdString()); + fMap.open(QIODevice::ReadOnly); + QString str = QString(fMap.readAll()); + im = Floorplan::Reader::readFromString(str.toStdString()); - const std::string sGrid = fGrid.fileName().toStdString(); - std::ifstream inp(sGrid, std::ifstream::binary); - //Assert::isTrue(inp.good(), "failed to open grid.dat"); + const std::string sGrid = fGrid.fileName().toStdString(); + std::ifstream inp(sGrid, std::ifstream::binary); + //Assert::isTrue(inp.good(), "failed to open grid.dat"); - const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); + const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); - // create a new, empty grid - if (grid) {delete grid; grid = nullptr;} - grid = new Grid(Settings::Grid::gridSize_cm); + // create a new, empty grid + if (grid) {delete grid; grid = nullptr;} + grid = new Grid(Settings::Grid::gridSize_cm); - // grid.dat empty? -> build one and save it - if (!inp.good() || (inp.peek()&&0) || inp.eof()) { - buildGridOnce(grid, im, sWiFiFP, sGrid); - } else { - grid->read(inp); - } + // grid.dat empty? -> build one and save it + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + buildGridOnce(grid, im, sWiFiFP, sGrid); + } else { + grid->read(inp); + } - // create a new navigator - if (nav) {delete nav; nav = nullptr;} - nav = new GridBased::NavControllerGrid(this, im, grid); + // create a new navigator + if (nav) {delete nav; nav = nullptr;} + nav = new GridBased::NavControllerGrid(this, im, grid); - WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); + WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); - getMapView3D()->setMap(im); - getMapView2D()->setMap(wifiCalib, im); + getMapView3D()->setMap(im); + getMapView2D()->setMap(wifiCalib, im); - getMapView3D()->showGridImportance(grid); - getMapView2D()->showGridImportance(grid); + getMapView3D()->showGridImportance(grid); + getMapView2D()->showGridImportance(grid); - getMapView3D()->setVisible(false); + getMapView3D()->setVisible(false); - // attach ipin step logger - //nav->addListener(sl); + // attach ipin step logger + //nav->addListener(sl); } void Controller::loadNavMesh(QDir dir) { - QFile fMap(dir.path() + "/map.xml"); + QFile fMap(dir.path() + "/map.xml"); QFile fGrid(dir.path() + "/grid.dat"); - QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); + QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiModel(dir.path() + "/wifimodel.dat"); - Assert::isTrue(fMap.exists(), "map.xml missing"); + Assert::isTrue(fMap.exists(), "map.xml missing"); - fMap.open(QIODevice::ReadOnly); - QString str = QString(fMap.readAll()); - im = Floorplan::Reader::readFromString(str.toStdString()); + fMap.open(QIODevice::ReadOnly); + QString str = QString(fMap.readAll()); + im = Floorplan::Reader::readFromString(str.toStdString()); - const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); + const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); const std::string sWiFiModel = fWiFiModel.fileName().toStdString(); wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel); @@ -340,37 +341,37 @@ void Controller::loadNavMesh(QDir dir) { NM::NavMeshFactory fac(navMesh, settings); fac.build(im); - // create a new navigator - if (nav) {delete nav; nav = nullptr;} + // create a new navigator + if (nav) {delete nav; nav = nullptr;} nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel); - WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); + WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); - getMapView3D()->setMap(im); - getMapView2D()->setMap(wifiCalib, im); + getMapView3D()->setMap(im); + getMapView2D()->setMap(wifiCalib, im); //getMapView3D()->showGridImportance(grid); //getMapView2D()->showGridImportance(grid); - getMapView3D()->setVisible(false); + getMapView3D()->setVisible(false); - // attach ipin step logger - //nav->addListener(sl); + // attach ipin step logger + //nav->addListener(sl); } void Controller::onDebugButton() { - mainWindow->getSensorDataWidget()->setVisible( !mainWindow->getSensorDataWidget()->isVisible() ); + mainWindow->getSensorDataWidget()->setVisible( !mainWindow->getSensorDataWidget()->isVisible() ); } void Controller::onStartButton() { - nav->start(); + nav->start(); } void Controller::onTransparentButton() { - mainWindow->getMapView3D()->toggleRenderMode(); + mainWindow->getMapView3D()->toggleRenderMode(); } void Controller::onCameraButton() { - nav->toggleCamera(); + nav->toggleCamera(); } diff --git a/Settings.h b/Settings.h index cdcbe63..e09bff2 100644 --- a/Settings.h +++ b/Settings.h @@ -39,7 +39,7 @@ namespace Settings { namespace WiFiModel { - constexpr float sigma = 8.0; //TODO: im Museum hatten wir 8.0 + 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! */ constexpr float TXP = -45; diff --git a/_android/src/BLE.java b/_android/src/BLE.java new file mode 100644 index 0000000..32ddf66 --- /dev/null +++ b/_android/src/BLE.java @@ -0,0 +1,111 @@ +package indoor.java; + +import android.app.Activity; +import android.bluetooth.BluetoothAdapter; +import android.bluetooth.BluetoothManager; +import android.bluetooth.le.BluetoothLeScanner; +import android.bluetooth.le.ScanCallback; +import android.bluetooth.le.ScanFilter; +import android.bluetooth.le.ScanSettings; +import android.content.Context; +import android.content.Intent; +import android.content.pm.PackageManager; +import android.util.Log; +import android.widget.Toast; + +import java.util.ArrayList; +import java.util.List; +import java.io.ByteArrayOutputStream; + +public class BLE { + + private static Activity act; + private static BluetoothAdapter bt = null; + private static BluetoothLeScanner scanner = null; + private static ScanCallback mLeScanCallback; + private static ScanSettings mLeSettings; + private static final List mLeFilter = new ArrayList<>(); + + /** called when a scan is completed successfully */ + public static native void onScanComplete(final byte[] result); + + /** + * start bluetooth scanning in the background. + * will call onScanComplete() once a scan is available + */ + public static int start() { + + Log.d("ble", "start()"); + + MyActivity act = MyActivity.act; + + // sanity check + if (!act.getPackageManager().hasSystemFeature(PackageManager.FEATURE_BLUETOOTH_LE)) { + Toast.makeText(act, "Bluetooth-LE not supported!", Toast.LENGTH_SHORT).show(); + return 1338; + } + + Log.d("ble", "Sanity Checks"); + + // Initializes a Bluetooth adapter. For API level 18 and above, get a reference to + // BluetoothAdapter through BluetoothManager. + final BluetoothManager mgr = (BluetoothManager) act.getSystemService(Context.BLUETOOTH_SERVICE); + bt = mgr.getAdapter(); + + Log.d("ble", "BLE Mangaer Start"); + + // create the scanner + scanner = bt.getBluetoothLeScanner(); + + // set the ble settings + mLeSettings = new ScanSettings.Builder() + .setReportDelay(0) + .setScanMode(ScanSettings.SCAN_MODE_LOW_LATENCY) + .setNumOfMatches(ScanSettings.MATCH_NUM_MAX_ADVERTISEMENT) + .build(); + + //mLeFilter.add(new ScanFilter.Builder().setServiceUuid(new ParcelUuid(UUID.fromString("fda50693-a4e2-4fb1-afcf-c6eb07647825"))).build()); + + // 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()); + + final byte[] rst = serialize(result); + BLE.onScanComplete(rst); + } + +// @Override public void onBatchScanResults(List results){ + +// for(android.bluetooth.le.ScanResult result : results){ +// Log.d("ble2", result.getDevice() + " " + result.getRssi()); + +// final byte[] rst = serialize(result); +// BLE.onScanComplete(rst); +// } +// } + }; + + if(bt == null){ + Toast.makeText(act, "Bluetooth-LE not supported!", Toast.LENGTH_SHORT).show(); + return 1338; + } + scanner.startScan(mLeFilter, mLeSettings, mLeScanCallback); + + return 1337; + } + + /** convert the given scan-result to a binary byte[] representation */ + private static byte[] serialize(final android.bluetooth.le.ScanResult res) { + final ByteArrayOutputStream baos = new ByteArrayOutputStream(); + try { + baos.write(res.getDevice().getAddress().getBytes()); + baos.write((byte)res.getRssi()); + baos.write((byte)0); + baos.write((byte)0); + } catch (final Exception e) {;} + + return baos.toByteArray(); + } + +} diff --git a/_android/src/WiFi.java b/_android/src/WiFi.java index a35e7be..2a7bf9e 100644 --- a/_android/src/WiFi.java +++ b/_android/src/WiFi.java @@ -19,8 +19,8 @@ public class WiFi { private static WifiManager manager; private static BroadcastReceiver receiver; - private static Thread tHeartbeat = null; - private static long lastScanStartTS = 0; + private static Thread tHeartbeat = null; + private static long lastScanStartTS = 0; /** called when a scan is completed successfully */ public static native void onScanComplete(final byte[] result); @@ -36,12 +36,12 @@ public class WiFi { MyActivity act = MyActivity.act; manager = (WifiManager) act.getSystemService(Context.WIFI_SERVICE); - // reset(); + // reset(); WiFi.receiver = new BroadcastReceiver() { public final void onReceive(final Context context, final Intent intent) { final byte[] result = serialize(manager.getScanResults()); - triggerOneScan(); + triggerOneScan(); WiFi.onScanComplete(result); } }; @@ -50,42 +50,42 @@ public class WiFi { act.registerReceiver(WiFi.receiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION)); // start the first scan - triggerOneScan(); + triggerOneScan(); - //this is a very nice hack. do not try this at home. - Method m = null; - try { - m = manager.getClass().getDeclaredMethod("setFrequencyBand", int.class, boolean.class); - m.setAccessible(true); - m.invoke(manager, 2, true); - m.invoke(manager, 2, true); - m.invoke(manager, 2, true); - Log.d("wifi", "HACK IS RUNNING, BIAAAATCH"); - } catch (Exception e) { - Log.d("wifi", "HACK HAS FAILED >.<"); - e.printStackTrace(); - } + //this is a very nice hack. do not try this at home. + Method m = null; + try { + m = manager.getClass().getDeclaredMethod("setFrequencyBand", int.class, boolean.class); + m.setAccessible(true); + m.invoke(manager, 2, true); + m.invoke(manager, 2, true); + m.invoke(manager, 2, true); + Log.d("wifi", "HACK IS RUNNING, BIAAAATCH"); + } catch (Exception e) { + Log.d("wifi", "HACK HAS FAILED >.<"); + e.printStackTrace(); + } - // if the scan result takes longer than X milliseconds, - // trigger another-scan to ensure nothing is stuck - final Runnable r = new Runnable() { - public void run() { - while(true) { - final long ts = System.currentTimeMillis(); - final long diff = ts - lastScanStartTS; - if (diff > 1000) { triggerOneScan(); } - try {Thread.sleep(200);} catch (final Exception e) {;} - } - } - }; + // if the scan result takes longer than X milliseconds, + // trigger another-scan to ensure nothing is stuck + final Runnable r = new Runnable() { + public void run() { + while(true) { + final long ts = System.currentTimeMillis(); + final long diff = ts - lastScanStartTS; + if (diff > 1000) { triggerOneScan(); } + try {Thread.sleep(200);} catch (final Exception e) {;} + } + } + }; - // start the heartbeat once - if (tHeartbeat == null) { - tHeartbeat = new Thread(r); - tHeartbeat.start(); - } + // start the heartbeat once + if (tHeartbeat == null) { + tHeartbeat = new Thread(r); + tHeartbeat.start(); + } - return 1337; + return 1337; } @@ -126,7 +126,7 @@ public class WiFi { try { if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");} - lastScanStartTS = System.currentTimeMillis(); + lastScanStartTS = System.currentTimeMillis(); }catch (final Exception e) { throw new RuntimeException(e); } diff --git a/nav/NavController.h b/nav/NavController.h index c831524..b612153 100644 --- a/nav/NavController.h +++ b/nav/NavController.h @@ -42,66 +42,67 @@ class Controller; class NavController : - public SensorListener, - public SensorListener, - public SensorListener, - public SensorListener, - public SensorListener, - public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, public SensorListener, public SensorListener - { + { protected: - Controller* mainController; - Floorplan::IndoorMap* im; + Controller* mainController; + Floorplan::IndoorMap* im; - bool running = false; - std::thread tFilter; - std::thread tDisplay; + bool running = false; + std::thread tFilter; + std::thread tDisplay; - /** the estimated path */ - std::vector estPath; + /** the estimated path */ + std::vector estPath; - /** all listeners */ - std::vector listeners; + /** all listeners */ + std::vector listeners; - /** display stuff */ - const int display_ms = Settings::MapView3D::msPerFrame.ms(); - Point3 curPosFast; - Point3 curPosSlow; - CurEst curEst; + /** display stuff */ + const int display_ms = Settings::MapView3D::msPerFrame.ms(); + Point3 curPosFast; + Point3 curPosSlow; + CurEst curEst; - Timestamp lastTransition; + Timestamp lastTransition; public: - NavController(Controller* mainController, Floorplan::IndoorMap* im); + NavController(Controller* mainController, Floorplan::IndoorMap* im); - virtual ~NavController() { - if (running) {stop();} - } + virtual ~NavController() { + if (running) {stop();} + } - /** attach a new event listener */ - void addListener(NavControllerListener* l) { - listeners.push_back(l); - } + /** attach a new event listener */ + void addListener(NavControllerListener* l) { + listeners.push_back(l); + } public: - virtual void stop() {;} + virtual void stop() {;} - virtual void start() = 0; + virtual void start() = 0; - int cameraMode = 0; - void toggleCamera() { - cameraMode = (cameraMode + 1) % 3; - } + int cameraMode = 0; + void toggleCamera() { + cameraMode = (cameraMode + 1) % 3; + } - /** update the map-view (called from within a background-loop) */ - void updateMapView(); + /** update the map-view (called from within a background-loop) */ + void updateMapView(); }; diff --git a/nav/Observation.h b/nav/Observation.h index bdd7e2b..5471008 100644 --- a/nav/Observation.h +++ b/nav/Observation.h @@ -2,45 +2,49 @@ #define STATE_H #include +#include #include #include /** observed sensor data */ struct MyObservation { - /** wifi measurements */ - WiFiMeasurements wifi; + /** wifi measurements */ + WiFiMeasurements wifi; - /** gps measurements */ - GPSData gps; + /** ble measurements */ + BeaconMeasurements ble; - // TODO: switch to a general activity enum/detector for barometer + accelerometer + ...? - /** detected activity */ - Activity activity; + /** gps measurements */ + GPSData gps; - /** time of evaluation */ - Timestamp currentTime; + // TODO: switch to a general activity enum/detector for barometer + accelerometer + ...? + /** detected activity */ + Activity activity; + + /** time of evaluation */ + Timestamp currentTime; }; /** (observed) control data */ struct MyControl { - /** turn angle (in radians) since the last transition */ - float turnSinceLastTransition_rad = 0; + /** turn angle (in radians) since the last transition */ + float turnSinceLastTransition_rad = 0; - /** number of steps since the last transition */ - int numStepsSinceLastTransition = 0; + /** number of steps since the last transition */ + int numStepsSinceLastTransition = 0; - // TODO: switch to a general activity enum/detector using barometer + accelerometer? - /** currently detected activity */ - Activity activity; + // TODO: switch to a general activity enum/detector using barometer + accelerometer? + /** currently detected activity */ + Activity activity; - /** reset the control-data after each transition */ - void resetAfterTransition() { - turnSinceLastTransition_rad = 0; - numStepsSinceLastTransition = 0; - } + /** reset the control-data after each transition */ + void resetAfterTransition() { + turnSinceLastTransition_rad = 0; + numStepsSinceLastTransition = 0; + } }; diff --git a/nav/grid/Filter.h b/nav/grid/Filter.h index 3c3caf9..146e2f7 100644 --- a/nav/grid/Filter.h +++ b/nav/grid/Filter.h @@ -31,6 +31,7 @@ #include "NodeResampling.h" #include "../Settings.h" +#include <../misc/fixc11.h> #include #include @@ -38,218 +39,218 @@ namespace GridBased { class PFInit : public SMC::ParticleFilterInitializer { - private: + private: - Grid* grid; + Grid* grid; - public: + public: - PFInit(Grid* grid) : grid(grid) { + PFInit(Grid* grid) : grid(grid) { - } + } 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); + std::minstd_rand gen; + std::uniform_int_distribution distIdx(0, grid->getNumNodes()-1); + std::uniform_real_distribution distHead(0, 2*M_PI); for (SMC::Particle& p : particles) { - const int idx = distIdx(gen); - const MyGridNode& node = (*grid)[idx]; - p.state.position = node; // random position - p.state.heading.direction = Heading(distHead(gen)); // random heading - p.weight = 1.0 / particles.size(); // equal weight - } + const int idx = distIdx(gen); + const MyGridNode& node = (*grid)[idx]; + p.state.position = node; // random position + p.state.heading.direction = Heading(distHead(gen)); // random heading + p.weight = 1.0 / particles.size(); // equal weight + } - // // fix position + heading + // // 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); - // } + //// 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 { - public: + public: - /** local, static control-data COPY */ - MyControl ctrl; + /** local, static control-data COPY */ + MyControl ctrl; - Grid* grid; - GridWalker walker; + Grid* grid; + GridWalker walker; - WalkModuleFavorZ modFavorZ; - WalkModuleHeadingControl modHeading; - WalkModuleNodeImportance modImportance; - WalkModuleFollowDestination modDestination; - WalkModuleActivityControl modActivity; + WalkModuleFavorZ modFavorZ; + WalkModuleHeadingControl modHeading; + WalkModuleNodeImportance modImportance; + WalkModuleFollowDestination modDestination; + WalkModuleActivityControl modActivity; - NodeResampling resampler; + NodeResampling resampler; - std::minstd_rand gen; + std::minstd_rand gen; - public: + public: - PFTrans(Grid* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) { + PFTrans(Grid* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) { - //walker.addModule(&modFavorZ); - walker.addModule(&modHeading); - //walker.addModule(&modImportance); - walker.addModule(&modActivity); + //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)); - } + if (Settings::destination != GridPoint(0,0,0)) { + //walker.addModule(&modDestination); + modDestination.setDestination(grid->getNodeFor(Settings::destination)); + } - } + } 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(); - std::normal_distribution noise(0, Settings::IMU::stepSigma); + std::normal_distribution noise(0, Settings::IMU::stepSigma); - // sanity check - Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + // sanity check + Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); //for (SMC::Particle& p : particles) { - #pragma omp parallel for num_threads(3) - for (int i = 0; i < Settings::numParticles; ++i) { + #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)); + //#pragma omp atomic + const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); SMC::Particle& p = particles[i]; - 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");} + 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");} - } + } - } + } - }; + }; class PFEval : public SMC::ParticleFilterEvaluation { - Grid* grid; + Grid* grid; - WiFiModelLogDistCeiling& wifiModel; + WiFiModelLogDistCeiling& wifiModel; - //WiFiObserverFree wiFiProbability; // free-calculation - WiFiObserverGrid wiFiProbability; // grid-calculation + //WiFiObserverFree wiFiProbability; // free-calculation + WiFiObserverGrid wiFiProbability; // grid-calculation - // smartphone is 1.3 meter above ground - const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); + // smartphone is 1.3 meter above ground + const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); - public: + public: - PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : - grid(grid), wifiModel(wifiModel), - //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free - wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid + PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : + grid(grid), wifiModel(wifiModel), + //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free + wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid - } + } double getStairProb(const SMC::Particle& p, const Activity act) { - const float kappa = 0.75; + const float kappa = 0.75; - const MyGridNode& gn = grid->getNodeFor(p.state.position); - switch (act) { + const MyGridNode& gn = grid->getNodeFor(p.state.position); + switch (act) { - case Activity::STANDING: - case Activity::WALKING: - if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} - if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} - {return 1-kappa;} + case Activity::STANDING: + case Activity::WALKING: + if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} + if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} + {return 1-kappa;} - case Activity::WALKING_UP: - case Activity::WALKING_DOWN: - if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} - if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} - {return 1-kappa;} + case Activity::WALKING_UP: + case Activity::WALKING_DOWN: + if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} + if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} + {return 1-kappa;} - } + } - return 1.0; + return 1.0; - } + } double evaluation(std::vector>& particles, const MyObservation& _observation) override { - double sum = 0; + double sum = 0; - // local copy!! observation might be changed async outside!! (will really produces crashes!) - const MyObservation observation = _observation; + // local copy!! observation might be changed async outside!! (will really produces crashes!) + 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(); + // 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)); + Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2)); - // sanity check - Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + // 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) { + #pragma omp parallel for num_threads(3) + for (int i = 0; i < Settings::numParticles; ++i) { SMC::Particle& p = particles[i]; - // WiFi free - //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); + // WiFi free + //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); - // WiFi grid - const MyGridNode& node = grid->getNodeFor(p.state.position); - const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs); + // WiFi grid + const MyGridNode& node = grid->getNodeFor(p.state.position); + const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs); - //Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms())); + //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; + 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");} + 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; + #pragma omp atomic + sum += p.weight; - } + } - return sum; + return sum; - } + } - }; + }; } diff --git a/nav/grid/NavControllerGrid.cpp b/nav/grid/NavControllerGrid.cpp index 4724fb7..a79a31f 100644 --- a/nav/grid/NavControllerGrid.cpp +++ b/nav/grid/NavControllerGrid.cpp @@ -14,131 +14,140 @@ #include "../ui/map/2D/MapView2D.h" #include "../ui/debug/InfoWidget.h" -#include "../Controller.h" +#include "../../Controller.h" Q_DECLARE_METATYPE(const void*) GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { - // filter init + // filter init std::unique_ptr> init(new PFInit(grid)); // 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)); + // resampling + std::unique_ptr> resample(new NodeResampling(*grid)); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.05)); - //std::unique_ptr resample(new RegionalResampling()); + //std::unique_ptr resample(new RegionalResampling()); - // eval and transition - wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + // 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)); - // setup the filter + // 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->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); - SensorFactory::get().getGyroscope().addListener(this); - SensorFactory::get().getBarometer().addListener(this); - SensorFactory::get().getWiFi().addListener(this); - SensorFactory::get().getSteps().addListener(this); - SensorFactory::get().getTurns().addListener(this); + // attach as listener to all sensors + SensorFactory::get().getAccelerometer().addListener(this); + SensorFactory::get().getGyroscope().addListener(this); + SensorFactory::get().getBarometer().addListener(this); + SensorFactory::get().getWiFi().addListener(this); + SensorFactory::get().getSteps().addListener(this); + SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getActivity().addListener(this); + SensorFactory::get().getBLE().addListener(this); } void GridBased::NavControllerGrid::start() { - Assert::isFalse(running, "already started!"); - running = true; - curCtrl.resetAfterTransition(); // ensure we start empty ;) - tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this); - tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this); + Assert::isFalse(running, "already started!"); + running = true; + curCtrl.resetAfterTransition(); // ensure we start empty ;) + tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this); + tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this); - // start all sensors - SensorFactory::get().getAccelerometer().start(); - SensorFactory::get().getGyroscope().start(); - SensorFactory::get().getBarometer().start(); - SensorFactory::get().getWiFi().start(); + // start all sensors + SensorFactory::get().getAccelerometer().start(); + SensorFactory::get().getGyroscope().start(); + SensorFactory::get().getBarometer().start(); + SensorFactory::get().getWiFi().start(); + SensorFactory::get().getBLE().start(); #ifndef ANDROID - // #include - // run with - // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin - // show with - // kcachegrind callgrind.out.xxxx - CALLGRIND_START_INSTRUMENTATION; + // #include + // run with + // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin + // show with + // kcachegrind callgrind.out.xxxx + CALLGRIND_START_INSTRUMENTATION; #endif } void GridBased::NavControllerGrid::stop() { - Assert::isTrue(running, "not started!"); - running = false; - tFilter.join(); - tDisplay.join(); + Assert::isTrue(running, "not started!"); + running = false; + tFilter.join(); + tDisplay.join(); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) { - (void) sensor; - (void) data; - (void) ts; - gotSensorData(ts); + (void) sensor; + (void) data; + (void) ts; + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) { - (void) sensor; - (void) ts; - curObs.wifi = data; - gotSensorData(ts); + (void) sensor; + (void) ts; + curObs.wifi = data; + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) { - (void) sensor; - (void) ts; - curObs.gps = data; - gotSensorData(ts); + (void) sensor; + (void) ts; + curObs.gps = data; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const BeaconMeasurement& data) { + (void) sensor; + (void) ts; + //Log::add("Beacon", "MAC: " + data.getBeacon().getMAC().asString() + "RSSI: " + std::to_string(data.getRSSI())); + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) { - (void) sensor; - (void) ts; - curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition - gotSensorData(ts); + (void) sensor; + (void) ts; + curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) { - (void) sensor; - (void) ts; - curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition - gotSensorData(ts); + (void) sensor; + (void) ts; + curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition + gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { @@ -152,8 +161,8 @@ void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, co /** called when any sensor has received new data */ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { - curObs.currentTime = ts; - if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} + curObs.currentTime = ts; + if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} } // void debugActivity(const ActivityData& activity) { @@ -167,11 +176,11 @@ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { // Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); // } - /** particle-filter update loop */ - void GridBased::NavControllerGrid::filterUpdateLoop() { + /** particle-filter update loop */ + void GridBased::NavControllerGrid::filterUpdateLoop() { - while(running && !Settings::Filter::useMainThread) { + while(running && !Settings::Filter::useMainThread) { // // fixed update rate based on the systems time -> LIVE! even for offline data // const Timestamp ts1 = Timestamp::fromUnixTime(); @@ -181,95 +190,95 @@ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { // const Timestamp sleep = Timestamp::fromMS(500) - needed; // std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms())); - const bool wasUpdated = filterUpdateIfNeeded(); - if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } + const bool wasUpdated = filterUpdateIfNeeded(); + if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - } - } + } + } - /** check whether its time for a filter update, and if so, execute the update and return true */ - bool GridBased::NavControllerGrid::filterUpdateIfNeeded() { + /** check whether its time for a filter update, and if so, execute the update and return true */ + bool GridBased::NavControllerGrid::filterUpdateIfNeeded() { - static float avgSum = 0; - static int avgCount = 0; + static float avgSum = 0; + static int avgCount = 0; - // fixed update rate based on incoming sensor data - // allows working with live data and faster for offline data - const Timestamp diff = curObs.currentTime - lastTransition; - if (diff >= Settings::Filter::updateEvery) { + // fixed update rate based on incoming sensor data + // allows working with live data and faster for offline data + const Timestamp diff = curObs.currentTime - lastTransition; + if (diff >= Settings::Filter::updateEvery) { - // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one - const Timestamp err = diff - Settings::Filter::updateEvery; - lastTransition = curObs.currentTime - err; + // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one + const Timestamp err = diff - Settings::Filter::updateEvery; + lastTransition = curObs.currentTime - err; - const Timestamp ts1 = Timestamp::fromUnixTime(); - filterUpdate(); - const Timestamp ts2 = Timestamp::fromUnixTime(); - const Timestamp tsDiff = ts2-ts1; - const QString filterTime = QString::number(tsDiff.ms()); - avgSum += tsDiff.ms(); ++avgCount; - //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); - QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); - return true; + const Timestamp ts1 = Timestamp::fromUnixTime(); + filterUpdate(); + const Timestamp ts2 = Timestamp::fromUnixTime(); + const Timestamp tsDiff = ts2-ts1; + const QString filterTime = QString::number(tsDiff.ms()); + avgSum += tsDiff.ms(); ++avgCount; + //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); + QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); + return true; - } else { + } else { - return false; + return false; - } + } - } + } - /** perform a filter-update (called from a background-loop) */ - void GridBased::NavControllerGrid::filterUpdate() { + /** perform a filter-update (called from a background-loop) */ + void GridBased::NavControllerGrid::filterUpdate() { - //lastEst = curEst; - MyState sCurEst = pf->update(&curCtrl, curObs); - curEst.pos_m = sCurEst.position.inMeter(); - curEst.head = sCurEst.heading.direction; - //Log::add("Nav", "cur est: " + curEst.position.asString()); + //lastEst = curEst; + MyState sCurEst = pf->update(&curCtrl, curObs); + curEst.pos_m = sCurEst.position.inMeter(); + curEst.head = sCurEst.heading.direction; + //Log::add("Nav", "cur est: " + curEst.position.asString()); - // inform listeners about the new estimation - for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);} + // inform listeners about the new estimation + for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);} - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); - // update estimated path - estPath.push_back(curEst.pos_m); - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); + // update estimated path + estPath.push_back(curEst.pos_m); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); - PFTrans* trans = (PFTrans*)pf->getTransition(); - const MyGridNode* node = grid->getNodePtrFor(sCurEst.position); - if (node) { - try { - pathToDest = trans->modDestination.getShortestPath(*node); - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); - Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); - } catch (...) {;} - } - // mainController->getMapView()->showGridImportance(); + PFTrans* trans = (PFTrans*)pf->getTransition(); + const MyGridNode* node = grid->getNodePtrFor(sCurEst.position); + if (node) { + try { + pathToDest = trans->modDestination.getShortestPath(*node); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); + Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); + } catch (...) {;} + } + // mainController->getMapView()->showGridImportance(); - } + } - /** UI update loop */ - void GridBased::NavControllerGrid::updateMapViewLoop() { + /** UI update loop */ + void GridBased::NavControllerGrid::updateMapViewLoop() { - while(running) { - const Timestamp ts1 = Timestamp::fromUnixTime(); - updateMapView(); - const Timestamp ts2 = Timestamp::fromUnixTime(); - const Timestamp tsDiff = ts2-ts1; - const QString mapViewTime = QString::number(tsDiff.ms()); - //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); - std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); - } - } + while(running) { + const Timestamp ts1 = Timestamp::fromUnixTime(); + updateMapView(); + const Timestamp ts2 = Timestamp::fromUnixTime(); + const Timestamp tsDiff = ts2-ts1; + const QString mapViewTime = QString::number(tsDiff.ms()); + //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); + std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); + } + } diff --git a/nav/grid/NavControllerGrid.h b/nav/grid/NavControllerGrid.h index ed985c4..5f13476 100644 --- a/nav/grid/NavControllerGrid.h +++ b/nav/grid/NavControllerGrid.h @@ -21,68 +21,70 @@ namespace GridBased { - class NavControllerGrid : public NavController { + class NavControllerGrid : public NavController { - private: + private: - Grid* grid; - WiFiModelLogDistCeiling wifiModel; + Grid* grid; + WiFiModelLogDistCeiling wifiModel; std::unique_ptr> pf; - DijkstraPath pathToDest; + DijkstraPath pathToDest; - MyObservation curObs; - MyControl curCtrl; + MyObservation curObs; + MyControl curCtrl; - public: + public: - NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid); + NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid); - void start() override; + void start() override; - void stop() override; + void stop() override; - void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const BeaconMeasurement& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override ; + void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override ; + + void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; - private: + private: - /** called when any sensor has received new data */ - void gotSensorData(const Timestamp ts); + /** called when any sensor has received new data */ + void gotSensorData(const Timestamp ts); - // void debugActivity(const ActivityData& activity); + // void debugActivity(const ActivityData& activity); - /** particle-filter update loop */ - void filterUpdateLoop(); + /** particle-filter update loop */ + void filterUpdateLoop(); - /** check whether its time for a filter update, and if so, execute the update and return true */ - bool filterUpdateIfNeeded(); + /** check whether its time for a filter update, and if so, execute the update and return true */ + bool filterUpdateIfNeeded(); - /** perform a filter-update (called from a background-loop) */ - void filterUpdate(); + /** perform a filter-update (called from a background-loop) */ + void filterUpdate(); - /** UI update loop */ - void updateMapViewLoop(); + /** UI update loop */ + void updateMapViewLoop(); - }; + }; } diff --git a/nav/mesh/FilterMesh.h b/nav/mesh/FilterMesh.h index 7185aee..5a2ca44 100644 --- a/nav/mesh/FilterMesh.h +++ b/nav/mesh/FilterMesh.h @@ -32,33 +32,33 @@ namespace MeshBased { - class PFInit : public SMC::ParticleFilterInitializer { + class PFInit : public SMC::ParticleFilterInitializer { private: - const NM::NavMesh* mesh; + const NM::NavMesh* mesh; 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); NM::NavMeshRandom rnd = mesh->getRandom(); - for (SMC::Particle& p : particles) { - p.state.loc = rnd.draw(); + 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) { + // 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,109 +71,109 @@ namespace MeshBased { }; - class PFTrans : public SMC::ParticleFilterTransition { + class PFTrans : public SMC::ParticleFilterTransition { 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 + // local, static control-data COPY MyControl ctrl; 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(); - // 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.loc; + // 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.loc = 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) { + double getStairProb(const SMC::Particle& p, const Activity act) { - const float kappa = 0.9; + const float kappa = 0.9; - switch (act) { + 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: + 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;} - 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;} - } + 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;} + } - return 1.0; - } + return 1.0; + } 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) : 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){} + //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; @@ -181,32 +181,32 @@ namespace MeshBased { const MyObservation observation = _observation; // vap-grouping - const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + 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!"); + //Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); - // assign weights - #pragma omp parallel for num_threads(3) - for (size_t i = 0; i < particles.size(); ++i) { - SMC::Particle& p = particles[i]; + // 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 pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs); - const double pStair = getStairProb(p, observation.activity); - const double pGPS = 1; + 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. + //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 = pWifi * pStair * pGPS; - p.weight *= prob; - if (p.weight != p.weight) {throw Exception("nan");} + p.weight *= prob; + if (p.weight != p.weight) {throw Exception("nan");} - #pragma omp atomic - sum += p.weight; - } + #pragma omp atomic + sum += p.weight; + } - return sum; + return sum; } diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp index de71215..b9da1bf 100644 --- a/nav/mesh/NavControllerMesh.cpp +++ b/nav/mesh/NavControllerMesh.cpp @@ -23,7 +23,7 @@ #include #include -#include +//#include //#ifndef ANDROID //#include @@ -37,7 +37,7 @@ Q_DECLARE_METATYPE(const void*) MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel) : NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) { - // filter init + // filter init std::unique_ptr> init(new MeshBased::PFInit(navMesh)); // estimation @@ -47,11 +47,11 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo // resampling - std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingKDE(navMesh, 0.2, Point2(1,1))); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingKLD()); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.95)); - //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); + std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); // eval and transition std::unique_ptr> eval(new MeshBased::PFEval(wifiModel)); @@ -68,35 +68,37 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo //pf->setNEffThreshold(0.65); // still too low? //pf->setNEffThreshold(0.25); // too low - // attach as listener to all sensors - SensorFactory::get().getAccelerometer().addListener(this); - SensorFactory::get().getGyroscope().addListener(this); - SensorFactory::get().getBarometer().addListener(this); - SensorFactory::get().getWiFi().addListener(this); - SensorFactory::get().getSteps().addListener(this); - SensorFactory::get().getTurns().addListener(this); + // attach as listener to all sensors + SensorFactory::get().getAccelerometer().addListener(this); + SensorFactory::get().getGyroscope().addListener(this); + SensorFactory::get().getBarometer().addListener(this); + SensorFactory::get().getWiFi().addListener(this); + SensorFactory::get().getSteps().addListener(this); + SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getActivity().addListener(this); + SensorFactory::get().getBLE().addListener(this); - // hacky.. but we need to call this one from the main thread! - //mainController->getMapView()->showParticles(pf->getParticles()); - qRegisterMetaType(); + // hacky.. but we need to call this one from the main thread! + //mainController->getMapView()->showParticles(pf->getParticles()); + qRegisterMetaType(); } void MeshBased::NavControllerMesh::start() { - Assert::isFalse(running, "already started!"); - running = true; - curCtrl.resetAfterTransition(); // ensure we start empty ;) - tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this); - tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this); + Assert::isFalse(running, "already started!"); + running = true; + curCtrl.resetAfterTransition(); // ensure we start empty ;) + tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this); + tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this); - // start all sensors - SensorFactory::get().getAccelerometer().start(); - SensorFactory::get().getGyroscope().start(); - SensorFactory::get().getBarometer().start(); - SensorFactory::get().getWiFi().start(); + // start all sensors + SensorFactory::get().getAccelerometer().start(); + SensorFactory::get().getGyroscope().start(); + SensorFactory::get().getBarometer().start(); + SensorFactory::get().getWiFi().start(); + SensorFactory::get().getBLE().start(); //#ifndef ANDROID // // #include @@ -110,60 +112,67 @@ void MeshBased::NavControllerMesh::start() { } void MeshBased::NavControllerMesh::stop() { - Assert::isTrue(running, "not started!"); - running = false; - tFilter.join(); - tDisplay.join(); + Assert::isTrue(running, "not started!"); + running = false; + tFilter.join(); + tDisplay.join(); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) { - (void) sensor; - (void) data; - (void) ts; - gotSensorData(ts); + (void) sensor; + (void) data; + (void) ts; + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) { - (void) sensor; - (void) ts; - curObs.wifi = data; - gotSensorData(ts); + (void) sensor; + (void) ts; + curObs.wifi = data; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const BeaconMeasurement& data) { + (void) sensor; + (void) ts; + curObs.ble.add(data); + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) { - (void) sensor; - (void) ts; - curObs.gps = data; - gotSensorData(ts); + (void) sensor; + (void) ts; + curObs.gps = data; + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) { - (void) sensor; - (void) ts; - curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition - gotSensorData(ts); + (void) sensor; + (void) ts; + curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) { - (void) sensor; - (void) ts; - curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition - gotSensorData(ts); + (void) sensor; + (void) ts; + curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition + gotSensorData(ts); } void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { @@ -177,8 +186,8 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, co /** called when any sensor has received new data */ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { - curObs.currentTime = ts; - if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} + curObs.currentTime = ts; + if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} } void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) { @@ -193,11 +202,11 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); } - /** particle-filter update loop */ - void MeshBased::NavControllerMesh::filterUpdateLoop() { + /** particle-filter update loop */ + void MeshBased::NavControllerMesh::filterUpdateLoop() { - while(running && !Settings::Filter::useMainThread) { + while(running && !Settings::Filter::useMainThread) { // // fixed update rate based on the systems time -> LIVE! even for offline data // const Timestamp ts1 = Timestamp::fromUnixTime(); @@ -207,37 +216,37 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) // const Timestamp sleep = Timestamp::fromMS(500) - needed; // std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms())); - const bool wasUpdated = filterUpdateIfNeeded(); - if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } + const bool wasUpdated = filterUpdateIfNeeded(); + if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - } - } + } + } - Timestamp lastTransition; + Timestamp lastTransition; - /** check whether its time for a filter update, and if so, execute the update and return true */ - bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() { + /** check whether its time for a filter update, and if so, execute the update and return true */ + bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() { - static float avgSum = 0; - static int avgCount = 0; + static float avgSum = 0; + static int avgCount = 0; const Timestamp diff = curObs.currentTime - lastTransition; if (curCtrl.numStepsSinceLastTransition > 0){ - // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one - const Timestamp err = diff - Settings::Filter::updateEvery; + // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one + const Timestamp err = diff - Settings::Filter::updateEvery; lastTransition = curObs.currentTime; - const Timestamp ts1 = Timestamp::fromUnixTime(); + const Timestamp ts1 = Timestamp::fromUnixTime(); filterUpdate(); - const Timestamp ts2 = Timestamp::fromUnixTime(); - const Timestamp tsDiff = ts2-ts1; + const Timestamp ts2 = Timestamp::fromUnixTime(); + const Timestamp tsDiff = ts2-ts1; const QString filterTime = QString::number(diff.ms()); - avgSum += tsDiff.ms(); ++avgCount; - //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); + avgSum += tsDiff.ms(); ++avgCount; + //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); - return true; + return true; } else if(diff >= Timestamp::fromMS(1000)) { @@ -251,19 +260,19 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) } else { - return false; + return false; - } + } - } + } - DijkstraPath pathToDest; + DijkstraPath pathToDest; void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() { //lastEst = curEst; MyState sCurEst = pf->updateEvaluationOnly(curObs); - curEst.pos_m = sCurEst.loc.pos; + curEst.pos_m = sCurEst.pos.pos; curEst.head = sCurEst.heading; // inform listeners about the new estimation @@ -278,12 +287,12 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); } - /** perform a filter-update (called from a background-loop) */ - void MeshBased::NavControllerMesh::filterUpdate() { + /** perform a filter-update (called from a background-loop) */ + void MeshBased::NavControllerMesh::filterUpdate() { //lastEst = curEst; MyState sCurEst = pf->update(&curCtrl, curObs); - curEst.pos_m = sCurEst.loc.pos; + curEst.pos_m = sCurEst.pos.pos; curEst.head = sCurEst.heading; // inform listeners about the new estimation @@ -309,18 +318,18 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) // } // mainController->getMapView()->showGridImportance(); - } + } - /** UI update loop */ - void MeshBased::NavControllerMesh::updateMapViewLoop() { + /** UI update loop */ + void MeshBased::NavControllerMesh::updateMapViewLoop() { - while(running) { - const Timestamp ts1 = Timestamp::fromUnixTime(); - updateMapView(); - const Timestamp ts2 = Timestamp::fromUnixTime(); - const Timestamp tsDiff = ts2-ts1; - const QString mapViewTime = QString::number(tsDiff.ms()); - //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); - std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); - } - } + while(running) { + const Timestamp ts1 = Timestamp::fromUnixTime(); + updateMapView(); + const Timestamp ts2 = Timestamp::fromUnixTime(); + const Timestamp tsDiff = ts2-ts1; + const QString mapViewTime = QString::number(tsDiff.ms()); + //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); + std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); + } + } diff --git a/nav/mesh/NavControllerMesh.h b/nav/mesh/NavControllerMesh.h index 5c1845d..7021bb3 100644 --- a/nav/mesh/NavControllerMesh.h +++ b/nav/mesh/NavControllerMesh.h @@ -24,68 +24,70 @@ namespace MeshBased { - class NavControllerMesh : public NavController { + class NavControllerMesh : public NavController { - private: + private: - NM::NavMesh* navMesh; + NM::NavMesh* navMesh; WiFiModel* wifiModel; std::unique_ptr> pf; - MyObservation curObs; - MyControl curCtrl; + MyObservation curObs; + MyControl curCtrl; - public: + public: NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh, WiFiModel* wifiModel); - void start() override; + void start() override; - void stop() override; + void stop() override; - void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const BeaconMeasurement& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; - void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; + void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; - private: + private: - /** called when any sensor has received new data */ - void gotSensorData(const Timestamp ts); + /** called when any sensor has received new data */ + void gotSensorData(const Timestamp ts); void debugActivity(const ActivityData& activity); - /** particle-filter update loop */ - void filterUpdateLoop(); + /** particle-filter update loop */ + void filterUpdateLoop(); - /** check whether its time for a filter update, and if so, execute the update and return true */ - bool filterUpdateIfNeeded(); + /** check whether its time for a filter update, and if so, execute the update and return true */ + bool filterUpdateIfNeeded(); - /** perform a filter-update (called from a background-loop) */ - void filterUpdate(); + /** perform a filter-update (called from a background-loop) */ + void filterUpdate(); /** perform a filter-update only with estimation (called from a background-loop) */ void filterUpdateEstimationOnly(); - /** UI update loop */ - void updateMapViewLoop(); + /** UI update loop */ + void updateMapViewLoop(); - }; + }; } diff --git a/nav/mesh/State.h b/nav/mesh/State.h index 9faef92..4c796e1 100644 --- a/nav/mesh/State.h +++ b/nav/mesh/State.h @@ -9,60 +9,60 @@ namespace MeshBased { struct MyState { - NM::NavMeshLocation loc; + NM::NavMeshLocation pos; Heading heading; /** ctor */ - MyState() : loc(), heading(0) { + MyState() : pos(), heading(0) { ; } /** ctor */ - MyState(NM::NavMeshLocation loc, Heading h) : loc(loc), heading(h) { + MyState(NM::NavMeshLocation loc, Heading h) : pos(loc), heading(h) { ; } - MyState& operator += (const MyState& o) { - loc.pos += o.loc.pos; - return *this; - } + MyState& operator += (const MyState& o) { + pos.pos += o.pos.pos; + return *this; + } - MyState& operator /= (const float val) { - loc.pos /= val; - return *this; - } + MyState& operator /= (const float val) { + pos.pos /= val; + return *this; + } - MyState operator * (const float val) const { - MyState copy = *this; - copy.loc.pos = copy.loc.pos * val; - return copy; - } + MyState operator * (const float val) const { + MyState copy = *this; + copy.pos.pos = copy.pos.pos * val; + return copy; + } - float getX(){ - return loc.pos.x; - } + float getX(){ + return pos.pos.x; + } - float getY() { - return loc.pos.y; - } + float getY() { + return pos.pos.y; + } - float getZ() { - return loc.pos.z; - } + float getZ() { + return pos.pos.z; + } - void setPosition(Point3 pos){ - loc.pos = pos; - } + void setPosition(Point3 other){ + pos.pos = other; + } - float getBinValue(const int dim) const { - switch (dim) { - case 0: return this->loc.pos.x; - case 1: return this->loc.pos.y; - case 2: return this->loc.pos.z; - case 3: return this->heading.getRAD(); - } - throw "cant find this value within the bin"; - } + float getBinValue(const int dim) const { + switch (dim) { + case 0: return this->pos.pos.x; + case 1: return this->pos.pos.y; + case 2: return this->pos.pos.z; + case 3: return this->heading.getRAD(); + } + throw "cant find this value within the bin"; + } }; diff --git a/sensors/SensorFactory.h b/sensors/SensorFactory.h index 6f5989d..e215324 100644 --- a/sensors/SensorFactory.h +++ b/sensors/SensorFactory.h @@ -8,6 +8,9 @@ #include "linux/WiFiSensorLinux.h" #include "android/WiFiSensorAndroid.h" +#include "BLESensor.h" +#include "android/BLESensorAndroid.h" + #include "AccelerometerSensor.h" #include "dummy/AccelerometerSensorDummy.h" #include "android/AccelerometerSensorAndroid.h" @@ -37,58 +40,61 @@ class SensorFactory { private: - /** this one is a dirty hack, as static class member variables do not work header-only */ - static SensorFactory** getPtr() { - static SensorFactory* ptr = nullptr; - return &ptr; - } + /** this one is a dirty hack, as static class member variables do not work header-only */ + static SensorFactory** getPtr() { + static SensorFactory* ptr = nullptr; + return &ptr; + } public: - /** set the to-be-used sensor-fatory */ - static void set(SensorFactory* fac) { - Assert::isNull(*getPtr(), "SensorFactory::set() was already called. currentely this is not intended"); - *getPtr() = fac; - } + /** set the to-be-used sensor-fatory */ + static void set(SensorFactory* fac) { + Assert::isNull(*getPtr(), "SensorFactory::set() was already called. currentely this is not intended"); + *getPtr() = fac; + } - /** get the currently configured sensory factory */ - static SensorFactory& get() { - Assert::isNotNull(*getPtr(), "call SensorFactory::set() first to set an actual factory instance!"); - return **getPtr(); - } + /** get the currently configured sensory factory */ + static SensorFactory& get() { + Assert::isNotNull(*getPtr(), "call SensorFactory::set() first to set an actual factory instance!"); + return **getPtr(); + } public: - /** get the WiFi sensor */ - virtual WiFiSensor& getWiFi() = 0; + /** get the WiFi sensor */ + virtual WiFiSensor& getWiFi() = 0; - /** get the Accelerometer sensor */ - virtual AccelerometerSensor& getAccelerometer() = 0; + /** get the Accelerometer sensor */ + virtual AccelerometerSensor& getAccelerometer() = 0; - /** get the Gyroscope sensor */ - virtual GyroscopeSensor& getGyroscope() = 0; + /** get the Gyroscope sensor */ + virtual GyroscopeSensor& getGyroscope() = 0; - /** get the Barometer sensor */ - virtual BarometerSensor& getBarometer() = 0; + /** get the Barometer sensor */ + virtual BarometerSensor& getBarometer() = 0; - /** get the compass sensor */ - virtual CompassSensor& getCompass() = 0; + /** get the compass sensor */ + virtual CompassSensor& getCompass() = 0; - /** get the gps sensor */ - virtual GPSSensor& getGPS() = 0; + /** get the gps sensor */ + virtual GPSSensor& getGPS() = 0; + + /** get the bluetooth low energy sensor */ + virtual BLESensor& getBLE() = 0; - /** get the Step sensor */ - StepSensor& getSteps() { - static StepSensor steps(getAccelerometer()); - return steps; - } + /** get the Step sensor */ + StepSensor& getSteps() { + static StepSensor steps(getAccelerometer()); + return steps; + } - /** get the Turn sensor */ - TurnSensor& getTurns() { - static TurnSensor turns(getAccelerometer(), getGyroscope()); - return turns; - } + /** get the Turn sensor */ + TurnSensor& getTurns() { + static TurnSensor turns(getAccelerometer(), getGyroscope()); + return turns; + } /** get the Activity sensor */ ActivitySensor& getActivity() { diff --git a/sensors/SensorWriter.h b/sensors/SensorWriter.h index b1c2279..cf30176 100644 --- a/sensors/SensorWriter.h +++ b/sensors/SensorWriter.h @@ -19,82 +19,81 @@ * writes it to a file */ class SensorWriter : - public SensorListener, - public SensorListener, - public SensorListener, + public SensorListener, + public SensorListener, + public SensorListener, // public SensorListener, // public SensorListener, // public SensorListener, - public SensorListener, - public SensorListener, - public SensorListener { + public SensorListener, + public SensorListener, + public SensorListener { private: - Offline::FileWriter writer; - bool active = false; + Offline::FileWriter writer; + bool active = false; public: - /** empty ctor */ - SensorWriter() { + /** empty ctor */ + SensorWriter() { - // attach as listener to all sensors we want to store - SensorFactory::get().getAccelerometer().addListener(this); - SensorFactory::get().getGyroscope().addListener(this); - SensorFactory::get().getBarometer().addListener(this); + // attach as listener to all sensors we want to store + SensorFactory::get().getAccelerometer().addListener(this); + SensorFactory::get().getGyroscope().addListener(this); + SensorFactory::get().getBarometer().addListener(this); // SensorFactory::get().getActivity().addListener(this); // SensorFactory::get().getSteps().addListener(this); // SensorFactory::get().getTurns().addListener(this); - SensorFactory::get().getWiFi().addListener(this); - SensorFactory::get().getCompass().addListener(this); - SensorFactory::get().getGPS().addListener(this); - - } + SensorFactory::get().getWiFi().addListener(this); + SensorFactory::get().getCompass().addListener(this); + SensorFactory::get().getGPS().addListener(this); + } - void start(const std::string& file) { - writer.open(file); - active = true; - } + void start(const std::string& file) { + writer.open(file); + active = true; + } - void stop() { - active = false; - writer.close(); - } + void stop() { + active = false; + writer.close(); + } public: - virtual void onSensorData(Sensor* , const Timestamp ts, const AccelerometerData& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const AccelerometerData& data) override { + if (!active) {return;} + writer.add(ts, data); + } - virtual void onSensorData(Sensor* , const Timestamp ts, const GyroscopeData& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const GyroscopeData& data) override { + if (!active) {return;} + writer.add(ts, data); + } - virtual void onSensorData(Sensor* , const Timestamp ts, const BarometerData& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const BarometerData& data) override { + if (!active) {return;} + writer.add(ts, data); + } - virtual void onSensorData(Sensor* , const Timestamp ts, const WiFiMeasurements& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const WiFiMeasurements& data) override { + if (!active) {return;} + writer.add(ts, data); + } - virtual void onSensorData(Sensor* , const Timestamp ts, const GPSData& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const GPSData& data) override { + if (!active) {return;} + writer.add(ts, data); + } - virtual void onSensorData(Sensor* , const Timestamp ts, const CompassData& data) override { - if (!active) {return;} - writer.add(ts, data); - } + virtual void onSensorData(Sensor* , const Timestamp ts, const CompassData& data) override { + if (!active) {return;} + writer.add(ts, data); + } }; diff --git a/sensors/TurnSensor.h b/sensors/TurnSensor.h index 2075b20..64506e8 100644 --- a/sensors/TurnSensor.h +++ b/sensors/TurnSensor.h @@ -1,6 +1,7 @@ #ifndef TURNSENSOR_H #define TURNSENSOR_H +#include #include #include "AccelerometerSensor.h" #include "GyroscopeSensor.h" diff --git a/sensors/android/BLESensorAndroid.cpp b/sensors/android/BLESensorAndroid.cpp new file mode 100644 index 0000000..600f68c --- /dev/null +++ b/sensors/android/BLESensorAndroid.cpp @@ -0,0 +1,18 @@ +#ifdef ANDROID + +#include "BLESensorAndroid.h" + +extern "C" { + + /** called after each successful WiFi scan */ + JNIEXPORT void JNICALL Java_indoor_java_BLE_onScanComplete(JNIEnv* env, jobject jobj, jbyteArray arrayID) { + (void) env; (void) jobj; + jsize length = env->GetArrayLength(arrayID); + jbyte* data = env->GetByteArrayElements(arrayID, 0); + std::string str((char*)data, length); + env->ReleaseByteArrayElements(arrayID, data, JNI_ABORT); + BLESensorAndroid::get().handle(str); + } +} + +#endif diff --git a/sensors/android/SensorFactoryAndroid.h b/sensors/android/SensorFactoryAndroid.h index 8729c02..83959f8 100644 --- a/sensors/android/SensorFactoryAndroid.h +++ b/sensors/android/SensorFactoryAndroid.h @@ -5,6 +5,7 @@ #include "../SensorFactory.h" +#include "BLESensorAndroid.h" #include "WiFiSensorAndroid.h" #include "AccelerometerSensorAndroid.h" #include "GyroscopeSensorAndroid.h" @@ -20,29 +21,33 @@ class SensorFactoryAndroid : public SensorFactory { public: - WiFiSensor& getWiFi() override { - return WiFiSensorAndroid::get(); - } + WiFiSensor& getWiFi() override { + return WiFiSensorAndroid::get(); + } - AccelerometerSensor& getAccelerometer() override { - return AccelerometerSensorAndroid::get(); - } + AccelerometerSensor& getAccelerometer() override { + return AccelerometerSensorAndroid::get(); + } - GyroscopeSensor& getGyroscope() override { - return GyroscopeSensorAndroid::get(); - } + GyroscopeSensor& getGyroscope() override { + return GyroscopeSensorAndroid::get(); + } - BarometerSensor& getBarometer() override { - return BarometerSensorAndroid::get(); - } + BarometerSensor& getBarometer() override { + return BarometerSensorAndroid::get(); + } - CompassSensor& getCompass() override { - return CompassSensorAndroid::get(); - } + CompassSensor& getCompass() override { + return CompassSensorAndroid::get(); + } - GPSSensor& getGPS() override { - return GPSSensorAndroid::get(); - } + GPSSensor& getGPS() override { + return GPSSensorAndroid::get(); + } + + BLESensor& getBLE() override { + return BLESensorAndroid::get(); + } }; diff --git a/tools/calibration/BLECalibrationDataModel.h b/tools/calibration/BLECalibrationDataModel.h new file mode 100644 index 0000000..3df3497 --- /dev/null +++ b/tools/calibration/BLECalibrationDataModel.h @@ -0,0 +1,4 @@ +#ifndef BLECALIBRATIONDATAMODEL_H +#define BLECALIBRATIONDATAMODEL_H + +#endif // BLECALIBRATIONDATAMODEL_H diff --git a/tools/calibration/WiFiCalibrationScanDialog.h b/tools/calibration/WiFiCalibrationScanDialog.h index 840a708..489d251 100644 --- a/tools/calibration/WiFiCalibrationScanDialog.h +++ b/tools/calibration/WiFiCalibrationScanDialog.h @@ -16,84 +16,94 @@ /** * show a dialog to perform a WiFiScan */ -class WiFiCalibrationScanDialog : public QObject, public SensorListener { +class WiFiCalibrationScanDialog : public QObject, public SensorListener, public SensorListener { - Q_OBJECT + Q_OBJECT private: - QDialog* dlg = new QDialog(); - QLabel* lblStats; - QLabel* lblPoint; - QProgressBar* barProg; + QDialog* dlg = new QDialog(); + QLabel* lblStats; + QLabel* lblPoint; + QProgressBar* barProg; - struct Scan { - const int numRecords = 30; - int recordsDone = 0; - } scan; + struct Scan { + const int numRecords = 30; + int recordsDone = 0; + } scan; - /** the measurements model to fill with scan-entries */ - WiFiFingerprint& model; + /** the measurements model to fill with scan-entries */ + WiFiFingerprint& model; + //BeaconFingerprint& blemodel; public: - static void get(WiFiFingerprint& model) { - WiFiCalibrationScanDialog dlg(model); - dlg.show(); - } + static void get(WiFiFingerprint& model) { + WiFiCalibrationScanDialog dlg(model); + dlg.show(); + } private: - /** ctor */ - WiFiCalibrationScanDialog(WiFiFingerprint& model); + /** ctor */ + WiFiCalibrationScanDialog(WiFiFingerprint& model); - void show() { - refresh(); - dlg->exec(); - } + void show() { + refresh(); + dlg->exec(); + } - void close() { - stopRecord(); - dlg->close(); - } + void close() { + stopRecord(); + dlg->close(); + } - void save() { - //mdl.save();; - } + void save() { + //mdl.save();; + } - Q_INVOKABLE void refresh() { - lblPoint->setText(QString(model.pos_m.asString().c_str())); - lblStats->setText( QString::number(model.measurements.entries.size()) + " RSSI measurements\n" + QString::number(scan.recordsDone) + " scans"); - barProg->setValue(scan.recordsDone); - barProg->setMaximum(scan.numRecords); - } + Q_INVOKABLE void refresh() { + lblPoint->setText(QString(model.pos_m.asString().c_str())); + lblStats->setText( QString::number(model.measurements.entries.size()) + " RSSI measurements\n" + QString::number(scan.recordsDone) + " scans"); + barProg->setValue(scan.recordsDone); + barProg->setMaximum(scan.numRecords); + } - void clear() { - model.measurements.entries.clear(); - refresh(); - } + void clear() { + model.measurements.entries.clear(); + refresh(); + } - void startRecord() { - scan.recordsDone = 0; - SensorFactory::get().getWiFi().addListener(this); - if (!SensorFactory::get().getWiFi().isRunning()) { - SensorFactory::get().getWiFi().start(); - } - } + void startRecord() { + scan.recordsDone = 0; + SensorFactory::get().getWiFi().addListener(this); + if (!SensorFactory::get().getWiFi().isRunning()) { + SensorFactory::get().getWiFi().start(); + } + } - void stopRecord() { - SensorFactory::get().getWiFi().removeListener(this); - } + void stopRecord() { + SensorFactory::get().getWiFi().removeListener(this); + } - virtual void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override { - (void) sensor; - (void) ts; - ++scan.recordsDone; - if (scan.recordsDone >= scan.numRecords) {stopRecord();} - model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end()); - QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection); - } + virtual void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override { + (void) sensor; + (void) ts; + ++scan.recordsDone; + if (scan.recordsDone >= scan.numRecords) {stopRecord();} + model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end()); + QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection); + } + + //tmp stuff + virtual void onSensorData(Sensor* sensor, const Timestamp ts, const BeaconMeasurement& data) override { + (void) sensor; + (void) ts; + + + QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection); + } }; diff --git a/ui/map/2D/ColorPoints2D.h b/ui/map/2D/ColorPoints2D.h index d86eea7..4af2081 100644 --- a/ui/map/2D/ColorPoints2D.h +++ b/ui/map/2D/ColorPoints2D.h @@ -64,59 +64,59 @@ public: } /** NOTE: must be called from Qt's main thread! */ - void setFromParticles(const std::vector>& particles){ + void setFromParticles(const std::vector>& particles){ - points.clear(); + points.clear(); - // group particles by grid-point - std::unordered_map weights; - for (const SMC::Particle& p : particles) { - const GridPoint gp = p.state.position; - if (weights.find(gp) != weights.end()) {continue;} - weights[gp] += p.weight; - } + // group particles by grid-point + std::unordered_map weights; + for (const SMC::Particle& p : particles) { + const GridPoint gp = p.state.position; + if (weights.find(gp) != weights.end()) {continue;} + weights[gp] += p.weight; + } - // find min/max - float min = +INFINITY; - float max = -INFINITY; - for (auto it : weights) { - if (it.second > max) {max = it.second;} - if (it.second < min) {min = it.second;} - } + // find min/max + float min = +INFINITY; + float max = -INFINITY; + for (auto it : weights) { + if (it.second > max) {max = it.second;} + if (it.second < min) {min = it.second;} + } - // draw colored - for (auto it : weights) { - const GridPoint gp = it.first; - const float w = it.second; - const float p = (w-min) / (max-min); // [0:1] - const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f); - float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale - const QColor color = QColor::fromHsvF(h, 1, 1); - points.push_back(PT(pt, color)); - } + // draw colored + for (auto it : weights) { + const GridPoint gp = it.first; + const float w = it.second; + const float p = (w-min) / (max-min); // [0:1] + const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f); + float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale + const QColor color = QColor::fromHsvF(h, 1, 1); + points.push_back(PT(pt, color)); + } - } + } - void setFromParticles(const std::vector>& particles){ + void setFromParticles(const std::vector>& particles){ - points.clear(); + points.clear(); - float min = +INFINITY; - float max = -INFINITY; - for (const auto p : particles){ - if (p.weight > max) {max = p.weight;} - if (p.weight < min) {min = p.weight;} - } + float min = +INFINITY; + float max = -INFINITY; + for (const auto p : particles){ + if (p.weight > max) {max = p.weight;} + if (p.weight < min) {min = p.weight;} + } - for (const auto p : particles) { - const Point3 pt(p.state.loc.pos.x, p.state.loc.pos.y, p.state.loc.pos.z); - const float prob = (p.weight-min) / (max-min); - float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale - const QColor color = QColor::fromHsvF(h, 1, 1); - points.push_back(PT(pt, color)); + for (const auto p : particles) { + const Point3 pt(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z); + const float prob = (p.weight-min) / (max-min); + float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale + const QColor color = QColor::fromHsvF(h, 1, 1); + points.push_back(PT(pt, color)); - } - } + } + } protected: diff --git a/ui/map/2D/Floor2D.h b/ui/map/2D/Floor2D.h index 9bdca9d..6392865 100644 --- a/ui/map/2D/Floor2D.h +++ b/ui/map/2D/Floor2D.h @@ -29,7 +29,7 @@ protected: if (floor->atHeight > r.clip.aboveHeight_m) {return;} for (const Floorplan::FloorObstacle* obs : floor->obstacles) { - const Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + const Floorplan::FloorObstacleWall* line = dynamic_cast(obs); if (line) {drawObstacle(qp, s, line);} } @@ -51,10 +51,11 @@ protected: private: - static inline QPen getPen(Floorplan::Material mat, Floorplan::ObstacleType type) { + static inline QPen getPen(Floorplan::Material mat, Floorplan::ObstacleType type, int thickness) { using namespace Floorplan; QPen pen; pen.setColor(Qt::darkGray); - if (mat == Material::CONCRETE) {pen.setWidth(3);} + if (mat == Material::CONCRETE) {pen.setWidth(thickness);} + if (mat == Material::DRYWALL) {pen.setWidth(thickness); pen.setColor(Qt::gray);} if (mat == Material::GLASS) {pen.setStyle(Qt::PenStyle::DotLine);} if (type == ObstacleType::HANDRAIL) {pen.setStyle(Qt::PenStyle::DashLine);} if (type == ObstacleType::UNKNOWN) {pen.setColor(Qt::red); pen.setWidth(5);} @@ -63,10 +64,10 @@ private: return pen; } - void drawObstacle(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleLine* line) { + void drawObstacle(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleWall* line) { const Point2 pt1 = s.mapToScreen(line->from); const Point2 pt2 = s.mapToScreen(line->to); - qp.setPen(getPen(line->material, line->type)); + qp.setPen(getPen(line->material, line->type, static_cast(s.mToPX(line->thickness_m)))); qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y); } diff --git a/yasmin.pro b/yasmin.pro index fb2b3c6..d3aabf6 100644 --- a/yasmin.pro +++ b/yasmin.pro @@ -7,27 +7,27 @@ QT += qml opengl svg CONFIG+=ANDROID DEFINES+=ANDROID ANDROID { - QT += androidextras - QT += sensors - QT += positioning +QT += androidextras +QT += sensors +QT += positioning - #http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545 - # wasn't faster... and seems dangerous :P - #QMAKE_CXXFLAGS -= -mfpu=vfp - #QMAKE_CFLAGS -= -mfpu=vfp - #QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations - #QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations +#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545 +# wasn't faster... and seems dangerous :P +#QMAKE_CXXFLAGS -= -mfpu=vfp +#QMAKE_CFLAGS -= -mfpu=vfp +#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations +#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations - # wasn't faster.. - #QMAKE_CXXFLAGS += -mtune=cortex-a57 - #QMAKE_CFLAGS += -mtune=cortex-a57 +# wasn't faster.. +#QMAKE_CXXFLAGS += -mtune=cortex-a57 +#QMAKE_CFLAGS += -mtune=cortex-a57 - QMAKE_CXXFLAGS += -O2 - #QMAKE_CFLAGS += -O3 +QMAKE_CXXFLAGS += -O2 +#QMAKE_CFLAGS += -O3 - #QMAKE_CXXFLAGS_DEBUG -= -O2 - #QMAKE_CXXFLAGS_DEBUG -= -O3 - #QMAKE_CXXFLAGS_DEBUG += -O0 +#QMAKE_CXXFLAGS_DEBUG -= -O2 +#QMAKE_CXXFLAGS_DEBUG -= -O3 +#QMAKE_CXXFLAGS_DEBUG += -O0 } @@ -49,8 +49,8 @@ CONFIG += c++11 ANDROID_PACKAGE_SOURCE_DIR = $$PWD/_android INCLUDEPATH += \ - ../ \ - ./lib/ +../ \ +./lib/ # linux desktop wifi @@ -58,33 +58,35 @@ INCLUDEPATH += \ #LIBS += -lnl-genl-3 -lnl-3 OTHER_FILES += \ - _android/src/WiFi.java \ - _android/src/MyActivity.java \ - _android/AndroidManifest.xml +_android/src/WiFi.java \ +_android/src/MyActivity.java \ +_android/AndroidManifest.xml \ +_android/src/BLE.java SOURCES += \ - main.cpp \ - lib/gpc/gpc.cpp \ - ../Indoor/lib/tinyxml/tinyxml2.cpp \ - ../Indoor/lib/Recast/*.cpp\ - ui/menu/MainMenu.cpp \ - ui/MainWindow.cpp \ - Controller.cpp \ - ui/dialog/LoadSetupDialog.cpp \ - ui/debug/SensorDataWidget.cpp \ - ui/debug/plot/PlottWidget.cpp \ - ui/debug/PlotTurns.cpp \ - ui/debug/PlotWiFiScan.cpp \ - sensors/android/WiFiSensorAndroid.cpp \ - sensors/linux/WiFiSensorLinuxC.c \ - ui/debug/InfoWidget.cpp \ - ui/map/3D/MapView3D.cpp \ - ui/map/2D/MapView2D.cpp \ - tools/calibration/WiFiCalibrationScanDialog.cpp \ - ui/debug/PlotGPS.cpp \ - nav/NavController.cpp \ - nav/mesh/NavControllerMesh.cpp \ - nav/grid/NavControllerGrid.cpp +main.cpp \ +lib/gpc/gpc.cpp \ +../Indoor/lib/tinyxml/tinyxml2.cpp \ +../Indoor/lib/Recast/*.cpp\ +ui/menu/MainMenu.cpp \ +ui/MainWindow.cpp \ +Controller.cpp \ +ui/dialog/LoadSetupDialog.cpp \ +ui/debug/SensorDataWidget.cpp \ +ui/debug/plot/PlottWidget.cpp \ +ui/debug/PlotTurns.cpp \ +ui/debug/PlotWiFiScan.cpp \ +sensors/android/WiFiSensorAndroid.cpp \ +sensors/linux/WiFiSensorLinuxC.c \ +ui/debug/InfoWidget.cpp \ +ui/map/3D/MapView3D.cpp \ +ui/map/2D/MapView2D.cpp \ +tools/calibration/WiFiCalibrationScanDialog.cpp \ +ui/debug/PlotGPS.cpp \ +nav/NavController.cpp \ +nav/mesh/NavControllerMesh.cpp \ +nav/grid/NavControllerGrid.cpp \ + sensors/android/BLESensorAndroid.cpp RESOURCES += qml.qrc @@ -102,113 +104,116 @@ INSTALLS += target # android-sources/src/WiFi.java HEADERS += \ - lib/gpc/gpc.h \ - sensors/linux/WiFiSensorLinux.h \ - sensors/android/WiFiSensorAndroid.h \ - sensors/StepSensor.h \ - sensors/TurnSensor.h \ - sensors/AccelerometerSensor.h \ - sensors/GyroscopeSensor.h \ - sensors/BarometerSensor.h \ - sensors/android/AccelerometerSensorAndroid.h \ - sensors/android/GyroscopeSensorAndroid.h \ - sensors/android/BarometerSensorAndroid.h \ - sensors/dummy/AccelerometerSensorDummy.h \ - sensors/dummy/GyroscopeSensorDummy.h \ - sensors/dummy/BarometerSensorDummy.h \ - sensors/Sensor.h \ - sensors/SensorFactory.h \ - sensors/WiFiSensor.h \ - misc/Debug.h \ - misc/fixc11.h \ - sensors/dummy/WiFiSensorDummy.h \ - lib/gpc/Polygon.h \ - ui/map/3D/FloorRenderer.h \ - ui/map/3D/gl/GL.h \ - ui/map/3D/gl/GLHelper.h \ - ui/map/3D/gl/GLLines.h \ - ui/map/3D/gl/GLTriangles.h \ - ui/map/3D/elements/Doors.h \ - ui/map/3D/elements/Ground.h \ - ui/map/3D/elements/Handrails.h \ - ui/map/3D/elements/Path.h \ - ui/map/3D/elements/Stairs.h \ - ui/map/3D/elements/Walls.h \ - ui/map/3D/gl/GLPoints.h \ - ui/map/3D/elements/ColorPoints.h \ - ui/map/3D/RenderParams.h \ - ui/map/3D/Renderable.h \ - ui/map/3D/gl/Shader.h \ - ui/map/3D/elements/Object.h \ - ui/Icons.h \ - ui/MainWindow.h \ - Controller.h \ - ui/menu/MainMenu.h \ - ui/dialog/LoadSetupDialog.h \ - ui/debug/plot/Axes.h \ - ui/debug/plot/Plot.h \ - ui/debug/plot/Data.h \ - ui/debug/plot/Range.h \ - nav/NavController.h \ - sensors/dummy/RandomSensor.h \ - ui/debug/SensorDataWidget.h \ - ui/debug/plot/PlottWidget.h \ - ui/debug/PlotTurns.h \ - ui/debug/PlotWiFiScan.h \ - sensors/linux/WiFiSensorLinuxC.h \ - sensors/offline/SensorFactoryOffline.h \ - sensors/dummy/SensorFactoryDummy.h \ - sensors/android/SensorFactoryAndroid.h \ - Settings.h \ - sensors/offline/AllInOneSensor.h \ - sensors/ActivitySensor.h \ - ui/LoggerUI.h \ - ui/debug/InfoWidget.h \ - ui/UIHelper.h \ - ui/map/3D/MapView3D.h \ - ui/map/2D/MapView2D.h \ - ui/map/2D/Floor2D.h \ - ui/map/2D/Scaler2D.h \ - ui/map/2D/Renderable2D.h \ - ui/map/2D/RenderParams2D.h \ - ui/map/2D/ColorPoints2D.h \ - ui/map/2D/Path2D.h \ - ui/map/2D/WiFiCalibTool.h \ - ui/map/2D/HasSelectableNodes.h \ - tools/calibration/WiFiCalibrationDataModel.h \ - tools/calibration/WiFiCalibrationScanDialog.h \ - tests/RuntimeTests.h \ - ipin/StepLogger.h \ - ipin/Scaler.h \ - ipin/Config.h \ - ipin/IPINHelper.h \ - nav/NavControllerListener.h \ - ipin/StepLoggerWrapper.h \ - ipin/StepLoggerWrapperAndroid.h \ - sensors/GPSSensor.h \ - sensors/android/GPSSensorAndroid.h \ - sensors/CompassSensor.h \ - sensors/android/CompassSensorAndroid.h \ - sensors/dummy/CompassSensorDummy.h \ - sensors/dummy/GPSSensorDummy.h \ - ui/debug/PlotGPS.h \ - sensors/SensorWriter.h \ - nav/mesh/State.h \ - nav/mesh/NavControllerMesh.h \ - nav/mesh/FilterMesh.h \ - nav/grid/Filter.h \ - nav/grid/NavControllerGrid.h \ - nav/grid/Node.h \ - nav/grid/NodeResampling.h \ - nav/grid/RegionalResampling.h \ - nav/Observation.h \ - nav/grid/State.h \ - nav/CurEst.h +lib/gpc/gpc.h \ +sensors/linux/WiFiSensorLinux.h \ +sensors/android/WiFiSensorAndroid.h \ +sensors/StepSensor.h \ +sensors/TurnSensor.h \ +sensors/AccelerometerSensor.h \ +sensors/GyroscopeSensor.h \ +sensors/BarometerSensor.h \ +sensors/android/AccelerometerSensorAndroid.h \ +sensors/android/GyroscopeSensorAndroid.h \ +sensors/android/BarometerSensorAndroid.h \ +sensors/android/BLESensorAndroid.h \ +sensors/dummy/AccelerometerSensorDummy.h \ +sensors/dummy/GyroscopeSensorDummy.h \ +sensors/dummy/BarometerSensorDummy.h \ +sensors/Sensor.h \ +sensors/SensorFactory.h \ +sensors/WiFiSensor.h \ +misc/Debug.h \ +misc/fixc11.h \ +sensors/dummy/WiFiSensorDummy.h \ +lib/gpc/Polygon.h \ +ui/map/3D/FloorRenderer.h \ +ui/map/3D/gl/GL.h \ +ui/map/3D/gl/GLHelper.h \ +ui/map/3D/gl/GLLines.h \ +ui/map/3D/gl/GLTriangles.h \ +ui/map/3D/elements/Doors.h \ +ui/map/3D/elements/Ground.h \ +ui/map/3D/elements/Handrails.h \ +ui/map/3D/elements/Path.h \ +ui/map/3D/elements/Stairs.h \ +ui/map/3D/elements/Walls.h \ +ui/map/3D/gl/GLPoints.h \ +ui/map/3D/elements/ColorPoints.h \ +ui/map/3D/RenderParams.h \ +ui/map/3D/Renderable.h \ +ui/map/3D/gl/Shader.h \ +ui/map/3D/elements/Object.h \ +ui/Icons.h \ +ui/MainWindow.h \ +Controller.h \ +ui/menu/MainMenu.h \ +ui/dialog/LoadSetupDialog.h \ +ui/debug/plot/Axes.h \ +ui/debug/plot/Plot.h \ +ui/debug/plot/Data.h \ +ui/debug/plot/Range.h \ +nav/NavController.h \ +sensors/dummy/RandomSensor.h \ +ui/debug/SensorDataWidget.h \ +ui/debug/plot/PlottWidget.h \ +ui/debug/PlotTurns.h \ +ui/debug/PlotWiFiScan.h \ +sensors/linux/WiFiSensorLinuxC.h \ +sensors/offline/SensorFactoryOffline.h \ +sensors/dummy/SensorFactoryDummy.h \ +sensors/android/SensorFactoryAndroid.h \ +Settings.h \ +sensors/offline/AllInOneSensor.h \ +sensors/ActivitySensor.h \ +ui/LoggerUI.h \ +ui/debug/InfoWidget.h \ +ui/UIHelper.h \ +ui/map/3D/MapView3D.h \ +ui/map/2D/MapView2D.h \ +ui/map/2D/Floor2D.h \ +ui/map/2D/Scaler2D.h \ +ui/map/2D/Renderable2D.h \ +ui/map/2D/RenderParams2D.h \ +ui/map/2D/ColorPoints2D.h \ +ui/map/2D/Path2D.h \ +ui/map/2D/WiFiCalibTool.h \ +ui/map/2D/HasSelectableNodes.h \ +tools/calibration/WiFiCalibrationDataModel.h \ +tools/calibration/WiFiCalibrationScanDialog.h \ +tests/RuntimeTests.h \ +ipin/StepLogger.h \ +ipin/Scaler.h \ +ipin/Config.h \ +ipin/IPINHelper.h \ +nav/NavControllerListener.h \ +ipin/StepLoggerWrapper.h \ +ipin/StepLoggerWrapperAndroid.h \ +sensors/GPSSensor.h \ +sensors/android/GPSSensorAndroid.h \ +sensors/CompassSensor.h \ +sensors/android/CompassSensorAndroid.h \ +sensors/dummy/CompassSensorDummy.h \ +sensors/dummy/GPSSensorDummy.h \ +ui/debug/PlotGPS.h \ +sensors/SensorWriter.h \ +nav/mesh/State.h \ +nav/mesh/NavControllerMesh.h \ +nav/mesh/FilterMesh.h \ +nav/grid/Filter.h \ +nav/grid/NavControllerGrid.h \ +nav/grid/Node.h \ +nav/grid/NodeResampling.h \ +nav/grid/RegionalResampling.h \ +nav/Observation.h \ +nav/grid/State.h \ +nav/CurEst.h \ +sensors/BLESensor.h \ + tools/calibration/BLECalibrationDataModel.h DISTFILES += \ - android-sources/src/MyActivity.java \ - res/gl/vertex1.glsl \ - res/gl/fragment1.glsl \ - res/gl/tex/empty_normals.jpg \ - _android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \ - _android/src/StepLoggerClient.java +android-sources/src/MyActivity.java \ +res/gl/vertex1.glsl \ +res/gl/fragment1.glsl \ +res/gl/tex/empty_normals.jpg \ +_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \ +_android/src/StepLoggerClient.java