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

fixed some minor bugs using ble on android

Browse Source 
added BLE to the evaluation of the particle filter
made first evaluation inside fhws
This commit is contained in:
mail@toni-fetzer.de
2019-06-10 16:58:24 +02:00
parent ef6066ae14
commit 7fe0f22a6e
9 changed files with 311 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
Controller.cpp
View File

@@ -250,10 +250,52 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi
BeaconModel* buildBLEModelOnce(Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& bleModelFile){ BeaconModel* buildBLEModelOnce(Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& bleModelFile){
// ask questions // ask questions
const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No); const QMessageBox::StandardButton replyBLEFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No);
BeaconModel* bleModel = new BeaconModelLogDistCeiling(map); BeaconModel* bleModel = new BeaconModelLogDistCeiling(map);
// WiFi setup
if (replyBLEFP == QMessageBox::Yes) {
std::ifstream inp(bleModelFile, std::ifstream::binary);
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Log::add("Controller", "Create new BLEModel");
BLECalibrationDataModel mdl(fpFile);
Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const BeaconFingerprint& fp : mdl.getFingerprints()) {
opt.addFingerprint(fp); //we added a function to add bluetooth fingerprints to the wifi optimizer
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.MIN_10_FPS);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const BeaconModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
((BeaconModelLogDistCeiling*) bleModel)->addBeacon(ap.mac, entry);
}
((BeaconModelLogDistCeiling*) bleModel)->saveXML(bleModelFile);
} else {
Log::add("Controller", "Use existing BLEModel");
XMLDoc doc;
XMLserialize::assertOK(doc.LoadFile(bleModelFile.c_str()), doc, "error while loading file");
XMLElem* root = doc.FirstChildElement("root");
//if(bleModel){delete bleModel;}
bleModel->readFromXML(&doc, root);
}
} else {
Log::add("Controller", "Read Beacons from File");
// load all APs from the floorplan and use same TXP/EXP/WAF for all of them
((BeaconModelLogDistCeiling*)bleModel)->loadBeaconsFromMap(map, Settings::BLEModel::TXP, Settings::BLEModel::EXP, Settings::BLEModel::WAF);
Assert::isFalse(bleModel->getAllBeacons().empty(), "no AccessPoints stored within the map.xml");
}
return bleModel; return bleModel;
} }
@@ -340,6 +382,7 @@ void Controller::loadNavMesh(QDir dir) {
QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
QFile fWiFiModel(dir.path() + "/wifimodel.dat"); QFile fWiFiModel(dir.path() + "/wifimodel.dat");
QFile fBLEFP(dir.path() + "/ble_fp.dat"); QFile fBLEFP(dir.path() + "/ble_fp.dat");
QFile fBLEModel(dir.path() + "/blemodel.dat");
Assert::isTrue(fMap.exists(), "map.xml missing"); Assert::isTrue(fMap.exists(), "map.xml missing");
@@ -347,12 +390,16 @@ void Controller::loadNavMesh(QDir dir) {
QString str = QString(fMap.readAll()); QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString()); im = Floorplan::Reader::readFromString(str.toStdString());
//build the wifi model
const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); const std::string sWiFiFP = fWiFiFP.fileName().toStdString();
const std::string sWiFiModel = fWiFiModel.fileName().toStdString(); const std::string sWiFiModel = fWiFiModel.fileName().toStdString();
const std::string sBLEFP = fBLEFP.fileName().toStdString();
wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel); wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel);
//build the ble model
const std::string sBLEFP = fBLEFP.fileName().toStdString();
const std::string sBLEModel = fBLEModel.fileName().toStdString();
bleModel = buildBLEModelOnce(im, sBLEFP, sBLEModel);
// create navmesh // create navmesh
if (navMesh) {delete navMesh; navMesh = nullptr;} if (navMesh) {delete navMesh; navMesh = nullptr;}
NM::NavMeshSettings settings; NM::NavMeshSettings settings;
@@ -362,7 +409,7 @@ void Controller::loadNavMesh(QDir dir) {
// create a new navigator // create a new navigator
if (nav) {delete nav; nav = nullptr;} if (nav) {delete nav; nav = nullptr;}
nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel); nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel, bleModel);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);
BLECalibrationDataModel* bleCalib = new BLECalibrationDataModel(sBLEFP); BLECalibrationDataModel* bleCalib = new BLECalibrationDataModel(sBLEFP);

48
Controller.h
View File

@@ -22,32 +22,33 @@ class InfoWidget;
class MapView3D; class MapView3D;
class MapView2D; class MapView2D;
class WiFiModel; class WiFiModel;
class BeaconModel;
namespace NM { namespace NM {
template <typename T> class NavMesh; template <typename T> class NavMesh;
class NavMeshTriangle; class NavMeshTriangle;
} }
namespace Floorplan { namespace Floorplan {
class IndoorMap; class IndoorMap;
} }
class Controller : public QObject { class Controller : public QObject {
Q_OBJECT Q_OBJECT
public: public:
/** ctor */ /** ctor */
explicit Controller(); explicit Controller();
public: public:
MapView3D* getMapView3D() const; MapView3D* getMapView3D() const;
MapView2D* getMapView2D() const; MapView2D* getMapView2D() const;
MainMenu* getMainMenu() const; MainMenu* getMainMenu() const;
InfoWidget* getInfoWidget() const; InfoWidget* getInfoWidget() const;
protected: protected:
@@ -60,26 +61,27 @@ protected:
private slots: private slots:
void onLoadButton(); void onLoadButton();
void onDebugButton(); void onDebugButton();
void onStartButton(); void onStartButton();
void onTransparentButton(); void onTransparentButton();
void onCameraButton(); void onCameraButton();
void on3DButton(); void on3DButton();
private: private:
void loadGrid(QDir dir); void loadGrid(QDir dir);
void loadNavMesh(QDir dir); void loadNavMesh(QDir dir);
MainWindow* mainWindow; MainWindow* mainWindow;
Grid<MyGridNode>* grid = nullptr; Grid<MyGridNode>* grid = nullptr;
NM::NavMesh<NM::NavMeshTriangle>* navMesh = nullptr; NM::NavMesh<NM::NavMeshTriangle>* navMesh = nullptr;
NavController* nav = nullptr; NavController* nav = nullptr;
Floorplan::IndoorMap* im = nullptr; Floorplan::IndoorMap* im = nullptr;
WiFiModel* wifiModel = nullptr; WiFiModel* wifiModel = nullptr;
BeaconModel* bleModel = nullptr;
}; };

154
Settings.h
View File

@@ -4,6 +4,7 @@
#include <Indoor/grid/GridPoint.h> #include <Indoor/grid/GridPoint.h>
#include <Indoor/data/Timestamp.h> #include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/VAPGrouper.h> #include <Indoor/sensors/radio/VAPGrouper.h>
#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
#include <QFile> #include <QFile>
#include <QDir> #include <QDir>
@@ -12,114 +13,123 @@ namespace Settings {
const int numParticles = 5000; const int numParticles = 5000;
namespace IMU { namespace IMU {
const float turnSigma = 1.5; // 3.5 const float turnSigma = 1.5; // 3.5
const float stepLength = 0.80; const float stepLength = 0.80;
const float stepSigma = 0.40; //toni changed const float stepSigma = 0.40; //toni changed
} }
const float smartphoneAboveGround = 1.3; const float smartphoneAboveGround = 1.3;
const float offlineSensorSpeedup = 2; const float offlineSensorSpeedup = 2;
namespace Grid { namespace Grid {
constexpr int gridSize_cm = 20; constexpr int gridSize_cm = 20;
} }
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination const GridPoint destination = GridPoint(0,0,0); // do not use destination
namespace SensorDebug { namespace SensorDebug {
const Timestamp updateEvery = Timestamp::fromMS(200); const Timestamp updateEvery = Timestamp::fromMS(200);
} }
namespace WiFiModel { namespace WiFiModel {
constexpr float sigma = 10.0; //TODO: im Museum hatten wir 8.0 constexpr float sigma = 10.0; //TODO: im Museum hatten wir 8.0
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -45; constexpr float TXP = -45;
constexpr float EXP = 2.3; constexpr float EXP = 2.3;
constexpr float WAF = -5.0; constexpr float WAF = -5.0;
// how to perform VAP grouping. see // how to perform VAP grouping. see
// - calibration in Controller.cpp // - calibration in Controller.cpp
// - eval in Filter.h // - eval in Filter.h
// NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE); // const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
// const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE); // const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
} }
namespace BLEModel {
constexpr float sigma = 8.0;
namespace MapView3D { constexpr float TXP = -59;
const int maxColorPoints = 10000; constexpr float EXP = 2.3;
constexpr int fps = 15; constexpr float WAF = -9.0;
const Timestamp msPerFrame = Timestamp::fromMS(1000/fps);
}
namespace Filter { const BeaconMeasurementGrouper vg_eval = BeaconMeasurementGrouper(BeaconMeasurementGrouper::Mode::DISABLED, BeaconMeasurementGrouper::Aggregation::AVERAGE);
const Timestamp updateEvery = Timestamp::fromMS(500); }
constexpr bool useMainThread = false; // perform filtering in the main thread
}
namespace Data { namespace MapView3D {
const int maxColorPoints = 10000;
constexpr int fps = 15;
const Timestamp msPerFrame = Timestamp::fromMS(1000/fps);
}
// notes namespace Filter {
// copy: scp -P 2222 /tmp/grid.dat kazu@192.168.24.11:/storage/sdcard1/YASMIN/maps/car/ const Timestamp updateEvery = Timestamp::fromMS(500);
// all: scp -P 2222 -r /apps/android/workspace/YASMIN_DATA/* kazu@192.168.24.11:/storage/sdcard1/YASMIN/ constexpr bool useMainThread = false; // perform filtering in the main thread
}
static inline std::string getRoot() { namespace Data {
#ifdef ANDROID
// const std::string folder = getenv("EXTERNAL_STORAGE") + std::string("/YASMIN/maps/"); // this is NOT the sdcard?! // notes
// qDebug(folder.c_str()); // copy: scp -P 2222 /tmp/grid.dat kazu@192.168.24.11:/storage/sdcard1/YASMIN/maps/car/
// return folder; // all: scp -P 2222 -r /apps/android/workspace/YASMIN_DATA/* kazu@192.168.24.11:/storage/sdcard1/YASMIN/
//return "/storage/sdcard1/YASMIN/";
//const std::string f1 = "/storage/sdcard1/YASMIN/"; static inline std::string getRoot() {
//const std::string f2 = "/sdcard/YASMIN/"; #ifdef ANDROID
//if (QFile(f1.c_str()).exists()) {return f1;} // const std::string folder = getenv("EXTERNAL_STORAGE") + std::string("/YASMIN/maps/"); // this is NOT the sdcard?!
//if (QFile(f2.c_str()).exists()) {return f2;} // qDebug(folder.c_str());
std::string tried; // return folder;
QDir dStorage("/storage"); //return "/storage/sdcard1/YASMIN/";
QStringList lst = dStorage.entryList(); //const std::string f1 = "/storage/sdcard1/YASMIN/";
//const std::string f2 = "/sdcard/YASMIN/";
//if (QFile(f1.c_str()).exists()) {return f1;}
//if (QFile(f2.c_str()).exists()) {return f2;}
std::string tried;
QDir dStorage("/storage");
QStringList lst = dStorage.entryList();
lst.append("sdcard/YASMIN/"); lst.append("sdcard/YASMIN/");
lst.append("emulated/0"); lst.append("emulated/0");
//TODO: THIS IS A MESS, PURE CHAOS xD //TODO: THIS IS A MESS, PURE CHAOS xD
// try each potential folder // try each potential folder
for (const QString subfolder : lst) { for (const QString subfolder : lst) {
QString path = dStorage.path() + "/" + subfolder + "/YASMIN/"; QString path = dStorage.path() + "/" + subfolder + "/YASMIN/";
if (QFile(path).exists()) {return path.toStdString();} if (QFile(path).exists()) {return path.toStdString();}
tried += path.toStdString() + "\n"; tried += path.toStdString() + "\n";
} }
throw Exception("data folder missing. tried:\n" + tried); throw Exception("data folder missing. tried:\n" + tried);
#else #else
//return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/"; //return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/";
return "/apps/android/workspace/YASMIN_DATA/"; return "/apps/android/workspace/YASMIN_DATA/";
#endif #endif
} }
/** get the directory where maps are stored */ /** get the directory where maps are stored */
static inline std::string getMapDir() { static inline std::string getMapDir() {
return getRoot() + "maps/"; return getRoot() + "maps/";
} }
static inline std::string getOfflineDir() { static inline std::string getOfflineDir() {
return getRoot() + "offline/"; return getRoot() + "offline/";
} }
static inline std::string getRecordsDir() { static inline std::string getRecordsDir() {
return getRoot() + "records/"; return getRoot() + "records/";
} }
} }
} }

11
_android/src/BLE.java
View File

@@ -29,6 +29,8 @@ public class BLE {
/** called when a scan is completed successfully */ /** called when a scan is completed successfully */
public static native void onScanComplete(final byte[] result); public static native void onScanComplete(final byte[] result);
public static int dataCounter = 0;
/** /**
* start bluetooth scanning in the background. * start bluetooth scanning in the background.
* will call onScanComplete() once a scan is available * will call onScanComplete() once a scan is available
@@ -52,7 +54,7 @@ public class BLE {
final BluetoothManager mgr = (BluetoothManager) act.getSystemService(Context.BLUETOOTH_SERVICE); final BluetoothManager mgr = (BluetoothManager) act.getSystemService(Context.BLUETOOTH_SERVICE);
bt = mgr.getAdapter(); bt = mgr.getAdapter();
Log.d("ble", "BLE Mangaer Start"); Log.d("ble", "BLE Manager Start");
// create the scanner // create the scanner
scanner = bt.getBluetoothLeScanner(); scanner = bt.getBluetoothLeScanner();
@@ -69,7 +71,12 @@ public class BLE {
// and attach the callback // and attach the callback
mLeScanCallback = new ScanCallback() { mLeScanCallback = new ScanCallback() {
@Override public void onScanResult(int callbackType, android.bluetooth.le.ScanResult result) { @Override public void onScanResult(int callbackType, android.bluetooth.le.ScanResult result) {
//Log.d("ble", result.getDevice() + " " + result.getRssi());
if(++dataCounter > 50){
Log.d("ble", result.getDevice() + " " + result.getRssi());
dataCounter = 0;
}
final byte[] rst = serialize(result); final byte[] rst = serialize(result);
BLE.onScanComplete(rst); BLE.onScanComplete(rst);

2
_android/src/WiFi.java
View File

@@ -122,7 +122,7 @@ public class WiFi {
/** try to trigger one scan process */ /** try to trigger one scan process */
private static void triggerOneScan() { private static void triggerOneScan() {
//Log.d("wifi", "triggerOneScan()"); Log.d("wifi", "triggerOneScan()");
try { try {
if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");} if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");}

1
nav/NavController.h
View File

@@ -5,6 +5,7 @@
#include "../sensors/GyroscopeSensor.h" #include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h" #include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h" #include "../sensors/WiFiSensor.h"
#include "../sensors/BLESensor.h"
#include "../sensors/SensorFactory.h" #include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h" #include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h" #include "../sensors/TurnSensor.h"

258
nav/mesh/FilterMesh.h
View File

@@ -14,11 +14,14 @@
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h> #include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h> #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h> #include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
#include <Indoor/navMesh/NavMesh.h> #include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h> #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h> #include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
@@ -32,185 +35,192 @@
namespace MeshBased { namespace MeshBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> { class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private: private:
const NM::NavMesh<NM::NavMeshTriangle>* mesh; const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public: public:
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) { PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
; ;
} }
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override { virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI); std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom(); NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (SMC::Particle<MyState>& p : particles) { for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw(); p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight p.weight = 1.0 / particles.size(); // equal weight
} }
// // fix position + heading // // fix position + heading
// for (SMC::Particle<MyState>& p : particles) { // for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000; //// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx]; //// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
// p.state.position = node; // p.state.position = node;
// p.state.heading.direction = Heading(0); // p.state.heading.direction = Heading(0);
// } // }
} }
}; };
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public: public:
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>; using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker; MyNavMeshWalk walker;
// local, static control-data COPY // local, static control-data COPY
MyControl ctrl; MyControl ctrl;
public: public:
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){ PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
// how to evaluate drawn points // how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04)); walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1)); walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
} }
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl; this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition(); ((MyControl*)_ctrl)->resetAfterTransition();
// walking and heading random // walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1); Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1); Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1); Distribution::Normal<float> dHeading(0.0, 0.1);
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
// how to walk // how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params; NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw(); params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition; params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.pos; params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){ if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1; params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1; params.stepSizes.stepSizeStair_m = 0.1;
} }
// walk // walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params); MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state // assign back to particle's state
p.weight *= res.probability; p.weight *= res.probability;
p.state.pos = res.location; p.state.pos = res.location;
p.state.heading = res.heading; p.state.heading = res.heading;
} }
} }
}; };
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> { class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel; WiFiModel& wifiModel;
WiFiObserverFree wifiProbability; WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) { BeaconModel& bleModel;
BeaconObserverFree bleProbability;
const float kappa = 0.9; double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
switch (act) { const float kappa = 0.9;
case Activity::WALKING: switch (act) {
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:
case Activity::WALKING_DOWN: if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {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;} 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-kappa;}
}
return 1.0; 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;}
}
public: return 1.0;
}
//TODO: Was ist hier besser? Im Museum hatten wir das unterste. public:
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 { //TODO: Was ist hier besser? Im Museum hatten wir das unterste.
PFEval(WiFiModel* wifiModel, BeaconModel* bleModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel), bleModel(*bleModel), bleProbability(Settings::BLEModel::sigma, *bleModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
double sum = 0; double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) double sum = 0;
const MyObservation observation = _observation;
// vap-grouping // local copy!! observation might be changed async outside!! (will really produces crashes!)
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); const MyObservation observation = _observation;
// sanity check // vap-grouping
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
// assign weights //TODO: do i need a vap grouping for beacons?
#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.pos.pos, observation.currentTime, wifiObs); // sanity check
const double pStair = getStairProb(p, observation.activity); //Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
const double pGPS = 1;
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind. // 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 prob = pWifi * pStair * pGPS; const double pBLE = bleProbability.getProbability(p.state.pos.pos, observation.currentTime, beaconObs);
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
p.weight *= prob; //TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic const double prob = pBLE * pStair * pGPS * pWifi;
sum += p.weight;
}
return sum; p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
} #pragma omp atomic
sum += p.weight;
}
}; return sum;
}
};
} }

8
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,11 +31,13 @@
#include "Settings.h" #include "Settings.h"
#include "../Controller.h"
Q_DECLARE_METATYPE(const void*) Q_DECLARE_METATYPE(const void*)
/** ctor */ /** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) : MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) { NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel), bleModel(bleModel) {
// filter init // filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh)); std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
@@ -54,7 +56,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>()); std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition // eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel)); std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel, bleModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh)); std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// setup the filter // setup the filter

5
nav/mesh/NavControllerMesh.h
View File

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