FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

started to add ble functions

Browse Source 
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
This commit is contained in:
mail@toni-fetzer.de
2019-06-05 18:04:31 +02:00
parent b9b9d8f9ac
commit 20ae2f5c2a
23 changed files with 1341 additions and 1152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
Controller.cpp
View File

@@ -44,71 +44,72 @@
Controller::Controller() { Controller::Controller() {
// OpenGL setup // OpenGL setup
// MUST happen before anything gets visible (= gets initialized) // MUST happen before anything gets visible (= gets initialized)
QSurfaceFormat format; QSurfaceFormat format;
format.setDepthBufferSize(16); format.setDepthBufferSize(16);
QSurfaceFormat::setDefaultFormat(format); QSurfaceFormat::setDefaultFormat(format);
// configure the to-be-used sensor factory // configure the to-be-used sensor factory
//SensorFactory::set(new SensorFactoryDummy()); //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/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/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("/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/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/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/path4/nexus/vor/1454776525797.csv"));
// live data on the smartphone // live data on the smartphone
if (1 == 1) { if (1 == 1) {
// use android's sensors // use android's sensors
SensorFactory::set(new SensorFactoryAndroid()); SensorFactory::set(new SensorFactoryAndroid());
// write them to file?? // write them to file??
//const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv"; //const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv";
//SensorWriter* writer = new SensorWriter(); //SensorWriter* writer = new SensorWriter();
//writer->start(file); //writer->start(file);
// start the sensors // start the sensors
SensorFactory::get().getAccelerometer().start(); SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start(); SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start(); SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start(); SensorFactory::get().getWiFi().start();
SensorFactory::get().getGPS().start(); SensorFactory::get().getGPS().start();
SensorFactory::get().getCompass().start(); SensorFactory::get().getCompass().start();
SensorFactory::get().getBLE().start();
} }
// create the main window // create the main window
mainWindow = new MainWindow(); mainWindow = new MainWindow();
//sl = new StepLoggerWrapperAndroid(scaler); //sl = new StepLoggerWrapperAndroid(scaler);
//sl = new StepLoggerWrapper(scaler, mainWindow); //sl = new StepLoggerWrapper(scaler, mainWindow);
// attach logger // attach logger
LoggerComposite* log = new LoggerComposite(); LoggerComposite* log = new LoggerComposite();
log->addLogger(new LoggerCOUT()); log->addLogger(new LoggerCOUT());
log->addLogger(new LoggerAndroid()); log->addLogger(new LoggerAndroid());
log->addLogger(new LoggerUI(mainWindow->getInfoWidget())); log->addLogger(new LoggerUI(mainWindow->getInfoWidget()));
Log::setLogger(log); Log::setLogger(log);
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onLoadButton, this, &Controller::onLoadButton), "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::onDebugButton, this, &Controller::onDebugButton), "connect() failed");
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onStartButton, this, &Controller::onStartButton), "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::onTransparentButton, this, &Controller::onTransparentButton), "connect() failed");
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onCameraButton, this, &Controller::onCameraButton), "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::on3DButton, this, &Controller::on3DButton), "connect() failed");
// order is important! otherwise OpenGL fails! // order is important! otherwise OpenGL fails!
mainWindow->show(); mainWindow->show();
// // start all sensors // // start all sensors
// SensorFactory::get().getAccelerometer().start(); // SensorFactory::get().getAccelerometer().start();
@@ -120,73 +121,73 @@ Controller::Controller() {
} }
MapView3D* Controller::getMapView3D() const { MapView3D* Controller::getMapView3D() const {
return mainWindow->getMapView3D(); return mainWindow->getMapView3D();
} }
MapView2D* Controller::getMapView2D() const { MapView2D* Controller::getMapView2D() const {
return mainWindow->getMapView2D(); return mainWindow->getMapView2D();
} }
MainMenu* Controller::getMainMenu() const { MainMenu* Controller::getMainMenu() const {
return mainWindow->getMainMenu(); return mainWindow->getMainMenu();
} }
InfoWidget* Controller::getInfoWidget() const { InfoWidget* Controller::getInfoWidget() const {
return mainWindow->getInfoWidget(); return mainWindow->getInfoWidget();
} }
#include <QMessageBox> #include <QMessageBox>
#include <Indoor/sensors/radio/setup/WiFiOptimizer.h> #include <Indoor/sensors/radio/setup/WiFiOptimizer.h>
void buildGridOnce(Grid<MyGridNode>* grid, Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& saveFile) { void buildGridOnce(Grid<MyGridNode>* 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 // 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); 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 // WiFi setup
WiFiModelLogDistCeiling wifiModel(map); WiFiModelLogDistCeiling wifiModel(map);
if (replyWiFiFP == QMessageBox::Yes) { if (replyWiFiFP == QMessageBox::Yes) {
WiFiCalibrationDataModel mdl(fpFile); WiFiCalibrationDataModel mdl(fpFile);
Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!"); Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : mdl.getFingerprints()) { for (const WiFiFingerprint& fp : mdl.getFingerprints()) {
opt.addFingerprint(fp); opt.addFingerprint(fp);
} }
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
wifiModel.addAP(ap.mac, entry); wifiModel.addAP(ap.mac, entry);
} }
} else { } else {
// NOTE IPIN UAH map does not make sense.. APs look like VAPs but are not // 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] // -> 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 // 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::vg_calib, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
wifiModel.loadAPs(map, 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"); Assert::isFalse(wifiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
} }
// build the grid // build the grid
GridFactory<MyGridNode> gf(*grid); GridFactory<MyGridNode> gf(*grid);
gf.build(map); gf.build(map);
// add node-importance // add node-importance
Importance::addImportance(*grid); Importance::addImportance(*grid);
// stamp WiFi signal-strengths onto the grid // stamp WiFi signal-strengths onto the grid
WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround); WiFiGridEstimator::estimate(*grid, wifiModel, Settings::smartphoneAboveGround);
// serialize the grid // serialize the grid
std::ofstream out(saveFile, std::ofstream::binary); std::ofstream out(saveFile, std::ofstream::binary);
grid->write(out); grid->write(out);
out.close(); out.close();
} }
@@ -245,90 +246,90 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi
void Controller::on3DButton() { void Controller::on3DButton() {
static bool use3D = false; static bool use3D = false;
use3D = !use3D; use3D = !use3D;
getMapView2D()->setVisible(!use3D); getMapView2D()->setVisible(!use3D);
getMapView3D()->setVisible( use3D); getMapView3D()->setVisible( use3D);
} }
void Controller::onLoadButton() { void Controller::onLoadButton() {
// pick a map to load // pick a map to load
QDir dir = LoadSetupDialog::pickSetupFolder(); QDir dir = LoadSetupDialog::pickSetupFolder();
// cancelled? // cancelled?
if (dir.path() == ".") { return; } if (dir.path() == ".") { return; }
loadNavMesh(dir); loadNavMesh(dir);
} }
void Controller::loadGrid(QDir dir) { void Controller::loadGrid(QDir dir) {
QFile fMap(dir.path() + "/map.xml"); QFile fMap(dir.path() + "/map.xml");
QFile fGrid(dir.path() + "/grid.dat"); QFile fGrid(dir.path() + "/grid.dat");
QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
Assert::isTrue(fMap.exists(), "map.xml missing"); Assert::isTrue(fMap.exists(), "map.xml missing");
//Assert::isTrue(fGrid.exists(), "grid.dat missing"); //Assert::isTrue(fGrid.exists(), "grid.dat missing");
fMap.open(QIODevice::ReadOnly); fMap.open(QIODevice::ReadOnly);
QString str = QString(fMap.readAll()); QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString()); im = Floorplan::Reader::readFromString(str.toStdString());
const std::string sGrid = fGrid.fileName().toStdString(); const std::string sGrid = fGrid.fileName().toStdString();
std::ifstream inp(sGrid, std::ifstream::binary); std::ifstream inp(sGrid, std::ifstream::binary);
//Assert::isTrue(inp.good(), "failed to open grid.dat"); //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 // create a new, empty grid
if (grid) {delete grid; grid = nullptr;} if (grid) {delete grid; grid = nullptr;}
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm); grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
// grid.dat empty? -> build one and save it // grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) { if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
buildGridOnce(grid, im, sWiFiFP, sGrid); buildGridOnce(grid, im, sWiFiFP, sGrid);
} else { } else {
grid->read(inp); grid->read(inp);
} }
// create a new navigator // create a new navigator
if (nav) {delete nav; nav = nullptr;} if (nav) {delete nav; nav = nullptr;}
nav = new GridBased::NavControllerGrid(this, im, grid); nav = new GridBased::NavControllerGrid(this, im, grid);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);
getMapView3D()->setMap(im); getMapView3D()->setMap(im);
getMapView2D()->setMap(wifiCalib, im); getMapView2D()->setMap(wifiCalib, im);
getMapView3D()->showGridImportance(grid); getMapView3D()->showGridImportance(grid);
getMapView2D()->showGridImportance(grid); getMapView2D()->showGridImportance(grid);
getMapView3D()->setVisible(false); getMapView3D()->setVisible(false);
// attach ipin step logger // attach ipin step logger
//nav->addListener(sl); //nav->addListener(sl);
} }
void Controller::loadNavMesh(QDir dir) { void Controller::loadNavMesh(QDir dir) {
QFile fMap(dir.path() + "/map.xml"); QFile fMap(dir.path() + "/map.xml");
QFile fGrid(dir.path() + "/grid.dat"); QFile fGrid(dir.path() + "/grid.dat");
QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
QFile fWiFiModel(dir.path() + "/wifimodel.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); fMap.open(QIODevice::ReadOnly);
QString str = QString(fMap.readAll()); QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString()); 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(); const std::string sWiFiModel = fWiFiModel.fileName().toStdString();
wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel); wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel);
@@ -340,37 +341,37 @@ void Controller::loadNavMesh(QDir dir) {
NM::NavMeshFactory<NM::NavMeshTriangle> fac(navMesh, settings); NM::NavMeshFactory<NM::NavMeshTriangle> fac(navMesh, settings);
fac.build(im); fac.build(im);
// create a new navigator // create a new navigator
if (nav) {delete nav; nav = nullptr;} if (nav) {delete nav; nav = nullptr;}
nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel); nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);
getMapView3D()->setMap(im); getMapView3D()->setMap(im);
getMapView2D()->setMap(wifiCalib, im); getMapView2D()->setMap(wifiCalib, im);
//getMapView3D()->showGridImportance(grid); //getMapView3D()->showGridImportance(grid);
//getMapView2D()->showGridImportance(grid); //getMapView2D()->showGridImportance(grid);
getMapView3D()->setVisible(false); getMapView3D()->setVisible(false);
// attach ipin step logger // attach ipin step logger
//nav->addListener(sl); //nav->addListener(sl);
} }
void Controller::onDebugButton() { void Controller::onDebugButton() {
mainWindow->getSensorDataWidget()->setVisible( !mainWindow->getSensorDataWidget()->isVisible() ); mainWindow->getSensorDataWidget()->setVisible( !mainWindow->getSensorDataWidget()->isVisible() );
} }
void Controller::onStartButton() { void Controller::onStartButton() {
nav->start(); nav->start();
} }
void Controller::onTransparentButton() { void Controller::onTransparentButton() {
mainWindow->getMapView3D()->toggleRenderMode(); mainWindow->getMapView3D()->toggleRenderMode();
} }
void Controller::onCameraButton() { void Controller::onCameraButton() {
nav->toggleCamera(); nav->toggleCamera();
} }

2
Settings.h
View File

@@ -39,7 +39,7 @@ namespace Settings {
namespace WiFiModel { 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! */ /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -45; constexpr float TXP = -45;

111
_android/src/BLE.java Normal file
View File

@@ -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<ScanFilter> 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<android.bluetooth.le.ScanResult> 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();
}
}

74
_android/src/WiFi.java
View File

@@ -19,8 +19,8 @@ public class WiFi {
private static WifiManager manager; private static WifiManager manager;
private static BroadcastReceiver receiver; private static BroadcastReceiver receiver;
private static Thread tHeartbeat = null; private static Thread tHeartbeat = null;
private static long lastScanStartTS = 0; private static long lastScanStartTS = 0;
/** called when a scan is completed successfully */ /** called when a scan is completed successfully */
public static native void onScanComplete(final byte[] result); public static native void onScanComplete(final byte[] result);
@@ -36,12 +36,12 @@ public class WiFi {
MyActivity act = MyActivity.act; MyActivity act = MyActivity.act;
manager = (WifiManager) act.getSystemService(Context.WIFI_SERVICE); manager = (WifiManager) act.getSystemService(Context.WIFI_SERVICE);
// reset(); // reset();
WiFi.receiver = new BroadcastReceiver() { WiFi.receiver = new BroadcastReceiver() {
public final void onReceive(final Context context, final Intent intent) { public final void onReceive(final Context context, final Intent intent) {
final byte[] result = serialize(manager.getScanResults()); final byte[] result = serialize(manager.getScanResults());
triggerOneScan(); triggerOneScan();
WiFi.onScanComplete(result); WiFi.onScanComplete(result);
} }
}; };
@@ -50,42 +50,42 @@ public class WiFi {
act.registerReceiver(WiFi.receiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION)); act.registerReceiver(WiFi.receiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION));
// start the first scan // start the first scan
triggerOneScan(); triggerOneScan();
//this is a very nice hack. do not try this at home. //this is a very nice hack. do not try this at home.
Method m = null; Method m = null;
try { try {
m = manager.getClass().getDeclaredMethod("setFrequencyBand", int.class, boolean.class); m = manager.getClass().getDeclaredMethod("setFrequencyBand", int.class, boolean.class);
m.setAccessible(true); m.setAccessible(true);
m.invoke(manager, 2, true); m.invoke(manager, 2, true);
m.invoke(manager, 2, true); m.invoke(manager, 2, true);
m.invoke(manager, 2, true); m.invoke(manager, 2, true);
Log.d("wifi", "HACK IS RUNNING, BIAAAATCH"); Log.d("wifi", "HACK IS RUNNING, BIAAAATCH");
} catch (Exception e) { } catch (Exception e) {
Log.d("wifi", "HACK HAS FAILED >.<"); Log.d("wifi", "HACK HAS FAILED >.<");
e.printStackTrace(); e.printStackTrace();
} }
// if the scan result takes longer than X milliseconds, // if the scan result takes longer than X milliseconds,
// trigger another-scan to ensure nothing is stuck // trigger another-scan to ensure nothing is stuck
final Runnable r = new Runnable() { final Runnable r = new Runnable() {
public void run() { public void run() {
while(true) { while(true) {
final long ts = System.currentTimeMillis(); final long ts = System.currentTimeMillis();
final long diff = ts - lastScanStartTS; final long diff = ts - lastScanStartTS;
if (diff > 1000) { triggerOneScan(); } if (diff > 1000) { triggerOneScan(); }
try {Thread.sleep(200);} catch (final Exception e) {;} try {Thread.sleep(200);} catch (final Exception e) {;}
} }
} }
}; };
// start the heartbeat once // start the heartbeat once
if (tHeartbeat == null) { if (tHeartbeat == null) {
tHeartbeat = new Thread(r); tHeartbeat = new Thread(r);
tHeartbeat.start(); tHeartbeat.start();
} }
return 1337; return 1337;
} }
@@ -126,7 +126,7 @@ public class WiFi {
try { try {
if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");} if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");}
lastScanStartTS = System.currentTimeMillis(); lastScanStartTS = System.currentTimeMillis();
}catch (final Exception e) { }catch (final Exception e) {
throw new RuntimeException(e); throw new RuntimeException(e);
} }

77
nav/NavController.h
View File

@@ -42,66 +42,67 @@
class Controller; class Controller;
class NavController : class NavController :
public SensorListener<AccelerometerData>, public SensorListener<AccelerometerData>,
public SensorListener<GyroscopeData>, public SensorListener<BeaconMeasurement>,
public SensorListener<BarometerData>, public SensorListener<GyroscopeData>,
public SensorListener<WiFiMeasurements>, public SensorListener<BarometerData>,
public SensorListener<GPSData>, public SensorListener<WiFiMeasurements>,
public SensorListener<StepData>, public SensorListener<GPSData>,
public SensorListener<StepData>,
public SensorListener<TurnData>, public SensorListener<TurnData>,
public SensorListener<ActivityData> public SensorListener<ActivityData>
{ {
protected: protected:
Controller* mainController; Controller* mainController;
Floorplan::IndoorMap* im; Floorplan::IndoorMap* im;
bool running = false; bool running = false;
std::thread tFilter; std::thread tFilter;
std::thread tDisplay; std::thread tDisplay;
/** the estimated path */ /** the estimated path */
std::vector<Point3> estPath; std::vector<Point3> estPath;
/** all listeners */ /** all listeners */
std::vector<NavControllerListener*> listeners; std::vector<NavControllerListener*> listeners;
/** display stuff */ /** display stuff */
const int display_ms = Settings::MapView3D::msPerFrame.ms(); const int display_ms = Settings::MapView3D::msPerFrame.ms();
Point3 curPosFast; Point3 curPosFast;
Point3 curPosSlow; Point3 curPosSlow;
CurEst curEst; CurEst curEst;
Timestamp lastTransition; Timestamp lastTransition;
public: public:
NavController(Controller* mainController, Floorplan::IndoorMap* im); NavController(Controller* mainController, Floorplan::IndoorMap* im);
virtual ~NavController() { virtual ~NavController() {
if (running) {stop();} if (running) {stop();}
} }
/** attach a new event listener */ /** attach a new event listener */
void addListener(NavControllerListener* l) { void addListener(NavControllerListener* l) {
listeners.push_back(l); listeners.push_back(l);
} }
public: public:
virtual void stop() {;} virtual void stop() {;}
virtual void start() = 0; virtual void start() = 0;
int cameraMode = 0; int cameraMode = 0;
void toggleCamera() { void toggleCamera() {
cameraMode = (cameraMode + 1) % 3; cameraMode = (cameraMode + 1) % 3;
} }
/** update the map-view (called from within a background-loop) */ /** update the map-view (called from within a background-loop) */
void updateMapView(); void updateMapView();
}; };

46
nav/Observation.h
View File

@@ -2,45 +2,49 @@
#define STATE_H #define STATE_H
#include <Indoor/sensors/radio/WiFiMeasurements.h> #include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <Indoor/sensors/gps/GPSData.h> #include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/activity/Activity.h> #include <Indoor/sensors/activity/Activity.h>
/** observed sensor data */ /** observed sensor data */
struct MyObservation { struct MyObservation {
/** wifi measurements */ /** wifi measurements */
WiFiMeasurements wifi; WiFiMeasurements wifi;
/** gps measurements */ /** ble measurements */
GPSData gps; BeaconMeasurements ble;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...? /** gps measurements */
/** detected activity */ GPSData gps;
Activity activity;
/** time of evaluation */ // TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
Timestamp currentTime; /** detected activity */
Activity activity;
/** time of evaluation */
Timestamp currentTime;
}; };
/** (observed) control data */ /** (observed) control data */
struct MyControl { struct MyControl {
/** turn angle (in radians) since the last transition */ /** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0; float turnSinceLastTransition_rad = 0;
/** number of steps since the last transition */ /** number of steps since the last transition */
int numStepsSinceLastTransition = 0; int numStepsSinceLastTransition = 0;
// TODO: switch to a general activity enum/detector using barometer + accelerometer? // TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */ /** currently detected activity */
Activity activity; Activity activity;
/** reset the control-data after each transition */ /** reset the control-data after each transition */
void resetAfterTransition() { void resetAfterTransition() {
turnSinceLastTransition_rad = 0; turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 0; numStepsSinceLastTransition = 0;
} }
}; };

255
nav/grid/Filter.h
View File

@@ -31,6 +31,7 @@
#include "NodeResampling.h" #include "NodeResampling.h"
#include "../Settings.h" #include "../Settings.h"
#include <../misc/fixc11.h>
#include <omp.h> #include <omp.h>
#include <future> #include <future>
@@ -38,218 +39,218 @@ namespace GridBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> { class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private: private:
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
public: public:
PFInit(Grid<MyGridNode>* grid) : grid(grid) { PFInit(Grid<MyGridNode>* grid) : grid(grid) {
} }
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override { virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1); std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
std::uniform_real_distribution<float> distHead(0, 2*M_PI); std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (SMC::Particle<MyState>& p : particles) { for (SMC::Particle<MyState>& p : particles) {
const int idx = distIdx(gen); const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx]; const MyGridNode& node = (*grid)[idx];
p.state.position = node; // random position p.state.position = node; // random position
p.state.heading.direction = Heading(distHead(gen)); // random heading p.state.heading.direction = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight p.weight = 1.0 / particles.size(); // equal weight
} }
// // fix position + heading // // fix position + heading
// for (SMC::Particle<MyState>& p : particles) { // for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000; //// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx]; //// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
// p.state.position = node; // p.state.position = node;
// p.state.heading.direction = Heading(0); // p.state.heading.direction = Heading(0);
// } // }
} }
}; };
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public: public:
/** local, static control-data COPY */ /** local, static control-data COPY */
MyControl ctrl; MyControl ctrl;
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker; GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ; WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading; WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance; WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination; WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity; WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler; NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen; std::minstd_rand gen;
public: public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) { PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
//walker.addModule(&modFavorZ); //walker.addModule(&modFavorZ);
walker.addModule(&modHeading); walker.addModule(&modHeading);
//walker.addModule(&modImportance); //walker.addModule(&modImportance);
walker.addModule(&modActivity); walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) { if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination); //walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination)); modDestination.setDestination(grid->getNodeFor(Settings::destination));
} }
} }
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl; this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition(); ((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma); std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// sanity check // 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!");
//for (SMC::Particle<MyState>& p : particles) { //for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic //#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
double prob; double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob); p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15); //p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15); //p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob; //p.weight = prob;
//p.weight = 1.0; // reset //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(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 *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");} if (p.weight != p.weight) {throw Exception("nan");}
} }
} }
}; };
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> { class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel; WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation //WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground // smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public: public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) : PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel), grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
} }
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) { double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75; const float kappa = 0.75;
const MyGridNode& gn = grid->getNodeFor(p.state.position); const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) { switch (act) {
case Activity::STANDING: case Activity::STANDING:
case Activity::WALKING: case Activity::WALKING:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;} {return 1-kappa;}
case Activity::WALKING_UP: case Activity::WALKING_UP:
case Activity::WALKING_DOWN: case Activity::WALKING_DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;} {return 1-kappa;}
} }
return 1.0; return 1.0;
} }
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override { double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0; double sum = 0;
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation; const MyObservation observation = _observation;
// vap-grouping // vap-grouping
const int numAP1 = observation.wifi.entries.size(); const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size(); 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 // 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!");
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
// WiFi free // WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
// WiFi grid // WiFi grid
const MyGridNode& node = grid->getNodeFor(p.state.position); const MyGridNode& node = grid->getNodeFor(p.state.position);
const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs); 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 pStair = getStairProb(p, observation.activity);
const double pGPS = 1; const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair; const double prob = pWiFi * pGPS * pStair;
p.weight *= prob; // NOTE: keeps the weight returned by the transition step! 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 //p.weight = prob; // does NOT keep the weights returned by the transition step
if (p.weight != p.weight) {throw Exception("nan");} if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic #pragma omp atomic
sum += p.weight; sum += p.weight;
} }
return sum; return sum;
} }
}; };
} }

291
nav/grid/NavControllerGrid.cpp
View File

@@ -14,131 +14,140 @@
#include "../ui/map/2D/MapView2D.h" #include "../ui/map/2D/MapView2D.h"
#include "../ui/debug/InfoWidget.h" #include "../ui/debug/InfoWidget.h"
#include "../Controller.h" #include "../../Controller.h"
Q_DECLARE_METATYPE(const void*) Q_DECLARE_METATYPE(const void*)
GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) {
// filter init // filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::unique_ptr<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation // estimation
//std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()); //std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)); std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling // resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid)); std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>()); //std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05)); //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling()); //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition // eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel)); std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid)); std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter // setup the filter
pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init))); pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf->setTransition(std::move(transition)); pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval)); pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation)); pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample)); pf->setResampling(std::move(resample));
pf->setNEffThreshold(0.85); //before 0.75, edit by toni pf->setNEffThreshold(0.85); //before 0.75, edit by toni
//pf->setNEffThreshold(0.65); // still too low? //pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low //pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors // attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this); SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this); SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this); SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getActivity().addListener(this); SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getBLE().addListener(this);
} }
void GridBased::NavControllerGrid::start() { void GridBased::NavControllerGrid::start() {
Assert::isFalse(running, "already started!"); Assert::isFalse(running, "already started!");
running = true; running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;) curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this); tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this); tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this);
// start all sensors // start all sensors
SensorFactory::get().getAccelerometer().start(); SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start(); SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start(); SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start(); SensorFactory::get().getWiFi().start();
SensorFactory::get().getBLE().start();
#ifndef ANDROID #ifndef ANDROID
// #include <valgrind/callgrind.h> // #include <valgrind/callgrind.h>
// run with // run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with // show with
// kcachegrind callgrind.out.xxxx // kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION; CALLGRIND_START_INSTRUMENTATION;
#endif #endif
} }
void GridBased::NavControllerGrid::stop() { void GridBased::NavControllerGrid::stop() {
Assert::isTrue(running, "not started!"); Assert::isTrue(running, "not started!");
running = false; running = false;
tFilter.join(); tFilter.join();
tDisplay.join(); tDisplay.join();
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor; (void) sensor;
(void) data; (void) data;
(void) ts; (void) ts;
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
(void) data; (void) data;
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
(void) data; (void) data;
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curObs.wifi = data; curObs.wifi = data;
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curObs.gps = data; curObs.gps = data;
gotSensorData(ts); gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<BeaconMeasurement>* 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<StepData>* sensor, const Timestamp ts, const StepData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts); gotSensorData(ts);
} }
void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) { void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
@@ -152,8 +161,8 @@ void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, co
/** called when any sensor has received new data */ /** called when any sensor has received new data */
void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts; curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
} }
// void debugActivity(const ActivityData& activity) { // 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"); // Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// } // }
/** particle-filter update loop */ /** particle-filter update loop */
void GridBased::NavControllerGrid::filterUpdateLoop() { 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 // // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime(); // const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -181,95 +190,95 @@ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
// const Timestamp sleep = Timestamp::fromMS(500) - needed; // const Timestamp sleep = Timestamp::fromMS(500) - needed;
// std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms())); // std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
const bool wasUpdated = filterUpdateIfNeeded(); const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } 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 */ /** check whether its time for a filter update, and if so, execute the update and return true */
bool GridBased::NavControllerGrid::filterUpdateIfNeeded() { bool GridBased::NavControllerGrid::filterUpdateIfNeeded() {
static float avgSum = 0; static float avgSum = 0;
static int avgCount = 0; static int avgCount = 0;
// fixed update rate based on incoming sensor data // fixed update rate based on incoming sensor data
// allows working with live data and faster for offline data // allows working with live data and faster for offline data
const Timestamp diff = curObs.currentTime - lastTransition; const Timestamp diff = curObs.currentTime - lastTransition;
if (diff >= Settings::Filter::updateEvery) { if (diff >= Settings::Filter::updateEvery) {
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one // 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; const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime - err; lastTransition = curObs.currentTime - err;
const Timestamp ts1 = Timestamp::fromUnixTime(); const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate(); filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1; const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(tsDiff.ms()); const QString filterTime = QString::number(tsDiff.ms());
avgSum += tsDiff.ms(); ++avgCount; avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/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)); QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true; return true;
} else { } else {
return false; return false;
} }
} }
/** perform a filter-update (called from a background-loop) */ /** perform a filter-update (called from a background-loop) */
void GridBased::NavControllerGrid::filterUpdate() { void GridBased::NavControllerGrid::filterUpdate() {
//lastEst = curEst; //lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs); MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.position.inMeter(); curEst.pos_m = sCurEst.position.inMeter();
curEst.head = sCurEst.heading.direction; curEst.head = sCurEst.heading.direction;
//Log::add("Nav", "cur est: " + curEst.position.asString()); //Log::add("Nav", "cur est: " + curEst.position.asString());
// inform listeners about the new estimation // inform listeners about the new estimation
for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);} 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->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->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
// update estimated path // update estimated path
estPath.push_back(curEst.pos_m); 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->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"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
PFTrans* trans = (PFTrans*)pf->getTransition(); PFTrans* trans = (PFTrans*)pf->getTransition();
const MyGridNode* node = grid->getNodePtrFor(sCurEst.position); const MyGridNode* node = grid->getNodePtrFor(sCurEst.position);
if (node) { if (node) {
try { try {
pathToDest = trans->modDestination.getShortestPath(*node); 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->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"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
} catch (...) {;} } catch (...) {;}
} }
// mainController->getMapView()->showGridImportance(); // mainController->getMapView()->showGridImportance();
} }
/** UI update loop */ /** UI update loop */
void GridBased::NavControllerGrid::updateMapViewLoop() { void GridBased::NavControllerGrid::updateMapViewLoop() {
while(running) { while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime(); const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView(); updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1; const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms()); const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
} }
} }

64
nav/grid/NavControllerGrid.h
View File

@@ -21,68 +21,70 @@
namespace GridBased { namespace GridBased {
class NavControllerGrid : public NavController { class NavControllerGrid : public NavController {
private: private:
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel; WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf; std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest; DijkstraPath<MyGridNode> pathToDest;
MyObservation curObs; MyObservation curObs;
MyControl curCtrl; MyControl curCtrl;
public: public:
NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid); NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);
void start() override; void start() override;
void stop() override; void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override; void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override; void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override; void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override; void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override; void onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ; void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ; void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private: private:
/** called when any sensor has received new data */ /** called when any sensor has received new data */
void gotSensorData(const Timestamp ts); void gotSensorData(const Timestamp ts);
// void debugActivity(const ActivityData& activity); // void debugActivity(const ActivityData& activity);
/** particle-filter update loop */ /** particle-filter update loop */
void filterUpdateLoop(); void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */ /** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded(); bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */ /** perform a filter-update (called from a background-loop) */
void filterUpdate(); void filterUpdate();
/** UI update loop */ /** UI update loop */
void updateMapViewLoop(); void updateMapViewLoop();
}; };
} }

176
nav/mesh/FilterMesh.h
View File

@@ -32,33 +32,33 @@
namespace MeshBased { namespace MeshBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> { class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private: private:
const NM::NavMesh<NM::NavMeshTriangle>* mesh; const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public: public:
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) { PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
; ;
} }
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override { virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI); std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom(); NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (SMC::Particle<MyState>& p : particles) { for (SMC::Particle<MyState>& p : particles) {
p.state.loc = rnd.draw(); p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight p.weight = 1.0 / particles.size(); // equal weight
} }
// // fix position + heading // // fix position + heading
// for (SMC::Particle<MyState>& p : particles) { // for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000; //// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx]; //// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
@@ -71,109 +71,109 @@ namespace MeshBased {
}; };
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public: public:
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>; using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker; MyNavMeshWalk walker;
// local, static control-data COPY // local, static control-data COPY
MyControl ctrl; MyControl ctrl;
public: public:
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){ PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
// how to evaluate drawn points // how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04)); walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1)); walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
} }
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl; this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition(); ((MyControl*)_ctrl)->resetAfterTransition();
// walking and heading random // walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1); Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1); Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1); Distribution::Normal<float> dHeading(0.0, 0.1);
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
// how to walk // how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params; NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw(); params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition; params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.loc; params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(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.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1; params.stepSizes.stepSizeStair_m = 0.1;
} }
// walk // walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params); MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state // assign back to particle's state
p.weight *= res.probability; p.weight *= res.probability;
p.state.loc = res.location; p.state.pos = res.location;
p.state.heading = res.heading; p.state.heading = res.heading;
} }
} }
}; };
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> { class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel; WiFiModel& wifiModel;
WiFiObserverFree wifiProbability; WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) { double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.9; const float kappa = 0.9;
switch (act) { switch (act) {
case Activity::WALKING: case Activity::WALKING:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;} {return 1-kappa;}
case Activity::WALKING_UP: case Activity::WALKING_UP:
case Activity::WALKING_DOWN: case Activity::WALKING_DOWN:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} if (p.state.pos.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.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;} {return 1-kappa;}
} }
return 1.0; return 1.0;
} }
public: public:
//TODO: Was ist hier besser? Im Museum hatten wir das unterste. //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){}
//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::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){} //PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override { double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0; double sum = 0;
@@ -181,32 +181,32 @@ namespace MeshBased {
const MyObservation observation = _observation; const MyObservation observation = _observation;
// vap-grouping // vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// sanity check // 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 // assign weights
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) { for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
const double pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs); const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity); const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1; 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; p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");} if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic #pragma omp atomic
sum += p.weight; sum += p.weight;
} }
return sum; return sum;
} }

207
nav/mesh/NavControllerMesh.cpp
View File

@@ -23,7 +23,7 @@
#include <Indoor/floorplan/v2/Floorplan.h> #include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h> //#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#ifndef ANDROID //#ifndef ANDROID
//#include <valgrind/callgrind.h> //#include <valgrind/callgrind.h>
@@ -37,7 +37,7 @@ Q_DECLARE_METATYPE(const void*)
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) : MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) { NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init // filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh)); std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// estimation // estimation
@@ -47,11 +47,11 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
// resampling // resampling
std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>()); //std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1))); //std::unique_ptr<SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1)));
//std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>()); //std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95)); //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95));
//std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>()); std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition // eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel)); std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
@@ -68,35 +68,37 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
//pf->setNEffThreshold(0.65); // still too low? //pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low //pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors // attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this); SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this); SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this); SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getActivity().addListener(this); SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getBLE().addListener(this);
// hacky.. but we need to call this one from the main thread! // hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles()); //mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>(); qRegisterMetaType<const void*>();
} }
void MeshBased::NavControllerMesh::start() { void MeshBased::NavControllerMesh::start() {
Assert::isFalse(running, "already started!"); Assert::isFalse(running, "already started!");
running = true; running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;) curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this); tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this); tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
// start all sensors // start all sensors
SensorFactory::get().getAccelerometer().start(); SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start(); SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start(); SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start(); SensorFactory::get().getWiFi().start();
SensorFactory::get().getBLE().start();
//#ifndef ANDROID //#ifndef ANDROID
// // #include <valgrind/callgrind.h> // // #include <valgrind/callgrind.h>
@@ -110,60 +112,67 @@ void MeshBased::NavControllerMesh::start() {
} }
void MeshBased::NavControllerMesh::stop() { void MeshBased::NavControllerMesh::stop() {
Assert::isTrue(running, "not started!"); Assert::isTrue(running, "not started!");
running = false; running = false;
tFilter.join(); tFilter.join();
tDisplay.join(); tDisplay.join();
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor; (void) sensor;
(void) data; (void) data;
(void) ts; (void) ts;
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
(void) data; (void) data;
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
(void) data; (void) data;
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curObs.wifi = data; curObs.wifi = data;
gotSensorData(ts); gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) {
(void) sensor;
(void) ts;
curObs.ble.add(data);
gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curObs.gps = data; curObs.gps = data;
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts); gotSensorData(ts);
} }
void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
@@ -177,8 +186,8 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, co
/** called when any sensor has received new data */ /** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts; curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
} }
void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) { 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"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
} }
/** particle-filter update loop */ /** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() { 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 // // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime(); // const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -207,37 +216,37 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// const Timestamp sleep = Timestamp::fromMS(500) - needed; // const Timestamp sleep = Timestamp::fromMS(500) - needed;
// std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms())); // std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
const bool wasUpdated = filterUpdateIfNeeded(); const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } 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 */ /** check whether its time for a filter update, and if so, execute the update and return true */
bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() { bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() {
static float avgSum = 0; static float avgSum = 0;
static int avgCount = 0; static int avgCount = 0;
const Timestamp diff = curObs.currentTime - lastTransition; const Timestamp diff = curObs.currentTime - lastTransition;
if (curCtrl.numStepsSinceLastTransition > 0){ if (curCtrl.numStepsSinceLastTransition > 0){
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one // 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; const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime; lastTransition = curObs.currentTime;
const Timestamp ts1 = Timestamp::fromUnixTime(); const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate(); filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1; const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(diff.ms()); const QString filterTime = QString::number(diff.ms());
avgSum += tsDiff.ms(); ++avgCount; avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/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)); QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true; return true;
} else if(diff >= Timestamp::fromMS(1000)) { } else if(diff >= Timestamp::fromMS(1000)) {
@@ -251,19 +260,19 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
} else { } else {
return false; return false;
} }
} }
DijkstraPath<MyGridNode> pathToDest; DijkstraPath<MyGridNode> pathToDest;
void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() { void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() {
//lastEst = curEst; //lastEst = curEst;
MyState sCurEst = pf->updateEvaluationOnly(curObs); MyState sCurEst = pf->updateEvaluationOnly(curObs);
curEst.pos_m = sCurEst.loc.pos; curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading; curEst.head = sCurEst.heading;
// inform listeners about the new estimation // 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"); 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) */ /** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() { void MeshBased::NavControllerMesh::filterUpdate() {
//lastEst = curEst; //lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs); MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.loc.pos; curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading; curEst.head = sCurEst.heading;
// inform listeners about the new estimation // inform listeners about the new estimation
@@ -309,18 +318,18 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// } // }
// mainController->getMapView()->showGridImportance(); // mainController->getMapView()->showGridImportance();
} }
/** UI update loop */ /** UI update loop */
void MeshBased::NavControllerMesh::updateMapViewLoop() { void MeshBased::NavControllerMesh::updateMapViewLoop() {
while(running) { while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime(); const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView(); updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1; const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms()); const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
} }
} }

56
nav/mesh/NavControllerMesh.h
View File

@@ -24,68 +24,70 @@
namespace MeshBased { namespace MeshBased {
class NavControllerMesh : public NavController { class NavControllerMesh : public NavController {
private: private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh; NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel; WiFiModel* wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf; std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs; MyObservation curObs;
MyControl curCtrl; MyControl curCtrl;
public: public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel); NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
void start() override; void start() override;
void stop() override; void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override; void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override; void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override; void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override; void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override; void onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override; void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ; void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private: private:
/** called when any sensor has received new data */ /** called when any sensor has received new data */
void gotSensorData(const Timestamp ts); void gotSensorData(const Timestamp ts);
void debugActivity(const ActivityData& activity); void debugActivity(const ActivityData& activity);
/** particle-filter update loop */ /** particle-filter update loop */
void filterUpdateLoop(); void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */ /** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded(); bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */ /** perform a filter-update (called from a background-loop) */
void filterUpdate(); void filterUpdate();
/** perform a filter-update only with estimation (called from a background-loop) */ /** perform a filter-update only with estimation (called from a background-loop) */
void filterUpdateEstimationOnly(); void filterUpdateEstimationOnly();
/** UI update loop */ /** UI update loop */
void updateMapViewLoop(); void updateMapViewLoop();
}; };
} }

74
nav/mesh/State.h
View File

@@ -9,60 +9,60 @@ namespace MeshBased {
struct MyState { struct MyState {
NM::NavMeshLocation<NM::NavMeshTriangle> loc; NM::NavMeshLocation<NM::NavMeshTriangle> pos;
Heading heading; Heading heading;
/** ctor */ /** ctor */
MyState() : loc(), heading(0) { MyState() : pos(), heading(0) {
; ;
} }
/** ctor */ /** ctor */
MyState(NM::NavMeshLocation<NM::NavMeshTriangle> loc, Heading h) : loc(loc), heading(h) { MyState(NM::NavMeshLocation<NM::NavMeshTriangle> loc, Heading h) : pos(loc), heading(h) {
; ;
} }
MyState& operator += (const MyState& o) { MyState& operator += (const MyState& o) {
loc.pos += o.loc.pos; pos.pos += o.pos.pos;
return *this; return *this;
} }
MyState& operator /= (const float val) { MyState& operator /= (const float val) {
loc.pos /= val; pos.pos /= val;
return *this; return *this;
} }
MyState operator * (const float val) const { MyState operator * (const float val) const {
MyState copy = *this; MyState copy = *this;
copy.loc.pos = copy.loc.pos * val; copy.pos.pos = copy.pos.pos * val;
return copy; return copy;
} }
float getX(){ float getX(){
return loc.pos.x; return pos.pos.x;
} }
float getY() { float getY() {
return loc.pos.y; return pos.pos.y;
} }
float getZ() { float getZ() {
return loc.pos.z; return pos.pos.z;
} }
void setPosition(Point3 pos){ void setPosition(Point3 other){
loc.pos = pos; pos.pos = other;
} }
float getBinValue(const int dim) const { float getBinValue(const int dim) const {
switch (dim) { switch (dim) {
case 0: return this->loc.pos.x; case 0: return this->pos.pos.x;
case 1: return this->loc.pos.y; case 1: return this->pos.pos.y;
case 2: return this->loc.pos.z; case 2: return this->pos.pos.z;
case 3: return this->heading.getRAD(); case 3: return this->heading.getRAD();
} }
throw "cant find this value within the bin"; throw "cant find this value within the bin";
} }
}; };

80
sensors/SensorFactory.h
View File

@@ -8,6 +8,9 @@
#include "linux/WiFiSensorLinux.h" #include "linux/WiFiSensorLinux.h"
#include "android/WiFiSensorAndroid.h" #include "android/WiFiSensorAndroid.h"
#include "BLESensor.h"
#include "android/BLESensorAndroid.h"
#include "AccelerometerSensor.h" #include "AccelerometerSensor.h"
#include "dummy/AccelerometerSensorDummy.h" #include "dummy/AccelerometerSensorDummy.h"
#include "android/AccelerometerSensorAndroid.h" #include "android/AccelerometerSensorAndroid.h"
@@ -37,58 +40,61 @@ class SensorFactory {
private: private:
/** this one is a dirty hack, as static class member variables do not work header-only */ /** this one is a dirty hack, as static class member variables do not work header-only */
static SensorFactory** getPtr() { static SensorFactory** getPtr() {
static SensorFactory* ptr = nullptr; static SensorFactory* ptr = nullptr;
return &ptr; return &ptr;
} }
public: public:
/** set the to-be-used sensor-fatory */ /** set the to-be-used sensor-fatory */
static void set(SensorFactory* fac) { static void set(SensorFactory* fac) {
Assert::isNull(*getPtr(), "SensorFactory::set() was already called. currentely this is not intended"); Assert::isNull(*getPtr(), "SensorFactory::set() was already called. currentely this is not intended");
*getPtr() = fac; *getPtr() = fac;
} }
/** get the currently configured sensory factory */ /** get the currently configured sensory factory */
static SensorFactory& get() { static SensorFactory& get() {
Assert::isNotNull(*getPtr(), "call SensorFactory::set() first to set an actual factory instance!"); Assert::isNotNull(*getPtr(), "call SensorFactory::set() first to set an actual factory instance!");
return **getPtr(); return **getPtr();
} }
public: public:
/** get the WiFi sensor */ /** get the WiFi sensor */
virtual WiFiSensor& getWiFi() = 0; virtual WiFiSensor& getWiFi() = 0;
/** get the Accelerometer sensor */ /** get the Accelerometer sensor */
virtual AccelerometerSensor& getAccelerometer() = 0; virtual AccelerometerSensor& getAccelerometer() = 0;
/** get the Gyroscope sensor */ /** get the Gyroscope sensor */
virtual GyroscopeSensor& getGyroscope() = 0; virtual GyroscopeSensor& getGyroscope() = 0;
/** get the Barometer sensor */ /** get the Barometer sensor */
virtual BarometerSensor& getBarometer() = 0; virtual BarometerSensor& getBarometer() = 0;
/** get the compass sensor */ /** get the compass sensor */
virtual CompassSensor& getCompass() = 0; virtual CompassSensor& getCompass() = 0;
/** get the gps sensor */ /** get the gps sensor */
virtual GPSSensor& getGPS() = 0; virtual GPSSensor& getGPS() = 0;
/** get the bluetooth low energy sensor */
virtual BLESensor& getBLE() = 0;
/** get the Step sensor */ /** get the Step sensor */
StepSensor& getSteps() { StepSensor& getSteps() {
static StepSensor steps(getAccelerometer()); static StepSensor steps(getAccelerometer());
return steps; return steps;
} }
/** get the Turn sensor */ /** get the Turn sensor */
TurnSensor& getTurns() { TurnSensor& getTurns() {
static TurnSensor turns(getAccelerometer(), getGyroscope()); static TurnSensor turns(getAccelerometer(), getGyroscope());
return turns; return turns;
} }
/** get the Activity sensor */ /** get the Activity sensor */
ActivitySensor& getActivity() { ActivitySensor& getActivity() {

101
sensors/SensorWriter.h
View File

@@ -19,82 +19,81 @@
* writes it to a file * writes it to a file
*/ */
class SensorWriter : class SensorWriter :
public SensorListener<AccelerometerData>, public SensorListener<AccelerometerData>,
public SensorListener<GyroscopeData>, public SensorListener<GyroscopeData>,
public SensorListener<BarometerData>, public SensorListener<BarometerData>,
// public SensorListener<ActivityData>, // public SensorListener<ActivityData>,
// public SensorListener<StepData>, // public SensorListener<StepData>,
// public SensorListener<TurnData>, // public SensorListener<TurnData>,
public SensorListener<WiFiMeasurements>, public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>, public SensorListener<GPSData>,
public SensorListener<CompassData> { public SensorListener<CompassData> {
private: private:
Offline::FileWriter writer; Offline::FileWriter writer;
bool active = false; bool active = false;
public: public:
/** empty ctor */ /** empty ctor */
SensorWriter() { SensorWriter() {
// attach as listener to all sensors we want to store // attach as listener to all sensors we want to store
SensorFactory::get().getAccelerometer().addListener(this); SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this); SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this); SensorFactory::get().getBarometer().addListener(this);
// SensorFactory::get().getActivity().addListener(this); // SensorFactory::get().getActivity().addListener(this);
// SensorFactory::get().getSteps().addListener(this); // SensorFactory::get().getSteps().addListener(this);
// SensorFactory::get().getTurns().addListener(this); // SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getCompass().addListener(this); SensorFactory::get().getCompass().addListener(this);
SensorFactory::get().getGPS().addListener(this); SensorFactory::get().getGPS().addListener(this);
}
}
void start(const std::string& file) { void start(const std::string& file) {
writer.open(file); writer.open(file);
active = true; active = true;
} }
void stop() { void stop() {
active = false; active = false;
writer.close(); writer.close();
} }
public: public:
virtual void onSensorData(Sensor<AccelerometerData>* , const Timestamp ts, const AccelerometerData& data) override { virtual void onSensorData(Sensor<AccelerometerData>* , const Timestamp ts, const AccelerometerData& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
virtual void onSensorData(Sensor<GyroscopeData>* , const Timestamp ts, const GyroscopeData& data) override { virtual void onSensorData(Sensor<GyroscopeData>* , const Timestamp ts, const GyroscopeData& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
virtual void onSensorData(Sensor<BarometerData>* , const Timestamp ts, const BarometerData& data) override { virtual void onSensorData(Sensor<BarometerData>* , const Timestamp ts, const BarometerData& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
virtual void onSensorData(Sensor<WiFiMeasurements>* , const Timestamp ts, const WiFiMeasurements& data) override { virtual void onSensorData(Sensor<WiFiMeasurements>* , const Timestamp ts, const WiFiMeasurements& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
virtual void onSensorData(Sensor<GPSData>* , const Timestamp ts, const GPSData& data) override { virtual void onSensorData(Sensor<GPSData>* , const Timestamp ts, const GPSData& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
virtual void onSensorData(Sensor<CompassData>* , const Timestamp ts, const CompassData& data) override { virtual void onSensorData(Sensor<CompassData>* , const Timestamp ts, const CompassData& data) override {
if (!active) {return;} if (!active) {return;}
writer.add(ts, data); writer.add(ts, data);
} }
}; };

1
sensors/TurnSensor.h
View File

@@ -1,6 +1,7 @@
#ifndef TURNSENSOR_H #ifndef TURNSENSOR_H
#define TURNSENSOR_H #define TURNSENSOR_H
#include <Indoor/sensors/imu/PoseDetection.h>
#include <Indoor/sensors/imu/TurnDetection.h> #include <Indoor/sensors/imu/TurnDetection.h>
#include "AccelerometerSensor.h" #include "AccelerometerSensor.h"
#include "GyroscopeSensor.h" #include "GyroscopeSensor.h"

18
sensors/android/BLESensorAndroid.cpp Normal file
View File

@@ -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

41
sensors/android/SensorFactoryAndroid.h
View File

@@ -5,6 +5,7 @@
#include "../SensorFactory.h" #include "../SensorFactory.h"
#include "BLESensorAndroid.h"
#include "WiFiSensorAndroid.h" #include "WiFiSensorAndroid.h"
#include "AccelerometerSensorAndroid.h" #include "AccelerometerSensorAndroid.h"
#include "GyroscopeSensorAndroid.h" #include "GyroscopeSensorAndroid.h"
@@ -20,29 +21,33 @@ class SensorFactoryAndroid : public SensorFactory {
public: public:
WiFiSensor& getWiFi() override { WiFiSensor& getWiFi() override {
return WiFiSensorAndroid::get(); return WiFiSensorAndroid::get();
} }
AccelerometerSensor& getAccelerometer() override { AccelerometerSensor& getAccelerometer() override {
return AccelerometerSensorAndroid::get(); return AccelerometerSensorAndroid::get();
} }
GyroscopeSensor& getGyroscope() override { GyroscopeSensor& getGyroscope() override {
return GyroscopeSensorAndroid::get(); return GyroscopeSensorAndroid::get();
} }
BarometerSensor& getBarometer() override { BarometerSensor& getBarometer() override {
return BarometerSensorAndroid::get(); return BarometerSensorAndroid::get();
} }
CompassSensor& getCompass() override { CompassSensor& getCompass() override {
return CompassSensorAndroid::get(); return CompassSensorAndroid::get();
} }
GPSSensor& getGPS() override { GPSSensor& getGPS() override {
return GPSSensorAndroid::get(); return GPSSensorAndroid::get();
} }
BLESensor& getBLE() override {
return BLESensorAndroid::get();
}
}; };

4
tools/calibration/BLECalibrationDataModel.h Normal file
View File

@@ -0,0 +1,4 @@
#ifndef BLECALIBRATIONDATAMODEL_H
#define BLECALIBRATIONDATAMODEL_H
#endif // BLECALIBRATIONDATAMODEL_H

124
tools/calibration/WiFiCalibrationScanDialog.h
View File

@@ -16,84 +16,94 @@
/** /**
* show a dialog to perform a WiFiScan * show a dialog to perform a WiFiScan
*/ */
class WiFiCalibrationScanDialog : public QObject, public SensorListener<WiFiMeasurements> { class WiFiCalibrationScanDialog : public QObject, public SensorListener<WiFiMeasurements>, public SensorListener<BeaconMeasurement> {
Q_OBJECT Q_OBJECT
private: private:
QDialog* dlg = new QDialog(); QDialog* dlg = new QDialog();
QLabel* lblStats; QLabel* lblStats;
QLabel* lblPoint; QLabel* lblPoint;
QProgressBar* barProg; QProgressBar* barProg;
struct Scan { struct Scan {
const int numRecords = 30; const int numRecords = 30;
int recordsDone = 0; int recordsDone = 0;
} scan; } scan;
/** the measurements model to fill with scan-entries */ /** the measurements model to fill with scan-entries */
WiFiFingerprint& model; WiFiFingerprint& model;
//BeaconFingerprint& blemodel;
public: public:
static void get(WiFiFingerprint& model) { static void get(WiFiFingerprint& model) {
WiFiCalibrationScanDialog dlg(model); WiFiCalibrationScanDialog dlg(model);
dlg.show(); dlg.show();
} }
private: private:
/** ctor */ /** ctor */
WiFiCalibrationScanDialog(WiFiFingerprint& model); WiFiCalibrationScanDialog(WiFiFingerprint& model);
void show() { void show() {
refresh(); refresh();
dlg->exec(); dlg->exec();
} }
void close() { void close() {
stopRecord(); stopRecord();
dlg->close(); dlg->close();
} }
void save() { void save() {
//mdl.save();; //mdl.save();;
} }
Q_INVOKABLE void refresh() { Q_INVOKABLE void refresh() {
lblPoint->setText(QString(model.pos_m.asString().c_str())); 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"); lblStats->setText( QString::number(model.measurements.entries.size()) + " RSSI measurements\n" + QString::number(scan.recordsDone) + " scans");
barProg->setValue(scan.recordsDone); barProg->setValue(scan.recordsDone);
barProg->setMaximum(scan.numRecords); barProg->setMaximum(scan.numRecords);
} }
void clear() { void clear() {
model.measurements.entries.clear(); model.measurements.entries.clear();
refresh(); refresh();
} }
void startRecord() { void startRecord() {
scan.recordsDone = 0; scan.recordsDone = 0;
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
if (!SensorFactory::get().getWiFi().isRunning()) { if (!SensorFactory::get().getWiFi().isRunning()) {
SensorFactory::get().getWiFi().start(); SensorFactory::get().getWiFi().start();
} }
} }
void stopRecord() { void stopRecord() {
SensorFactory::get().getWiFi().removeListener(this); SensorFactory::get().getWiFi().removeListener(this);
} }
virtual void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override { virtual void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor; (void) sensor;
(void) ts; (void) ts;
++scan.recordsDone; ++scan.recordsDone;
if (scan.recordsDone >= scan.numRecords) {stopRecord();} if (scan.recordsDone >= scan.numRecords) {stopRecord();}
model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end()); model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end());
QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection); QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection);
} }
//tmp stuff
virtual void onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) override {
(void) sensor;
(void) ts;
QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection);
}
}; };

86
ui/map/2D/ColorPoints2D.h
View File

@@ -64,59 +64,59 @@ public:
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
void setFromParticles(const std::vector<SMC::Particle<GridBased::MyState>>& particles){ void setFromParticles(const std::vector<SMC::Particle<GridBased::MyState>>& particles){
points.clear(); points.clear();
// group particles by grid-point // group particles by grid-point
std::unordered_map<GridPoint, float> weights; std::unordered_map<GridPoint, float> weights;
for (const SMC::Particle<GridBased::MyState>& p : particles) { for (const SMC::Particle<GridBased::MyState>& p : particles) {
const GridPoint gp = p.state.position; const GridPoint gp = p.state.position;
if (weights.find(gp) != weights.end()) {continue;} if (weights.find(gp) != weights.end()) {continue;}
weights[gp] += p.weight; weights[gp] += p.weight;
} }
// find min/max // find min/max
float min = +INFINITY; float min = +INFINITY;
float max = -INFINITY; float max = -INFINITY;
for (auto it : weights) { for (auto it : weights) {
if (it.second > max) {max = it.second;} if (it.second > max) {max = it.second;}
if (it.second < min) {min = it.second;} if (it.second < min) {min = it.second;}
} }
// draw colored // draw colored
for (auto it : weights) { for (auto it : weights) {
const GridPoint gp = it.first; const GridPoint gp = it.first;
const float w = it.second; const float w = it.second;
const float p = (w-min) / (max-min); // [0:1] 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); 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 float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1); const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color)); points.push_back(PT(pt, color));
} }
} }
void setFromParticles(const std::vector<SMC::Particle<MeshBased::MyState>>& particles){ void setFromParticles(const std::vector<SMC::Particle<MeshBased::MyState>>& particles){
points.clear(); points.clear();
float min = +INFINITY; float min = +INFINITY;
float max = -INFINITY; float max = -INFINITY;
for (const auto p : particles){ for (const auto p : particles){
if (p.weight > max) {max = p.weight;} if (p.weight > max) {max = p.weight;}
if (p.weight < min) {min = p.weight;} if (p.weight < min) {min = p.weight;}
} }
for (const auto p : particles) { for (const auto p : particles) {
const Point3 pt(p.state.loc.pos.x, p.state.loc.pos.y, p.state.loc.pos.z); 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); const float prob = (p.weight-min) / (max-min);
float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1); const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color)); points.push_back(PT(pt, color));
} }
} }
protected: protected:

11
ui/map/2D/Floor2D.h
View File

@@ -29,7 +29,7 @@ protected:
if (floor->atHeight > r.clip.aboveHeight_m) {return;} if (floor->atHeight > r.clip.aboveHeight_m) {return;}
for (const Floorplan::FloorObstacle* obs : floor->obstacles) { for (const Floorplan::FloorObstacle* obs : floor->obstacles) {
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs); const Floorplan::FloorObstacleWall* line = dynamic_cast<const Floorplan::FloorObstacleWall*>(obs);
if (line) {drawObstacle(qp, s, line);} if (line) {drawObstacle(qp, s, line);}
} }
@@ -51,10 +51,11 @@ protected:
private: 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; using namespace Floorplan;
QPen pen; pen.setColor(Qt::darkGray); 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 (mat == Material::GLASS) {pen.setStyle(Qt::PenStyle::DotLine);}
if (type == ObstacleType::HANDRAIL) {pen.setStyle(Qt::PenStyle::DashLine);} if (type == ObstacleType::HANDRAIL) {pen.setStyle(Qt::PenStyle::DashLine);}
if (type == ObstacleType::UNKNOWN) {pen.setColor(Qt::red); pen.setWidth(5);} if (type == ObstacleType::UNKNOWN) {pen.setColor(Qt::red); pen.setWidth(5);}
@@ -63,10 +64,10 @@ private:
return pen; 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 pt1 = s.mapToScreen(line->from);
const Point2 pt2 = s.mapToScreen(line->to); const Point2 pt2 = s.mapToScreen(line->to);
qp.setPen(getPen(line->material, line->type)); qp.setPen(getPen(line->material, line->type, static_cast<int>(s.mToPX(line->thickness_m))));
qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y); qp.drawLine(pt1.x, pt1.y, pt2.x, pt2.y);
} }

309
yasmin.pro
View File

@@ -7,27 +7,27 @@ QT += qml opengl svg
CONFIG+=ANDROID DEFINES+=ANDROID CONFIG+=ANDROID DEFINES+=ANDROID
ANDROID { ANDROID {
QT += androidextras QT += androidextras
QT += sensors QT += sensors
QT += positioning QT += positioning
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545 #http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
# wasn't faster... and seems dangerous :P # wasn't faster... and seems dangerous :P
#QMAKE_CXXFLAGS -= -mfpu=vfp #QMAKE_CXXFLAGS -= -mfpu=vfp
#QMAKE_CFLAGS -= -mfpu=vfp #QMAKE_CFLAGS -= -mfpu=vfp
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations #QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations #QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
# wasn't faster.. # wasn't faster..
#QMAKE_CXXFLAGS += -mtune=cortex-a57 #QMAKE_CXXFLAGS += -mtune=cortex-a57
#QMAKE_CFLAGS += -mtune=cortex-a57 #QMAKE_CFLAGS += -mtune=cortex-a57
QMAKE_CXXFLAGS += -O2 QMAKE_CXXFLAGS += -O2
#QMAKE_CFLAGS += -O3 #QMAKE_CFLAGS += -O3
#QMAKE_CXXFLAGS_DEBUG -= -O2 #QMAKE_CXXFLAGS_DEBUG -= -O2
#QMAKE_CXXFLAGS_DEBUG -= -O3 #QMAKE_CXXFLAGS_DEBUG -= -O3
#QMAKE_CXXFLAGS_DEBUG += -O0 #QMAKE_CXXFLAGS_DEBUG += -O0
} }
@@ -49,8 +49,8 @@ CONFIG += c++11
ANDROID_PACKAGE_SOURCE_DIR = $$PWD/_android ANDROID_PACKAGE_SOURCE_DIR = $$PWD/_android
INCLUDEPATH += \ INCLUDEPATH += \
../ \ ../ \
./lib/ ./lib/
# linux desktop wifi # linux desktop wifi
@@ -58,33 +58,35 @@ INCLUDEPATH += \
#LIBS += -lnl-genl-3 -lnl-3 #LIBS += -lnl-genl-3 -lnl-3
OTHER_FILES += \ OTHER_FILES += \
_android/src/WiFi.java \ _android/src/WiFi.java \
_android/src/MyActivity.java \ _android/src/MyActivity.java \
_android/AndroidManifest.xml _android/AndroidManifest.xml \
_android/src/BLE.java
SOURCES += \ SOURCES += \
main.cpp \ main.cpp \
lib/gpc/gpc.cpp \ lib/gpc/gpc.cpp \
../Indoor/lib/tinyxml/tinyxml2.cpp \ ../Indoor/lib/tinyxml/tinyxml2.cpp \
../Indoor/lib/Recast/*.cpp\ ../Indoor/lib/Recast/*.cpp\
ui/menu/MainMenu.cpp \ ui/menu/MainMenu.cpp \
ui/MainWindow.cpp \ ui/MainWindow.cpp \
Controller.cpp \ Controller.cpp \
ui/dialog/LoadSetupDialog.cpp \ ui/dialog/LoadSetupDialog.cpp \
ui/debug/SensorDataWidget.cpp \ ui/debug/SensorDataWidget.cpp \
ui/debug/plot/PlottWidget.cpp \ ui/debug/plot/PlottWidget.cpp \
ui/debug/PlotTurns.cpp \ ui/debug/PlotTurns.cpp \
ui/debug/PlotWiFiScan.cpp \ ui/debug/PlotWiFiScan.cpp \
sensors/android/WiFiSensorAndroid.cpp \ sensors/android/WiFiSensorAndroid.cpp \
sensors/linux/WiFiSensorLinuxC.c \ sensors/linux/WiFiSensorLinuxC.c \
ui/debug/InfoWidget.cpp \ ui/debug/InfoWidget.cpp \
ui/map/3D/MapView3D.cpp \ ui/map/3D/MapView3D.cpp \
ui/map/2D/MapView2D.cpp \ ui/map/2D/MapView2D.cpp \
tools/calibration/WiFiCalibrationScanDialog.cpp \ tools/calibration/WiFiCalibrationScanDialog.cpp \
ui/debug/PlotGPS.cpp \ ui/debug/PlotGPS.cpp \
nav/NavController.cpp \ nav/NavController.cpp \
nav/mesh/NavControllerMesh.cpp \ nav/mesh/NavControllerMesh.cpp \
nav/grid/NavControllerGrid.cpp nav/grid/NavControllerGrid.cpp \
sensors/android/BLESensorAndroid.cpp
RESOURCES += qml.qrc RESOURCES += qml.qrc
@@ -102,113 +104,116 @@ INSTALLS += target
# android-sources/src/WiFi.java # android-sources/src/WiFi.java
HEADERS += \ HEADERS += \
lib/gpc/gpc.h \ lib/gpc/gpc.h \
sensors/linux/WiFiSensorLinux.h \ sensors/linux/WiFiSensorLinux.h \
sensors/android/WiFiSensorAndroid.h \ sensors/android/WiFiSensorAndroid.h \
sensors/StepSensor.h \ sensors/StepSensor.h \
sensors/TurnSensor.h \ sensors/TurnSensor.h \
sensors/AccelerometerSensor.h \ sensors/AccelerometerSensor.h \
sensors/GyroscopeSensor.h \ sensors/GyroscopeSensor.h \
sensors/BarometerSensor.h \ sensors/BarometerSensor.h \
sensors/android/AccelerometerSensorAndroid.h \ sensors/android/AccelerometerSensorAndroid.h \
sensors/android/GyroscopeSensorAndroid.h \ sensors/android/GyroscopeSensorAndroid.h \
sensors/android/BarometerSensorAndroid.h \ sensors/android/BarometerSensorAndroid.h \
sensors/dummy/AccelerometerSensorDummy.h \ sensors/android/BLESensorAndroid.h \
sensors/dummy/GyroscopeSensorDummy.h \ sensors/dummy/AccelerometerSensorDummy.h \
sensors/dummy/BarometerSensorDummy.h \ sensors/dummy/GyroscopeSensorDummy.h \
sensors/Sensor.h \ sensors/dummy/BarometerSensorDummy.h \
sensors/SensorFactory.h \ sensors/Sensor.h \
sensors/WiFiSensor.h \ sensors/SensorFactory.h \
misc/Debug.h \ sensors/WiFiSensor.h \
misc/fixc11.h \ misc/Debug.h \
sensors/dummy/WiFiSensorDummy.h \ misc/fixc11.h \
lib/gpc/Polygon.h \ sensors/dummy/WiFiSensorDummy.h \
ui/map/3D/FloorRenderer.h \ lib/gpc/Polygon.h \
ui/map/3D/gl/GL.h \ ui/map/3D/FloorRenderer.h \
ui/map/3D/gl/GLHelper.h \ ui/map/3D/gl/GL.h \
ui/map/3D/gl/GLLines.h \ ui/map/3D/gl/GLHelper.h \
ui/map/3D/gl/GLTriangles.h \ ui/map/3D/gl/GLLines.h \
ui/map/3D/elements/Doors.h \ ui/map/3D/gl/GLTriangles.h \
ui/map/3D/elements/Ground.h \ ui/map/3D/elements/Doors.h \
ui/map/3D/elements/Handrails.h \ ui/map/3D/elements/Ground.h \
ui/map/3D/elements/Path.h \ ui/map/3D/elements/Handrails.h \
ui/map/3D/elements/Stairs.h \ ui/map/3D/elements/Path.h \
ui/map/3D/elements/Walls.h \ ui/map/3D/elements/Stairs.h \
ui/map/3D/gl/GLPoints.h \ ui/map/3D/elements/Walls.h \
ui/map/3D/elements/ColorPoints.h \ ui/map/3D/gl/GLPoints.h \
ui/map/3D/RenderParams.h \ ui/map/3D/elements/ColorPoints.h \
ui/map/3D/Renderable.h \ ui/map/3D/RenderParams.h \
ui/map/3D/gl/Shader.h \ ui/map/3D/Renderable.h \
ui/map/3D/elements/Object.h \ ui/map/3D/gl/Shader.h \
ui/Icons.h \ ui/map/3D/elements/Object.h \
ui/MainWindow.h \ ui/Icons.h \
Controller.h \ ui/MainWindow.h \
ui/menu/MainMenu.h \ Controller.h \
ui/dialog/LoadSetupDialog.h \ ui/menu/MainMenu.h \
ui/debug/plot/Axes.h \ ui/dialog/LoadSetupDialog.h \
ui/debug/plot/Plot.h \ ui/debug/plot/Axes.h \
ui/debug/plot/Data.h \ ui/debug/plot/Plot.h \
ui/debug/plot/Range.h \ ui/debug/plot/Data.h \
nav/NavController.h \ ui/debug/plot/Range.h \
sensors/dummy/RandomSensor.h \ nav/NavController.h \
ui/debug/SensorDataWidget.h \ sensors/dummy/RandomSensor.h \
ui/debug/plot/PlottWidget.h \ ui/debug/SensorDataWidget.h \
ui/debug/PlotTurns.h \ ui/debug/plot/PlottWidget.h \
ui/debug/PlotWiFiScan.h \ ui/debug/PlotTurns.h \
sensors/linux/WiFiSensorLinuxC.h \ ui/debug/PlotWiFiScan.h \
sensors/offline/SensorFactoryOffline.h \ sensors/linux/WiFiSensorLinuxC.h \
sensors/dummy/SensorFactoryDummy.h \ sensors/offline/SensorFactoryOffline.h \
sensors/android/SensorFactoryAndroid.h \ sensors/dummy/SensorFactoryDummy.h \
Settings.h \ sensors/android/SensorFactoryAndroid.h \
sensors/offline/AllInOneSensor.h \ Settings.h \
sensors/ActivitySensor.h \ sensors/offline/AllInOneSensor.h \
ui/LoggerUI.h \ sensors/ActivitySensor.h \
ui/debug/InfoWidget.h \ ui/LoggerUI.h \
ui/UIHelper.h \ ui/debug/InfoWidget.h \
ui/map/3D/MapView3D.h \ ui/UIHelper.h \
ui/map/2D/MapView2D.h \ ui/map/3D/MapView3D.h \
ui/map/2D/Floor2D.h \ ui/map/2D/MapView2D.h \
ui/map/2D/Scaler2D.h \ ui/map/2D/Floor2D.h \
ui/map/2D/Renderable2D.h \ ui/map/2D/Scaler2D.h \
ui/map/2D/RenderParams2D.h \ ui/map/2D/Renderable2D.h \
ui/map/2D/ColorPoints2D.h \ ui/map/2D/RenderParams2D.h \
ui/map/2D/Path2D.h \ ui/map/2D/ColorPoints2D.h \
ui/map/2D/WiFiCalibTool.h \ ui/map/2D/Path2D.h \
ui/map/2D/HasSelectableNodes.h \ ui/map/2D/WiFiCalibTool.h \
tools/calibration/WiFiCalibrationDataModel.h \ ui/map/2D/HasSelectableNodes.h \
tools/calibration/WiFiCalibrationScanDialog.h \ tools/calibration/WiFiCalibrationDataModel.h \
tests/RuntimeTests.h \ tools/calibration/WiFiCalibrationScanDialog.h \
ipin/StepLogger.h \ tests/RuntimeTests.h \
ipin/Scaler.h \ ipin/StepLogger.h \
ipin/Config.h \ ipin/Scaler.h \
ipin/IPINHelper.h \ ipin/Config.h \
nav/NavControllerListener.h \ ipin/IPINHelper.h \
ipin/StepLoggerWrapper.h \ nav/NavControllerListener.h \
ipin/StepLoggerWrapperAndroid.h \ ipin/StepLoggerWrapper.h \
sensors/GPSSensor.h \ ipin/StepLoggerWrapperAndroid.h \
sensors/android/GPSSensorAndroid.h \ sensors/GPSSensor.h \
sensors/CompassSensor.h \ sensors/android/GPSSensorAndroid.h \
sensors/android/CompassSensorAndroid.h \ sensors/CompassSensor.h \
sensors/dummy/CompassSensorDummy.h \ sensors/android/CompassSensorAndroid.h \
sensors/dummy/GPSSensorDummy.h \ sensors/dummy/CompassSensorDummy.h \
ui/debug/PlotGPS.h \ sensors/dummy/GPSSensorDummy.h \
sensors/SensorWriter.h \ ui/debug/PlotGPS.h \
nav/mesh/State.h \ sensors/SensorWriter.h \
nav/mesh/NavControllerMesh.h \ nav/mesh/State.h \
nav/mesh/FilterMesh.h \ nav/mesh/NavControllerMesh.h \
nav/grid/Filter.h \ nav/mesh/FilterMesh.h \
nav/grid/NavControllerGrid.h \ nav/grid/Filter.h \
nav/grid/Node.h \ nav/grid/NavControllerGrid.h \
nav/grid/NodeResampling.h \ nav/grid/Node.h \
nav/grid/RegionalResampling.h \ nav/grid/NodeResampling.h \
nav/Observation.h \ nav/grid/RegionalResampling.h \
nav/grid/State.h \ nav/Observation.h \
nav/CurEst.h nav/grid/State.h \
nav/CurEst.h \
sensors/BLESensor.h \
tools/calibration/BLECalibrationDataModel.h
DISTFILES += \ DISTFILES += \
android-sources/src/MyActivity.java \ android-sources/src/MyActivity.java \
res/gl/vertex1.glsl \ res/gl/vertex1.glsl \
res/gl/fragment1.glsl \ res/gl/fragment1.glsl \
res/gl/tex/empty_normals.jpg \ res/gl/tex/empty_normals.jpg \
_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \ _android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \
_android/src/StepLoggerClient.java _android/src/StepLoggerClient.java