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

2
Settings.h
View File

@@ -39,7 +39,7 @@ namespace Settings {
namespace WiFiModel {
constexpr float sigma = 8.0; //TODO: im Museum hatten wir 8.0
constexpr float sigma = 10.0; //TODO: im Museum hatten wir 8.0
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -45;

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

77
nav/NavController.h
View File

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

46
nav/Observation.h
View File

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

255
nav/grid/Filter.h
View File

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

291
nav/grid/NavControllerGrid.cpp
View File

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

64
nav/grid/NavControllerGrid.h
View File

@@ -21,68 +21,70 @@
namespace GridBased {
class NavControllerGrid : public NavController {
class NavControllerGrid : public NavController {
private:
private:
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest;
DijkstraPath<MyGridNode> pathToDest;
MyObservation curObs;
MyControl curCtrl;
MyObservation curObs;
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 ;
private:
private:
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
// void debugActivity(const ActivityData& activity);
// void debugActivity(const ActivityData& activity);
/** particle-filter update loop */
void filterUpdateLoop();
/** particle-filter update loop */
void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** UI update loop */
void updateMapViewLoop();
/** UI update loop */
void updateMapViewLoop();
};
};
}

176
nav/mesh/FilterMesh.h
View File

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

207
nav/mesh/NavControllerMesh.cpp
View File

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

56
nav/mesh/NavControllerMesh.h
View File

@@ -24,68 +24,70 @@
namespace MeshBased {
class NavControllerMesh : public NavController {
class NavControllerMesh : public NavController {
private:
private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs;
MyControl curCtrl;
MyObservation curObs;
MyControl curCtrl;
public:
public:
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 ;
private:
private:
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
void debugActivity(const ActivityData& activity);
/** particle-filter update loop */
void filterUpdateLoop();
/** particle-filter update loop */
void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update only with estimation (called from a background-loop) */
void filterUpdateEstimationOnly();
/** UI update loop */
void updateMapViewLoop();
/** UI update loop */
void updateMapViewLoop();
};
};
}

74
nav/mesh/State.h
View File

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

80
sensors/SensorFactory.h
View File

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

101
sensors/SensorWriter.h
View File

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

1
sensors/TurnSensor.h
View File

@@ -1,6 +1,7 @@
#ifndef TURNSENSOR_H
#define TURNSENSOR_H
#include <Indoor/sensors/imu/PoseDetection.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include "AccelerometerSensor.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 "BLESensorAndroid.h"
#include "WiFiSensorAndroid.h"
#include "AccelerometerSensorAndroid.h"
#include "GyroscopeSensorAndroid.h"
@@ -20,29 +21,33 @@ class SensorFactoryAndroid : public SensorFactory {
public:
WiFiSensor& getWiFi() override {
return WiFiSensorAndroid::get();
}
WiFiSensor& getWiFi() override {
return WiFiSensorAndroid::get();
}
AccelerometerSensor& getAccelerometer() override {
return AccelerometerSensorAndroid::get();
}
AccelerometerSensor& getAccelerometer() override {
return AccelerometerSensorAndroid::get();
}
GyroscopeSensor& getGyroscope() override {
return GyroscopeSensorAndroid::get();
}
GyroscopeSensor& getGyroscope() override {
return GyroscopeSensorAndroid::get();
}
BarometerSensor& getBarometer() override {
return BarometerSensorAndroid::get();
}
BarometerSensor& getBarometer() override {
return BarometerSensorAndroid::get();
}
CompassSensor& getCompass() override {
return CompassSensorAndroid::get();
}
CompassSensor& getCompass() override {
return CompassSensorAndroid::get();
}
GPSSensor& getGPS() override {
return GPSSensorAndroid::get();
}
GPSSensor& getGPS() override {
return GPSSensorAndroid::get();
}
BLESensor& getBLE() override {
return BLESensorAndroid::get();
}
};

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
*/
class WiFiCalibrationScanDialog : public QObject, public SensorListener<WiFiMeasurements> {
class WiFiCalibrationScanDialog : public QObject, public SensorListener<WiFiMeasurements>, public SensorListener<BeaconMeasurement> {
Q_OBJECT
Q_OBJECT
private:
QDialog* dlg = new QDialog();
QLabel* lblStats;
QLabel* lblPoint;
QProgressBar* barProg;
QDialog* dlg = new QDialog();
QLabel* lblStats;
QLabel* lblPoint;
QProgressBar* barProg;
struct Scan {
const int numRecords = 30;
int recordsDone = 0;
} scan;
struct Scan {
const int numRecords = 30;
int recordsDone = 0;
} scan;
/** the measurements model to fill with scan-entries */
WiFiFingerprint& model;
/** the measurements model to fill with scan-entries */
WiFiFingerprint& model;
//BeaconFingerprint& blemodel;
public:
static void get(WiFiFingerprint& model) {
WiFiCalibrationScanDialog dlg(model);
dlg.show();
}
static void get(WiFiFingerprint& model) {
WiFiCalibrationScanDialog dlg(model);
dlg.show();
}
private:
/** ctor */
WiFiCalibrationScanDialog(WiFiFingerprint& model);
/** ctor */
WiFiCalibrationScanDialog(WiFiFingerprint& model);
void show() {
refresh();
dlg->exec();
}
void show() {
refresh();
dlg->exec();
}
void close() {
stopRecord();
dlg->close();
}
void close() {
stopRecord();
dlg->close();
}
void save() {
//mdl.save();;
}
void save() {
//mdl.save();;
}
Q_INVOKABLE void refresh() {
lblPoint->setText(QString(model.pos_m.asString().c_str()));
lblStats->setText( QString::number(model.measurements.entries.size()) + " RSSI measurements\n" + QString::number(scan.recordsDone) + " scans");
barProg->setValue(scan.recordsDone);
barProg->setMaximum(scan.numRecords);
}
Q_INVOKABLE void refresh() {
lblPoint->setText(QString(model.pos_m.asString().c_str()));
lblStats->setText( QString::number(model.measurements.entries.size()) + " RSSI measurements\n" + QString::number(scan.recordsDone) + " scans");
barProg->setValue(scan.recordsDone);
barProg->setMaximum(scan.numRecords);
}
void clear() {
model.measurements.entries.clear();
refresh();
}
void clear() {
model.measurements.entries.clear();
refresh();
}
void startRecord() {
scan.recordsDone = 0;
SensorFactory::get().getWiFi().addListener(this);
if (!SensorFactory::get().getWiFi().isRunning()) {
SensorFactory::get().getWiFi().start();
}
}
void startRecord() {
scan.recordsDone = 0;
SensorFactory::get().getWiFi().addListener(this);
if (!SensorFactory::get().getWiFi().isRunning()) {
SensorFactory::get().getWiFi().start();
}
}
void stopRecord() {
SensorFactory::get().getWiFi().removeListener(this);
}
void stopRecord() {
SensorFactory::get().getWiFi().removeListener(this);
}
virtual void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor;
(void) ts;
++scan.recordsDone;
if (scan.recordsDone >= scan.numRecords) {stopRecord();}
model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end());
QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection);
}
virtual void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor;
(void) ts;
++scan.recordsDone;
if (scan.recordsDone >= scan.numRecords) {stopRecord();}
model.measurements.entries.insert(model.measurements.entries.end(), data.entries.begin(), data.entries.end());
QMetaObject::invokeMethod(this, "refresh", Qt::QueuedConnection);
}
//tmp stuff
virtual void onSensorData(Sensor<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! */
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
std::unordered_map<GridPoint, float> weights;
for (const SMC::Particle<GridBased::MyState>& p : particles) {
const GridPoint gp = p.state.position;
if (weights.find(gp) != weights.end()) {continue;}
weights[gp] += p.weight;
}
// group particles by grid-point
std::unordered_map<GridPoint, float> weights;
for (const SMC::Particle<GridBased::MyState>& p : particles) {
const GridPoint gp = p.state.position;
if (weights.find(gp) != weights.end()) {continue;}
weights[gp] += p.weight;
}
// find min/max
float min = +INFINITY;
float max = -INFINITY;
for (auto it : weights) {
if (it.second > max) {max = it.second;}
if (it.second < min) {min = it.second;}
}
// find min/max
float min = +INFINITY;
float max = -INFINITY;
for (auto it : weights) {
if (it.second > max) {max = it.second;}
if (it.second < min) {min = it.second;}
}
// draw colored
for (auto it : weights) {
const GridPoint gp = it.first;
const float w = it.second;
const float p = (w-min) / (max-min); // [0:1]
const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f);
float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color));
}
// draw colored
for (auto it : weights) {
const GridPoint gp = it.first;
const float w = it.second;
const float p = (w-min) / (max-min); // [0:1]
const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f);
float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color));
}
}
}
void setFromParticles(const std::vector<SMC::Particle<MeshBased::MyState>>& particles){
void setFromParticles(const std::vector<SMC::Particle<MeshBased::MyState>>& particles){
points.clear();
points.clear();
float min = +INFINITY;
float max = -INFINITY;
for (const auto p : particles){
if (p.weight > max) {max = p.weight;}
if (p.weight < min) {min = p.weight;}
}
float min = +INFINITY;
float max = -INFINITY;
for (const auto p : particles){
if (p.weight > max) {max = p.weight;}
if (p.weight < min) {min = p.weight;}
}
for (const auto p : particles) {
const Point3 pt(p.state.loc.pos.x, p.state.loc.pos.y, p.state.loc.pos.z);
const float prob = (p.weight-min) / (max-min);
float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color));
for (const auto p : particles) {
const Point3 pt(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const float prob = (p.weight-min) / (max-min);
float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale
const QColor color = QColor::fromHsvF(h, 1, 1);
points.push_back(PT(pt, color));
}
}
}
}
protected:

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

@@ -29,7 +29,7 @@ protected:
if (floor->atHeight > r.clip.aboveHeight_m) {return;}
for (const Floorplan::FloorObstacle* obs : floor->obstacles) {
const Floorplan::FloorObstacleLine* line = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
const Floorplan::FloorObstacleWall* line = dynamic_cast<const Floorplan::FloorObstacleWall*>(obs);
if (line) {drawObstacle(qp, s, line);}
}
@@ -51,10 +51,11 @@ protected:
private:
static inline QPen getPen(Floorplan::Material mat, Floorplan::ObstacleType type) {
static inline QPen getPen(Floorplan::Material mat, Floorplan::ObstacleType type, int thickness) {
using namespace Floorplan;
QPen pen; pen.setColor(Qt::darkGray);
if (mat == Material::CONCRETE) {pen.setWidth(3);}
if (mat == Material::CONCRETE) {pen.setWidth(thickness);}
if (mat == Material::DRYWALL) {pen.setWidth(thickness); pen.setColor(Qt::gray);}
if (mat == Material::GLASS) {pen.setStyle(Qt::PenStyle::DotLine);}
if (type == ObstacleType::HANDRAIL) {pen.setStyle(Qt::PenStyle::DashLine);}
if (type == ObstacleType::UNKNOWN) {pen.setColor(Qt::red); pen.setWidth(5);}
@@ -63,10 +64,10 @@ private:
return pen;
}
void drawObstacle(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleLine* line) {
void drawObstacle(QPainter& qp, const Scaler2D& s, const Floorplan::FloorObstacleWall* line) {
const Point2 pt1 = s.mapToScreen(line->from);
const Point2 pt2 = s.mapToScreen(line->to);
qp.setPen(getPen(line->material, line->type));
qp.setPen(getPen(line->material, line->type, static_cast<int>(s.mToPX(line->thickness_m))));
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
ANDROID {
QT += androidextras
QT += sensors
QT += positioning
QT += androidextras
QT += sensors
QT += positioning
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
# wasn't faster... and seems dangerous :P
#QMAKE_CXXFLAGS -= -mfpu=vfp
#QMAKE_CFLAGS -= -mfpu=vfp
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
# wasn't faster... and seems dangerous :P
#QMAKE_CXXFLAGS -= -mfpu=vfp
#QMAKE_CFLAGS -= -mfpu=vfp
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
# wasn't faster..
#QMAKE_CXXFLAGS += -mtune=cortex-a57
#QMAKE_CFLAGS += -mtune=cortex-a57
# wasn't faster..
#QMAKE_CXXFLAGS += -mtune=cortex-a57
#QMAKE_CFLAGS += -mtune=cortex-a57
QMAKE_CXXFLAGS += -O2
#QMAKE_CFLAGS += -O3
QMAKE_CXXFLAGS += -O2
#QMAKE_CFLAGS += -O3
#QMAKE_CXXFLAGS_DEBUG -= -O2
#QMAKE_CXXFLAGS_DEBUG -= -O3
#QMAKE_CXXFLAGS_DEBUG += -O0
#QMAKE_CXXFLAGS_DEBUG -= -O2
#QMAKE_CXXFLAGS_DEBUG -= -O3
#QMAKE_CXXFLAGS_DEBUG += -O0
}
@@ -49,8 +49,8 @@ CONFIG += c++11
ANDROID_PACKAGE_SOURCE_DIR = $$PWD/_android
INCLUDEPATH += \
../ \
./lib/
../ \
./lib/
# linux desktop wifi
@@ -58,33 +58,35 @@ INCLUDEPATH += \
#LIBS += -lnl-genl-3 -lnl-3
OTHER_FILES += \
_android/src/WiFi.java \
_android/src/MyActivity.java \
_android/AndroidManifest.xml
_android/src/WiFi.java \
_android/src/MyActivity.java \
_android/AndroidManifest.xml \
_android/src/BLE.java
SOURCES += \
main.cpp \
lib/gpc/gpc.cpp \
../Indoor/lib/tinyxml/tinyxml2.cpp \
../Indoor/lib/Recast/*.cpp\
ui/menu/MainMenu.cpp \
ui/MainWindow.cpp \
Controller.cpp \
ui/dialog/LoadSetupDialog.cpp \
ui/debug/SensorDataWidget.cpp \
ui/debug/plot/PlottWidget.cpp \
ui/debug/PlotTurns.cpp \
ui/debug/PlotWiFiScan.cpp \
sensors/android/WiFiSensorAndroid.cpp \
sensors/linux/WiFiSensorLinuxC.c \
ui/debug/InfoWidget.cpp \
ui/map/3D/MapView3D.cpp \
ui/map/2D/MapView2D.cpp \
tools/calibration/WiFiCalibrationScanDialog.cpp \
ui/debug/PlotGPS.cpp \
nav/NavController.cpp \
nav/mesh/NavControllerMesh.cpp \
nav/grid/NavControllerGrid.cpp
main.cpp \
lib/gpc/gpc.cpp \
../Indoor/lib/tinyxml/tinyxml2.cpp \
../Indoor/lib/Recast/*.cpp\
ui/menu/MainMenu.cpp \
ui/MainWindow.cpp \
Controller.cpp \
ui/dialog/LoadSetupDialog.cpp \
ui/debug/SensorDataWidget.cpp \
ui/debug/plot/PlottWidget.cpp \
ui/debug/PlotTurns.cpp \
ui/debug/PlotWiFiScan.cpp \
sensors/android/WiFiSensorAndroid.cpp \
sensors/linux/WiFiSensorLinuxC.c \
ui/debug/InfoWidget.cpp \
ui/map/3D/MapView3D.cpp \
ui/map/2D/MapView2D.cpp \
tools/calibration/WiFiCalibrationScanDialog.cpp \
ui/debug/PlotGPS.cpp \
nav/NavController.cpp \
nav/mesh/NavControllerMesh.cpp \
nav/grid/NavControllerGrid.cpp \
sensors/android/BLESensorAndroid.cpp
RESOURCES += qml.qrc
@@ -102,113 +104,116 @@ INSTALLS += target
# android-sources/src/WiFi.java
HEADERS += \
lib/gpc/gpc.h \
sensors/linux/WiFiSensorLinux.h \
sensors/android/WiFiSensorAndroid.h \
sensors/StepSensor.h \
sensors/TurnSensor.h \
sensors/AccelerometerSensor.h \
sensors/GyroscopeSensor.h \
sensors/BarometerSensor.h \
sensors/android/AccelerometerSensorAndroid.h \
sensors/android/GyroscopeSensorAndroid.h \
sensors/android/BarometerSensorAndroid.h \
sensors/dummy/AccelerometerSensorDummy.h \
sensors/dummy/GyroscopeSensorDummy.h \
sensors/dummy/BarometerSensorDummy.h \
sensors/Sensor.h \
sensors/SensorFactory.h \
sensors/WiFiSensor.h \
misc/Debug.h \
misc/fixc11.h \
sensors/dummy/WiFiSensorDummy.h \
lib/gpc/Polygon.h \
ui/map/3D/FloorRenderer.h \
ui/map/3D/gl/GL.h \
ui/map/3D/gl/GLHelper.h \
ui/map/3D/gl/GLLines.h \
ui/map/3D/gl/GLTriangles.h \
ui/map/3D/elements/Doors.h \
ui/map/3D/elements/Ground.h \
ui/map/3D/elements/Handrails.h \
ui/map/3D/elements/Path.h \
ui/map/3D/elements/Stairs.h \
ui/map/3D/elements/Walls.h \
ui/map/3D/gl/GLPoints.h \
ui/map/3D/elements/ColorPoints.h \
ui/map/3D/RenderParams.h \
ui/map/3D/Renderable.h \
ui/map/3D/gl/Shader.h \
ui/map/3D/elements/Object.h \
ui/Icons.h \
ui/MainWindow.h \
Controller.h \
ui/menu/MainMenu.h \
ui/dialog/LoadSetupDialog.h \
ui/debug/plot/Axes.h \
ui/debug/plot/Plot.h \
ui/debug/plot/Data.h \
ui/debug/plot/Range.h \
nav/NavController.h \
sensors/dummy/RandomSensor.h \
ui/debug/SensorDataWidget.h \
ui/debug/plot/PlottWidget.h \
ui/debug/PlotTurns.h \
ui/debug/PlotWiFiScan.h \
sensors/linux/WiFiSensorLinuxC.h \
sensors/offline/SensorFactoryOffline.h \
sensors/dummy/SensorFactoryDummy.h \
sensors/android/SensorFactoryAndroid.h \
Settings.h \
sensors/offline/AllInOneSensor.h \
sensors/ActivitySensor.h \
ui/LoggerUI.h \
ui/debug/InfoWidget.h \
ui/UIHelper.h \
ui/map/3D/MapView3D.h \
ui/map/2D/MapView2D.h \
ui/map/2D/Floor2D.h \
ui/map/2D/Scaler2D.h \
ui/map/2D/Renderable2D.h \
ui/map/2D/RenderParams2D.h \
ui/map/2D/ColorPoints2D.h \
ui/map/2D/Path2D.h \
ui/map/2D/WiFiCalibTool.h \
ui/map/2D/HasSelectableNodes.h \
tools/calibration/WiFiCalibrationDataModel.h \
tools/calibration/WiFiCalibrationScanDialog.h \
tests/RuntimeTests.h \
ipin/StepLogger.h \
ipin/Scaler.h \
ipin/Config.h \
ipin/IPINHelper.h \
nav/NavControllerListener.h \
ipin/StepLoggerWrapper.h \
ipin/StepLoggerWrapperAndroid.h \
sensors/GPSSensor.h \
sensors/android/GPSSensorAndroid.h \
sensors/CompassSensor.h \
sensors/android/CompassSensorAndroid.h \
sensors/dummy/CompassSensorDummy.h \
sensors/dummy/GPSSensorDummy.h \
ui/debug/PlotGPS.h \
sensors/SensorWriter.h \
nav/mesh/State.h \
nav/mesh/NavControllerMesh.h \
nav/mesh/FilterMesh.h \
nav/grid/Filter.h \
nav/grid/NavControllerGrid.h \
nav/grid/Node.h \
nav/grid/NodeResampling.h \
nav/grid/RegionalResampling.h \
nav/Observation.h \
nav/grid/State.h \
nav/CurEst.h
lib/gpc/gpc.h \
sensors/linux/WiFiSensorLinux.h \
sensors/android/WiFiSensorAndroid.h \
sensors/StepSensor.h \
sensors/TurnSensor.h \
sensors/AccelerometerSensor.h \
sensors/GyroscopeSensor.h \
sensors/BarometerSensor.h \
sensors/android/AccelerometerSensorAndroid.h \
sensors/android/GyroscopeSensorAndroid.h \
sensors/android/BarometerSensorAndroid.h \
sensors/android/BLESensorAndroid.h \
sensors/dummy/AccelerometerSensorDummy.h \
sensors/dummy/GyroscopeSensorDummy.h \
sensors/dummy/BarometerSensorDummy.h \
sensors/Sensor.h \
sensors/SensorFactory.h \
sensors/WiFiSensor.h \
misc/Debug.h \
misc/fixc11.h \
sensors/dummy/WiFiSensorDummy.h \
lib/gpc/Polygon.h \
ui/map/3D/FloorRenderer.h \
ui/map/3D/gl/GL.h \
ui/map/3D/gl/GLHelper.h \
ui/map/3D/gl/GLLines.h \
ui/map/3D/gl/GLTriangles.h \
ui/map/3D/elements/Doors.h \
ui/map/3D/elements/Ground.h \
ui/map/3D/elements/Handrails.h \
ui/map/3D/elements/Path.h \
ui/map/3D/elements/Stairs.h \
ui/map/3D/elements/Walls.h \
ui/map/3D/gl/GLPoints.h \
ui/map/3D/elements/ColorPoints.h \
ui/map/3D/RenderParams.h \
ui/map/3D/Renderable.h \
ui/map/3D/gl/Shader.h \
ui/map/3D/elements/Object.h \
ui/Icons.h \
ui/MainWindow.h \
Controller.h \
ui/menu/MainMenu.h \
ui/dialog/LoadSetupDialog.h \
ui/debug/plot/Axes.h \
ui/debug/plot/Plot.h \
ui/debug/plot/Data.h \
ui/debug/plot/Range.h \
nav/NavController.h \
sensors/dummy/RandomSensor.h \
ui/debug/SensorDataWidget.h \
ui/debug/plot/PlottWidget.h \
ui/debug/PlotTurns.h \
ui/debug/PlotWiFiScan.h \
sensors/linux/WiFiSensorLinuxC.h \
sensors/offline/SensorFactoryOffline.h \
sensors/dummy/SensorFactoryDummy.h \
sensors/android/SensorFactoryAndroid.h \
Settings.h \
sensors/offline/AllInOneSensor.h \
sensors/ActivitySensor.h \
ui/LoggerUI.h \
ui/debug/InfoWidget.h \
ui/UIHelper.h \
ui/map/3D/MapView3D.h \
ui/map/2D/MapView2D.h \
ui/map/2D/Floor2D.h \
ui/map/2D/Scaler2D.h \
ui/map/2D/Renderable2D.h \
ui/map/2D/RenderParams2D.h \
ui/map/2D/ColorPoints2D.h \
ui/map/2D/Path2D.h \
ui/map/2D/WiFiCalibTool.h \
ui/map/2D/HasSelectableNodes.h \
tools/calibration/WiFiCalibrationDataModel.h \
tools/calibration/WiFiCalibrationScanDialog.h \
tests/RuntimeTests.h \
ipin/StepLogger.h \
ipin/Scaler.h \
ipin/Config.h \
ipin/IPINHelper.h \
nav/NavControllerListener.h \
ipin/StepLoggerWrapper.h \
ipin/StepLoggerWrapperAndroid.h \
sensors/GPSSensor.h \
sensors/android/GPSSensorAndroid.h \
sensors/CompassSensor.h \
sensors/android/CompassSensorAndroid.h \
sensors/dummy/CompassSensorDummy.h \
sensors/dummy/GPSSensorDummy.h \
ui/debug/PlotGPS.h \
sensors/SensorWriter.h \
nav/mesh/State.h \
nav/mesh/NavControllerMesh.h \
nav/mesh/FilterMesh.h \
nav/grid/Filter.h \
nav/grid/NavControllerGrid.h \
nav/grid/Node.h \
nav/grid/NodeResampling.h \
nav/grid/RegionalResampling.h \
nav/Observation.h \
nav/grid/State.h \
nav/CurEst.h \
sensors/BLESensor.h \
tools/calibration/BLECalibrationDataModel.h
DISTFILES += \
android-sources/src/MyActivity.java \
res/gl/vertex1.glsl \
res/gl/fragment1.glsl \
res/gl/tex/empty_normals.jpg \
_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \
_android/src/StepLoggerClient.java
android-sources/src/MyActivity.java \
res/gl/vertex1.glsl \
res/gl/fragment1.glsl \
res/gl/tex/empty_normals.jpg \
_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \
_android/src/StepLoggerClient.java