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){
// 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);
// 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;
}
@@ -340,6 +382,7 @@ void Controller::loadNavMesh(QDir dir) {
QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
QFile fWiFiModel(dir.path() + "/wifimodel.dat");
QFile fBLEFP(dir.path() + "/ble_fp.dat");
QFile fBLEModel(dir.path() + "/blemodel.dat");
Assert::isTrue(fMap.exists(), "map.xml missing");
@@ -347,12 +390,16 @@ void Controller::loadNavMesh(QDir dir) {
QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString());
//build the wifi model
const std::string sWiFiFP = fWiFiFP.fileName().toStdString();
const std::string sWiFiModel = fWiFiModel.fileName().toStdString();
const std::string sBLEFP = fBLEFP.fileName().toStdString();
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
if (navMesh) {delete navMesh; navMesh = nullptr;}
NM::NavMeshSettings settings;
@@ -362,7 +409,7 @@ void Controller::loadNavMesh(QDir dir) {
// create a new navigator
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);
BLECalibrationDataModel* bleCalib = new BLECalibrationDataModel(sBLEFP);

48
Controller.h
View File

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

154
Settings.h
View File

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

11
_android/src/BLE.java
View File

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

2
_android/src/WiFi.java
View File

@@ -122,7 +122,7 @@ public class WiFi {
/** try to trigger one scan process */
private static void triggerOneScan() {
//Log.d("wifi", "triggerOneScan()");
Log.d("wifi", "triggerOneScan()");
try {
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/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/BLESensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.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/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.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/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
@@ -32,185 +35,192 @@
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::uniform_real_distribution<float> distHead(0, 2*M_PI);
std::minstd_rand gen;
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) {
p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
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) {
//// 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);
// }
// // 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);
// }
}
}
};
};
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::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
MyControl ctrl;
// local, static control-data COPY
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
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();
// 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.pos;
// 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.pos = 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) {
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:
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;}
switch (act) {
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;}
}
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;}
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.
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){}
public:
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!)
const MyObservation observation = _observation;
double sum = 0;
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
//TODO: do i need a vap grouping for beacons?
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//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;
if (p.weight != p.weight) {throw Exception("nan");}
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
#pragma omp atomic
sum += p.weight;
}
const double prob = pBLE * pStair * pGPS * pWifi;
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 "../Controller.h"
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(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), bleModel(bleModel) {
// filter init
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>());
// 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));
// setup the filter

5
nav/mesh/NavControllerMesh.h
View File

@@ -12,6 +12,7 @@
#include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/Assertions.h>
#include <thread>
@@ -19,7 +20,6 @@
//#include "State.h"
#include "FilterMesh.h"
#include "../Controller.h"
#include "../NavController.h"
namespace MeshBased {
@@ -30,6 +30,7 @@ namespace MeshBased {
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel;
BeaconModel* bleModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
@@ -38,7 +39,7 @@ namespace MeshBased {
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;