started to add ble functions
added ble as sensor to java and c++ added sensorlistener for ble added ble to observation and onDataSensor in filter started to work on ble fingerprints for optimization
This commit is contained in:
mail@toni-fetzer.de
285
Controller.cpp
285
Controller.cpp
@@ -44,71 +44,72 @@
|
||||
|
||||
Controller::Controller() {
|
||||
|
||||
// OpenGL setup
|
||||
// MUST happen before anything gets visible (= gets initialized)
|
||||
QSurfaceFormat format;
|
||||
format.setDepthBufferSize(16);
|
||||
QSurfaceFormat::setDefaultFormat(format);
|
||||
// OpenGL setup
|
||||
// MUST happen before anything gets visible (= gets initialized)
|
||||
QSurfaceFormat format;
|
||||
format.setDepthBufferSize(16);
|
||||
QSurfaceFormat::setDefaultFormat(format);
|
||||
|
||||
|
||||
|
||||
// configure the to-be-used sensor factory
|
||||
//SensorFactory::set(new SensorFactoryDummy());
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/nexus6/kleinerKreis_216steps_6runden_telefongerade.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s3mini/kleinerKreis_225steps_6runden_telefongerade.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s4/kleinerKreis_220steps_6runden_telefongeneigt.csv"));
|
||||
// configure the to-be-used sensor factory
|
||||
//SensorFactory::set(new SensorFactoryDummy());
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/nexus6/kleinerKreis_216steps_6runden_telefongerade.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s3mini/kleinerKreis_225steps_6runden_telefongerade.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline("/apps/android/workspace/YASMIN_DATA/offline/gyroacctestingfrank/s4/kleinerKreis_220steps_6runden_telefongeneigt.csv"));
|
||||
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "bergwerk/path3/nexus/vor/1454782562231.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/rueck/1454776724285_rueck.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "bergwerk/path3/nexus/vor/1454782562231.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/rueck/1454776724285_rueck.csv"));
|
||||
//SensorFactory::set(new SensorFactoryOffline(Settings::Data::getOfflineDir() + "/bergwerk/path4/nexus/vor/1454776525797.csv"));
|
||||
|
||||
// live data on the smartphone
|
||||
if (1 == 1) {
|
||||
// live data on the smartphone
|
||||
if (1 == 1) {
|
||||
|
||||
// use android's sensors
|
||||
SensorFactory::set(new SensorFactoryAndroid());
|
||||
// use android's sensors
|
||||
SensorFactory::set(new SensorFactoryAndroid());
|
||||
|
||||
// write them to file??
|
||||
//const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv";
|
||||
//SensorWriter* writer = new SensorWriter();
|
||||
//writer->start(file);
|
||||
// write them to file??
|
||||
//const std::string file = Settings::Data::getRecordsDir() + "/" + std::to_string(Timestamp::fromUnixTime().ms()) + ".csv";
|
||||
//SensorWriter* writer = new SensorWriter();
|
||||
//writer->start(file);
|
||||
|
||||
// start the sensors
|
||||
// start the sensors
|
||||
SensorFactory::get().getAccelerometer().start();
|
||||
SensorFactory::get().getGyroscope().start();
|
||||
SensorFactory::get().getBarometer().start();
|
||||
SensorFactory::get().getWiFi().start();
|
||||
SensorFactory::get().getGPS().start();
|
||||
SensorFactory::get().getCompass().start();
|
||||
SensorFactory::get().getBLE().start();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// create the main window
|
||||
mainWindow = new MainWindow();
|
||||
//sl = new StepLoggerWrapperAndroid(scaler);
|
||||
//sl = new StepLoggerWrapper(scaler, mainWindow);
|
||||
// create the main window
|
||||
mainWindow = new MainWindow();
|
||||
//sl = new StepLoggerWrapperAndroid(scaler);
|
||||
//sl = new StepLoggerWrapper(scaler, mainWindow);
|
||||
|
||||
|
||||
// attach logger
|
||||
LoggerComposite* log = new LoggerComposite();
|
||||
log->addLogger(new LoggerCOUT());
|
||||
log->addLogger(new LoggerAndroid());
|
||||
log->addLogger(new LoggerUI(mainWindow->getInfoWidget()));
|
||||
Log::setLogger(log);
|
||||
// attach logger
|
||||
LoggerComposite* log = new LoggerComposite();
|
||||
log->addLogger(new LoggerCOUT());
|
||||
log->addLogger(new LoggerAndroid());
|
||||
log->addLogger(new LoggerUI(mainWindow->getInfoWidget()));
|
||||
Log::setLogger(log);
|
||||
|
||||
|
||||
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onLoadButton, this, &Controller::onLoadButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onDebugButton, this, &Controller::onDebugButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onStartButton, this, &Controller::onStartButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onTransparentButton, this, &Controller::onTransparentButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onCameraButton, this, &Controller::onCameraButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::on3DButton, this, &Controller::on3DButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onLoadButton, this, &Controller::onLoadButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onDebugButton, this, &Controller::onDebugButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onStartButton, this, &Controller::onStartButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onTransparentButton, this, &Controller::onTransparentButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::onCameraButton, this, &Controller::onCameraButton), "connect() failed");
|
||||
Assert::isTrue(connect(mainWindow->getMainMenu(), &MainMenu::on3DButton, this, &Controller::on3DButton), "connect() failed");
|
||||
|
||||
// order is important! otherwise OpenGL fails!
|
||||
mainWindow->show();
|
||||
// order is important! otherwise OpenGL fails!
|
||||
mainWindow->show();
|
||||
|
||||
// // start all sensors
|
||||
// SensorFactory::get().getAccelerometer().start();
|
||||
@@ -120,73 +121,73 @@ Controller::Controller() {
|
||||
}
|
||||
|
||||
MapView3D* Controller::getMapView3D() const {
|
||||
return mainWindow->getMapView3D();
|
||||
return mainWindow->getMapView3D();
|
||||
}
|
||||
|
||||
MapView2D* Controller::getMapView2D() const {
|
||||
return mainWindow->getMapView2D();
|
||||
return mainWindow->getMapView2D();
|
||||
}
|
||||
|
||||
|
||||
MainMenu* Controller::getMainMenu() const {
|
||||
return mainWindow->getMainMenu();
|
||||
return mainWindow->getMainMenu();
|
||||
}
|
||||
|
||||
InfoWidget* Controller::getInfoWidget() const {
|
||||
return mainWindow->getInfoWidget();
|
||||
return mainWindow->getInfoWidget();
|
||||
}
|
||||
|
||||
#include <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();
|
||||
}
|
||||
|
||||
@@ -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
111
_android/src/BLE.java
Normal 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();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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";
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -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,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
18
sensors/android/BLESensorAndroid.cpp
Normal 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
|
||||
@@ -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
4
tools/calibration/BLECalibrationDataModel.h
Normal file
@@ -0,0 +1,4 @@
|
||||
#ifndef BLECALIBRATIONDATAMODEL_H
|
||||
#define BLECALIBRATIONDATAMODEL_H
|
||||
|
||||
#endif // BLECALIBRATIONDATAMODEL_H
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -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:
|
||||
|
||||
|
||||
@@ -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
309
yasmin.pro
@@ -7,27 +7,27 @@ QT += qml opengl svg
|
||||
|
||||
CONFIG+=ANDROID DEFINES+=ANDROID
|
||||
ANDROID {
|
||||
QT += androidextras
|
||||
QT += sensors
|
||||
QT += positioning
|
||||
QT += androidextras
|
||||
QT += sensors
|
||||
QT += positioning
|
||||
|
||||
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
|
||||
# wasn't faster... and seems dangerous :P
|
||||
#QMAKE_CXXFLAGS -= -mfpu=vfp
|
||||
#QMAKE_CFLAGS -= -mfpu=vfp
|
||||
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
|
||||
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
|
||||
#http://stackoverflow.com/questions/28391685/opencv-with-hard-float-support-for-android#28393545
|
||||
# wasn't faster... and seems dangerous :P
|
||||
#QMAKE_CXXFLAGS -= -mfpu=vfp
|
||||
#QMAKE_CFLAGS -= -mfpu=vfp
|
||||
#QMAKE_CXXFLAGS += -mfpu=neon -funsafe-math-optimizations
|
||||
#QMAKE_CFLAGS += -mfpu=neon -funsafe-math-optimizations
|
||||
|
||||
# wasn't faster..
|
||||
#QMAKE_CXXFLAGS += -mtune=cortex-a57
|
||||
#QMAKE_CFLAGS += -mtune=cortex-a57
|
||||
# wasn't faster..
|
||||
#QMAKE_CXXFLAGS += -mtune=cortex-a57
|
||||
#QMAKE_CFLAGS += -mtune=cortex-a57
|
||||
|
||||
QMAKE_CXXFLAGS += -O2
|
||||
#QMAKE_CFLAGS += -O3
|
||||
QMAKE_CXXFLAGS += -O2
|
||||
#QMAKE_CFLAGS += -O3
|
||||
|
||||
#QMAKE_CXXFLAGS_DEBUG -= -O2
|
||||
#QMAKE_CXXFLAGS_DEBUG -= -O3
|
||||
#QMAKE_CXXFLAGS_DEBUG += -O0
|
||||
#QMAKE_CXXFLAGS_DEBUG -= -O2
|
||||
#QMAKE_CXXFLAGS_DEBUG -= -O3
|
||||
#QMAKE_CXXFLAGS_DEBUG += -O0
|
||||
|
||||
}
|
||||
|
||||
@@ -49,8 +49,8 @@ CONFIG += c++11
|
||||
ANDROID_PACKAGE_SOURCE_DIR = $$PWD/_android
|
||||
|
||||
INCLUDEPATH += \
|
||||
../ \
|
||||
./lib/
|
||||
../ \
|
||||
./lib/
|
||||
|
||||
|
||||
# linux desktop wifi
|
||||
@@ -58,33 +58,35 @@ INCLUDEPATH += \
|
||||
#LIBS += -lnl-genl-3 -lnl-3
|
||||
|
||||
OTHER_FILES += \
|
||||
_android/src/WiFi.java \
|
||||
_android/src/MyActivity.java \
|
||||
_android/AndroidManifest.xml
|
||||
_android/src/WiFi.java \
|
||||
_android/src/MyActivity.java \
|
||||
_android/AndroidManifest.xml \
|
||||
_android/src/BLE.java
|
||||
|
||||
SOURCES += \
|
||||
main.cpp \
|
||||
lib/gpc/gpc.cpp \
|
||||
../Indoor/lib/tinyxml/tinyxml2.cpp \
|
||||
../Indoor/lib/Recast/*.cpp\
|
||||
ui/menu/MainMenu.cpp \
|
||||
ui/MainWindow.cpp \
|
||||
Controller.cpp \
|
||||
ui/dialog/LoadSetupDialog.cpp \
|
||||
ui/debug/SensorDataWidget.cpp \
|
||||
ui/debug/plot/PlottWidget.cpp \
|
||||
ui/debug/PlotTurns.cpp \
|
||||
ui/debug/PlotWiFiScan.cpp \
|
||||
sensors/android/WiFiSensorAndroid.cpp \
|
||||
sensors/linux/WiFiSensorLinuxC.c \
|
||||
ui/debug/InfoWidget.cpp \
|
||||
ui/map/3D/MapView3D.cpp \
|
||||
ui/map/2D/MapView2D.cpp \
|
||||
tools/calibration/WiFiCalibrationScanDialog.cpp \
|
||||
ui/debug/PlotGPS.cpp \
|
||||
nav/NavController.cpp \
|
||||
nav/mesh/NavControllerMesh.cpp \
|
||||
nav/grid/NavControllerGrid.cpp
|
||||
main.cpp \
|
||||
lib/gpc/gpc.cpp \
|
||||
../Indoor/lib/tinyxml/tinyxml2.cpp \
|
||||
../Indoor/lib/Recast/*.cpp\
|
||||
ui/menu/MainMenu.cpp \
|
||||
ui/MainWindow.cpp \
|
||||
Controller.cpp \
|
||||
ui/dialog/LoadSetupDialog.cpp \
|
||||
ui/debug/SensorDataWidget.cpp \
|
||||
ui/debug/plot/PlottWidget.cpp \
|
||||
ui/debug/PlotTurns.cpp \
|
||||
ui/debug/PlotWiFiScan.cpp \
|
||||
sensors/android/WiFiSensorAndroid.cpp \
|
||||
sensors/linux/WiFiSensorLinuxC.c \
|
||||
ui/debug/InfoWidget.cpp \
|
||||
ui/map/3D/MapView3D.cpp \
|
||||
ui/map/2D/MapView2D.cpp \
|
||||
tools/calibration/WiFiCalibrationScanDialog.cpp \
|
||||
ui/debug/PlotGPS.cpp \
|
||||
nav/NavController.cpp \
|
||||
nav/mesh/NavControllerMesh.cpp \
|
||||
nav/grid/NavControllerGrid.cpp \
|
||||
sensors/android/BLESensorAndroid.cpp
|
||||
|
||||
RESOURCES += qml.qrc
|
||||
|
||||
@@ -102,113 +104,116 @@ INSTALLS += target
|
||||
# android-sources/src/WiFi.java
|
||||
|
||||
HEADERS += \
|
||||
lib/gpc/gpc.h \
|
||||
sensors/linux/WiFiSensorLinux.h \
|
||||
sensors/android/WiFiSensorAndroid.h \
|
||||
sensors/StepSensor.h \
|
||||
sensors/TurnSensor.h \
|
||||
sensors/AccelerometerSensor.h \
|
||||
sensors/GyroscopeSensor.h \
|
||||
sensors/BarometerSensor.h \
|
||||
sensors/android/AccelerometerSensorAndroid.h \
|
||||
sensors/android/GyroscopeSensorAndroid.h \
|
||||
sensors/android/BarometerSensorAndroid.h \
|
||||
sensors/dummy/AccelerometerSensorDummy.h \
|
||||
sensors/dummy/GyroscopeSensorDummy.h \
|
||||
sensors/dummy/BarometerSensorDummy.h \
|
||||
sensors/Sensor.h \
|
||||
sensors/SensorFactory.h \
|
||||
sensors/WiFiSensor.h \
|
||||
misc/Debug.h \
|
||||
misc/fixc11.h \
|
||||
sensors/dummy/WiFiSensorDummy.h \
|
||||
lib/gpc/Polygon.h \
|
||||
ui/map/3D/FloorRenderer.h \
|
||||
ui/map/3D/gl/GL.h \
|
||||
ui/map/3D/gl/GLHelper.h \
|
||||
ui/map/3D/gl/GLLines.h \
|
||||
ui/map/3D/gl/GLTriangles.h \
|
||||
ui/map/3D/elements/Doors.h \
|
||||
ui/map/3D/elements/Ground.h \
|
||||
ui/map/3D/elements/Handrails.h \
|
||||
ui/map/3D/elements/Path.h \
|
||||
ui/map/3D/elements/Stairs.h \
|
||||
ui/map/3D/elements/Walls.h \
|
||||
ui/map/3D/gl/GLPoints.h \
|
||||
ui/map/3D/elements/ColorPoints.h \
|
||||
ui/map/3D/RenderParams.h \
|
||||
ui/map/3D/Renderable.h \
|
||||
ui/map/3D/gl/Shader.h \
|
||||
ui/map/3D/elements/Object.h \
|
||||
ui/Icons.h \
|
||||
ui/MainWindow.h \
|
||||
Controller.h \
|
||||
ui/menu/MainMenu.h \
|
||||
ui/dialog/LoadSetupDialog.h \
|
||||
ui/debug/plot/Axes.h \
|
||||
ui/debug/plot/Plot.h \
|
||||
ui/debug/plot/Data.h \
|
||||
ui/debug/plot/Range.h \
|
||||
nav/NavController.h \
|
||||
sensors/dummy/RandomSensor.h \
|
||||
ui/debug/SensorDataWidget.h \
|
||||
ui/debug/plot/PlottWidget.h \
|
||||
ui/debug/PlotTurns.h \
|
||||
ui/debug/PlotWiFiScan.h \
|
||||
sensors/linux/WiFiSensorLinuxC.h \
|
||||
sensors/offline/SensorFactoryOffline.h \
|
||||
sensors/dummy/SensorFactoryDummy.h \
|
||||
sensors/android/SensorFactoryAndroid.h \
|
||||
Settings.h \
|
||||
sensors/offline/AllInOneSensor.h \
|
||||
sensors/ActivitySensor.h \
|
||||
ui/LoggerUI.h \
|
||||
ui/debug/InfoWidget.h \
|
||||
ui/UIHelper.h \
|
||||
ui/map/3D/MapView3D.h \
|
||||
ui/map/2D/MapView2D.h \
|
||||
ui/map/2D/Floor2D.h \
|
||||
ui/map/2D/Scaler2D.h \
|
||||
ui/map/2D/Renderable2D.h \
|
||||
ui/map/2D/RenderParams2D.h \
|
||||
ui/map/2D/ColorPoints2D.h \
|
||||
ui/map/2D/Path2D.h \
|
||||
ui/map/2D/WiFiCalibTool.h \
|
||||
ui/map/2D/HasSelectableNodes.h \
|
||||
tools/calibration/WiFiCalibrationDataModel.h \
|
||||
tools/calibration/WiFiCalibrationScanDialog.h \
|
||||
tests/RuntimeTests.h \
|
||||
ipin/StepLogger.h \
|
||||
ipin/Scaler.h \
|
||||
ipin/Config.h \
|
||||
ipin/IPINHelper.h \
|
||||
nav/NavControllerListener.h \
|
||||
ipin/StepLoggerWrapper.h \
|
||||
ipin/StepLoggerWrapperAndroid.h \
|
||||
sensors/GPSSensor.h \
|
||||
sensors/android/GPSSensorAndroid.h \
|
||||
sensors/CompassSensor.h \
|
||||
sensors/android/CompassSensorAndroid.h \
|
||||
sensors/dummy/CompassSensorDummy.h \
|
||||
sensors/dummy/GPSSensorDummy.h \
|
||||
ui/debug/PlotGPS.h \
|
||||
sensors/SensorWriter.h \
|
||||
nav/mesh/State.h \
|
||||
nav/mesh/NavControllerMesh.h \
|
||||
nav/mesh/FilterMesh.h \
|
||||
nav/grid/Filter.h \
|
||||
nav/grid/NavControllerGrid.h \
|
||||
nav/grid/Node.h \
|
||||
nav/grid/NodeResampling.h \
|
||||
nav/grid/RegionalResampling.h \
|
||||
nav/Observation.h \
|
||||
nav/grid/State.h \
|
||||
nav/CurEst.h
|
||||
lib/gpc/gpc.h \
|
||||
sensors/linux/WiFiSensorLinux.h \
|
||||
sensors/android/WiFiSensorAndroid.h \
|
||||
sensors/StepSensor.h \
|
||||
sensors/TurnSensor.h \
|
||||
sensors/AccelerometerSensor.h \
|
||||
sensors/GyroscopeSensor.h \
|
||||
sensors/BarometerSensor.h \
|
||||
sensors/android/AccelerometerSensorAndroid.h \
|
||||
sensors/android/GyroscopeSensorAndroid.h \
|
||||
sensors/android/BarometerSensorAndroid.h \
|
||||
sensors/android/BLESensorAndroid.h \
|
||||
sensors/dummy/AccelerometerSensorDummy.h \
|
||||
sensors/dummy/GyroscopeSensorDummy.h \
|
||||
sensors/dummy/BarometerSensorDummy.h \
|
||||
sensors/Sensor.h \
|
||||
sensors/SensorFactory.h \
|
||||
sensors/WiFiSensor.h \
|
||||
misc/Debug.h \
|
||||
misc/fixc11.h \
|
||||
sensors/dummy/WiFiSensorDummy.h \
|
||||
lib/gpc/Polygon.h \
|
||||
ui/map/3D/FloorRenderer.h \
|
||||
ui/map/3D/gl/GL.h \
|
||||
ui/map/3D/gl/GLHelper.h \
|
||||
ui/map/3D/gl/GLLines.h \
|
||||
ui/map/3D/gl/GLTriangles.h \
|
||||
ui/map/3D/elements/Doors.h \
|
||||
ui/map/3D/elements/Ground.h \
|
||||
ui/map/3D/elements/Handrails.h \
|
||||
ui/map/3D/elements/Path.h \
|
||||
ui/map/3D/elements/Stairs.h \
|
||||
ui/map/3D/elements/Walls.h \
|
||||
ui/map/3D/gl/GLPoints.h \
|
||||
ui/map/3D/elements/ColorPoints.h \
|
||||
ui/map/3D/RenderParams.h \
|
||||
ui/map/3D/Renderable.h \
|
||||
ui/map/3D/gl/Shader.h \
|
||||
ui/map/3D/elements/Object.h \
|
||||
ui/Icons.h \
|
||||
ui/MainWindow.h \
|
||||
Controller.h \
|
||||
ui/menu/MainMenu.h \
|
||||
ui/dialog/LoadSetupDialog.h \
|
||||
ui/debug/plot/Axes.h \
|
||||
ui/debug/plot/Plot.h \
|
||||
ui/debug/plot/Data.h \
|
||||
ui/debug/plot/Range.h \
|
||||
nav/NavController.h \
|
||||
sensors/dummy/RandomSensor.h \
|
||||
ui/debug/SensorDataWidget.h \
|
||||
ui/debug/plot/PlottWidget.h \
|
||||
ui/debug/PlotTurns.h \
|
||||
ui/debug/PlotWiFiScan.h \
|
||||
sensors/linux/WiFiSensorLinuxC.h \
|
||||
sensors/offline/SensorFactoryOffline.h \
|
||||
sensors/dummy/SensorFactoryDummy.h \
|
||||
sensors/android/SensorFactoryAndroid.h \
|
||||
Settings.h \
|
||||
sensors/offline/AllInOneSensor.h \
|
||||
sensors/ActivitySensor.h \
|
||||
ui/LoggerUI.h \
|
||||
ui/debug/InfoWidget.h \
|
||||
ui/UIHelper.h \
|
||||
ui/map/3D/MapView3D.h \
|
||||
ui/map/2D/MapView2D.h \
|
||||
ui/map/2D/Floor2D.h \
|
||||
ui/map/2D/Scaler2D.h \
|
||||
ui/map/2D/Renderable2D.h \
|
||||
ui/map/2D/RenderParams2D.h \
|
||||
ui/map/2D/ColorPoints2D.h \
|
||||
ui/map/2D/Path2D.h \
|
||||
ui/map/2D/WiFiCalibTool.h \
|
||||
ui/map/2D/HasSelectableNodes.h \
|
||||
tools/calibration/WiFiCalibrationDataModel.h \
|
||||
tools/calibration/WiFiCalibrationScanDialog.h \
|
||||
tests/RuntimeTests.h \
|
||||
ipin/StepLogger.h \
|
||||
ipin/Scaler.h \
|
||||
ipin/Config.h \
|
||||
ipin/IPINHelper.h \
|
||||
nav/NavControllerListener.h \
|
||||
ipin/StepLoggerWrapper.h \
|
||||
ipin/StepLoggerWrapperAndroid.h \
|
||||
sensors/GPSSensor.h \
|
||||
sensors/android/GPSSensorAndroid.h \
|
||||
sensors/CompassSensor.h \
|
||||
sensors/android/CompassSensorAndroid.h \
|
||||
sensors/dummy/CompassSensorDummy.h \
|
||||
sensors/dummy/GPSSensorDummy.h \
|
||||
ui/debug/PlotGPS.h \
|
||||
sensors/SensorWriter.h \
|
||||
nav/mesh/State.h \
|
||||
nav/mesh/NavControllerMesh.h \
|
||||
nav/mesh/FilterMesh.h \
|
||||
nav/grid/Filter.h \
|
||||
nav/grid/NavControllerGrid.h \
|
||||
nav/grid/Node.h \
|
||||
nav/grid/NodeResampling.h \
|
||||
nav/grid/RegionalResampling.h \
|
||||
nav/Observation.h \
|
||||
nav/grid/State.h \
|
||||
nav/CurEst.h \
|
||||
sensors/BLESensor.h \
|
||||
tools/calibration/BLECalibrationDataModel.h
|
||||
|
||||
DISTFILES += \
|
||||
android-sources/src/MyActivity.java \
|
||||
res/gl/vertex1.glsl \
|
||||
res/gl/fragment1.glsl \
|
||||
res/gl/tex/empty_normals.jpg \
|
||||
_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \
|
||||
_android/src/StepLoggerClient.java
|
||||
android-sources/src/MyActivity.java \
|
||||
res/gl/vertex1.glsl \
|
||||
res/gl/fragment1.glsl \
|
||||
res/gl/tex/empty_normals.jpg \
|
||||
_android/src/aidl/it/cnr/isti/steplogger/IStepLoggerService.aidl \
|
||||
_android/src/StepLoggerClient.java
|
||||
|
||||
Reference in New Issue
Block a user