From b4a1a3d9695c09bb842f22807cbee4f5bd2e7e0d Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 11 Jul 2018 19:04:42 +0200 Subject: [PATCH] refactoring to add nav mesh --- Controller.cpp | 60 ++++- Controller.h | 12 + Settings.h | 22 +- nav/CurEst.h | 16 ++ nav/Filter.h | 248 --------------------- nav/NavController.cpp | 46 ++++ nav/NavController.h | 366 ++++--------------------------- nav/{State.h => Observation.h} | 40 +--- nav/RegionalResampling.h | 68 ------ nav/grid/Filter.h | 253 +++++++++++++++++++++ nav/grid/NavControllerGrid.cpp | 266 ++++++++++++++++++++++ nav/grid/NavControllerGrid.h | 89 ++++++++ nav/{ => grid}/Node.h | 0 nav/{ => grid}/NodeResampling.h | 0 nav/grid/RegionalResampling.h | 72 ++++++ nav/grid/State.h | 44 ++++ nav/mesh/FilterMesh.h | 220 +++++++++++++++++++ nav/mesh/NavControllerMesh.cpp | 290 ++++++++++++++++++++++++ nav/mesh/NavControllerMesh.h | 88 ++++++++ nav/mesh/State.h | 45 ++++ ui/map/2D/ColorPoints2D.h | 4 +- ui/map/2D/MapView2D.cpp | 2 +- ui/map/2D/MapView2D.h | 6 +- ui/map/2D/Path2D.h | 2 +- ui/map/3D/MapView3D.h | 8 +- ui/map/3D/elements/ColorPoints.h | 2 +- yasmin.pro | 24 +- 27 files changed, 1581 insertions(+), 712 deletions(-) create mode 100644 nav/CurEst.h delete mode 100644 nav/Filter.h create mode 100644 nav/NavController.cpp rename nav/{State.h => Observation.h} (52%) delete mode 100644 nav/RegionalResampling.h create mode 100644 nav/grid/Filter.h create mode 100644 nav/grid/NavControllerGrid.cpp create mode 100644 nav/grid/NavControllerGrid.h rename nav/{ => grid}/Node.h (100%) rename nav/{ => grid}/NodeResampling.h (100%) create mode 100644 nav/grid/RegionalResampling.h create mode 100644 nav/grid/State.h create mode 100644 nav/mesh/FilterMesh.h create mode 100644 nav/mesh/NavControllerMesh.cpp create mode 100644 nav/mesh/NavControllerMesh.h create mode 100644 nav/mesh/State.h diff --git a/Controller.cpp b/Controller.cpp index c9725b5..f5ff4aa 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -11,6 +11,9 @@ #include #include +#include +#include + #include #include #include @@ -34,6 +37,8 @@ #include "tools/calibration/WiFiCalibrationDataModel.h" #include "nav/NavController.h" +#include "nav/grid/NavControllerGrid.h" +#include "nav/mesh/NavControllerMesh.h" Controller::Controller() { @@ -196,12 +201,19 @@ void Controller::on3DButton() { void Controller::onLoadButton() { + // pick a map to load QDir dir = LoadSetupDialog::pickSetupFolder(); // cancelled? if (dir.path() == ".") { return; } + 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"); @@ -213,14 +225,6 @@ void Controller::onLoadButton() { QString str = QString(fMap.readAll()); im = Floorplan::Reader::readFromString(str.toStdString()); - - //just for debugging and testing in SHL REMOVE THAT LATER IMPORTANT!!!! -// im->floors[0]->obstacles.clear(); -// im->floors[1]->obstacles.clear(); -// im->floors[2]->obstacles.clear(); -// im->floors[3]->obstacles.clear(); - - const std::string sGrid = fGrid.fileName().toStdString(); std::ifstream inp(sGrid, std::ifstream::binary); //Assert::isTrue(inp.good(), "failed to open grid.dat"); @@ -240,7 +244,7 @@ void Controller::onLoadButton() { // create a new navigator if (nav) {delete nav; nav = nullptr;} - nav = new NavController(this, grid, im); + nav = new GridBased::NavControllerGrid(this, im, grid); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); @@ -255,6 +259,44 @@ void Controller::onLoadButton() { // attach ipin step logger //nav->addListener(sl); +} + +void Controller::loadNavMesh(QDir dir) { + + 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"); + + fMap.open(QIODevice::ReadOnly); + QString str = QString(fMap.readAll()); + im = Floorplan::Reader::readFromString(str.toStdString()); + + const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); + + // create navmesh + if (navMesh) {delete navMesh; navMesh = nullptr;} + NM::NavMeshSettings settings; + NM::NavMeshFactory fac(navMesh, settings); + fac.build(im); + + // create a new navigator + if (nav) {delete nav; nav = nullptr;} + nav = new MeshBased::NavControllerMesh(this, im, navMesh); + + WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); + + getMapView3D()->setMap(im); + getMapView2D()->setMap(wifiCalib, im); + + getMapView3D()->showGridImportance(grid); + getMapView2D()->showGridImportance(grid); + + getMapView3D()->setVisible(false); + + // attach ipin step logger + //nav->addListener(sl); } diff --git a/Controller.h b/Controller.h index 9ea7d3d..2833cac 100644 --- a/Controller.h +++ b/Controller.h @@ -1,6 +1,9 @@ #ifndef CONTROLLER_H #define CONTROLLER_H + +#include + #include "misc/fixc11.h" //#include "ipin/Scaler.h" //#include "ipin/StepLoggerWrapper.h" @@ -19,6 +22,11 @@ class InfoWidget; class MapView3D; class MapView2D; +namespace NM { + template class NavMesh; + class NavMeshTriangle; +} + namespace Floorplan { class IndoorMap; } @@ -61,9 +69,13 @@ private slots: private: + void loadGrid(QDir dir); + void loadNavMesh(QDir dir); + MainWindow* mainWindow; Grid* grid = nullptr; + NM::NavMesh* navMesh = nullptr; NavController* nav = nullptr; Floorplan::IndoorMap* im = nullptr; diff --git a/Settings.h b/Settings.h index 304a916..6708529 100644 --- a/Settings.h +++ b/Settings.h @@ -6,6 +6,7 @@ #include #include +#include namespace Settings { @@ -80,11 +81,22 @@ namespace Settings { // 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;} - throw Exception("data folder missing. one of those:\n" + f1 + "\n" + f2); + //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/"); + + // 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/"; diff --git a/nav/CurEst.h b/nav/CurEst.h new file mode 100644 index 0000000..ea7a9ae --- /dev/null +++ b/nav/CurEst.h @@ -0,0 +1,16 @@ +#ifndef CUREST_H +#define CUREST_H + +#include +#include + +struct CurEst { + + Point3 pos_m; + Heading head; + + CurEst() : pos_m(0,0,0), head(0) {;} + +}; + +#endif // CUREST_H diff --git a/nav/Filter.h b/nav/Filter.h deleted file mode 100644 index df1298a..0000000 --- a/nav/Filter.h +++ /dev/null @@ -1,248 +0,0 @@ -#ifndef FILTER_H -#define FILTER_H - -#include - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "State.h" -#include "Node.h" -#include "NodeResampling.h" -#include "../Settings.h" - -#include -#include - -class PFInit : public K::ParticleFilterInitializer { - -private: - - Grid* grid; - -public: - - PFInit(Grid* grid) : grid(grid) { - - } - - virtual void initialize(std::vector>& particles) override { - - std::minstd_rand gen; - std::uniform_int_distribution distIdx(0, grid->getNumNodes()-1); - std::uniform_real_distribution distHead(0, 2*M_PI); - - - for (K::Particle& 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 - } - -// // fix position + heading -// for (K::Particle& 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 K::ParticleFilterTransition { - -public: - - /** local, static control-data COPY */ - MyControl ctrl; - - Grid* grid; - GridWalker walker; - - WalkModuleFavorZ modFavorZ; - WalkModuleHeadingControl modHeading; - WalkModuleNodeImportance modImportance; - WalkModuleFollowDestination modDestination; - WalkModuleActivityControl modActivity; - - NodeResampling resampler; - - std::minstd_rand gen; - -public: - - PFTrans(Grid* 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); - - - if (Settings::destination != GridPoint(0,0,0)) { - //walker.addModule(&modDestination); - modDestination.setDestination(grid->getNodeFor(Settings::destination)); - } - - } - - - - void transition(std::vector>& particles, const MyControl* _ctrl) override { - - // local copy!! observation might be changed async outside!! (will really produces crashes!) - this->ctrl = *_ctrl; - ((MyControl*)_ctrl)->resetAfterTransition(); - - std::normal_distribution noise(0, Settings::IMU::stepSigma); - - // sanity check - Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); - - //for (K::Particle& p : particles) { - #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)); - - K::Particle& 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");} - - } - - } - -}; - -class PFEval : public K::ParticleFilterEvaluation { - - Grid* grid; - - WiFiModelLogDistCeiling& wifiModel; - - - //WiFiObserverFree wiFiProbability; // free-calculation - WiFiObserverGrid wiFiProbability; // grid-calculation - - // smartphone is 1.3 meter above ground - const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); - -public: - - PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : - grid(grid), wifiModel(wifiModel), - //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free - wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid - - - } - - double getStairProb(const K::Particle& p, const Activity act) { - - const float kappa = 0.75; - - 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::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; - - } - - double evaluation(std::vector>& particles, const MyObservation& _observation) override { - - double sum = 0; - - // 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(); - - 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!"); - - #pragma omp parallel for num_threads(3) - for (int i = 0; i < Settings::numParticles; ++i) { - - K::Particle& p = particles[i]; - - // 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); - - - //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; - - 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; - - } - - return sum; - - } - -}; - - - -#endif // FILTER_H diff --git a/nav/NavController.cpp b/nav/NavController.cpp new file mode 100644 index 0000000..2813ec4 --- /dev/null +++ b/nav/NavController.cpp @@ -0,0 +1,46 @@ +#include "NavController.h" + +#include "Controller.h" +#include "../ui/map/3D/MapView3D.h" +#include "../ui/map/2D/MapView2D.h" + +Q_DECLARE_METATYPE(const void*) + +NavController::NavController(Controller* mainController, Floorplan::IndoorMap* im) : mainController(mainController), im(im) { + + // hacky.. but we need to call this one from the main thread! + //mainController->getMapView()->showParticles(pf->getParticles()); + qRegisterMetaType(); + +} + +void NavController::updateMapView() { + + const float kappa1 = display_ms / 1000.0f; + const float kappa2 = kappa1 * 0.7; + + const float myHeight_m = 1.80; + + curPosFast = curPosFast * (1-kappa1) + curEst.pos_m * (kappa1); + curPosSlow = curPosSlow * (1-kappa2) + curEst.pos_m * (kappa2); + + const Point3 dir = (curPosFast - curPosSlow).normalized(); + const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized(); + + // how to update the camera + if (cameraMode == 0) { + mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir); + } else if (cameraMode == 1) { + mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2); + } else if (cameraMode == 2) { + const Point3 spectator = curPosFast + Point3(0,0,25) - dir*15; + const Point3 spectatorDir = (curPosFast - spectator).normalized(); + mainController->getMapView3D()->setLookEye(spectator); + mainController->getMapView3D()->setLookDir(spectatorDir); + } + + mainController->getMapView3D()->setClipAbove(curEst.pos_m.z + 2); + mainController->getMapView3D()->setCurrentEstimation(curEst.pos_m, dir); + mainController->getMapView2D()->setCurrentEstimation(curEst.pos_m, dir); + +} diff --git a/nav/NavController.h b/nav/NavController.h index b98f249..19eabcd 100644 --- a/nav/NavController.h +++ b/nav/NavController.h @@ -9,34 +9,37 @@ #include "../sensors/StepSensor.h" #include "../sensors/TurnSensor.h" -#include "../ui/debug/SensorDataWidget.h" -#include "../ui/map/3D/MapView3D.h" -#include "../ui/debug/InfoWidget.h" +//#include "../ui/debug/SensorDataWidget.h" +//#include "../ui/map/3D/MapView3D.h" +//#include "../ui/debug/InfoWidget.h" #include #include -#include "State.h" -#include "Filter.h" -#include "Controller.h" +#include "Observation.h" +#include "../Settings.h" + +//#include "Filter.h" +//#include "Controller.h" +//#include "NavControllerListener.h" + +//#include +//#include +//#include +//#include + +////#ifndef ANDROID +////#include +////#endif + +//#include "Settings.h" +//#include "RegionalResampling.h" +//#include "NodeResampling.h" + #include "NavControllerListener.h" +#include "CurEst.h" -#include -#include -#include -#include - -#ifndef ANDROID -#include -#endif - -#include "Settings.h" -#include "RegionalResampling.h" -#include "NodeResampling.h" - -Q_DECLARE_METATYPE(const void*) - - +class Controller; class NavController : public SensorListener, @@ -50,344 +53,55 @@ class NavController : { -private: +protected: Controller* mainController; - Grid* grid; - WiFiModelLogDistCeiling wifiModel; Floorplan::IndoorMap* im; - MyObservation curObs; - MyControl curCtrl; - bool running = false; std::thread tFilter; std::thread tDisplay; - std::unique_ptr> pf; - /** the estimated path */ std::vector estPath; /** all listeners */ std::vector listeners; + /** display stuff */ + const int display_ms = Settings::MapView3D::msPerFrame.ms(); + Point3 curPosFast; + Point3 curPosSlow; + CurEst curEst; + + Timestamp lastTransition; + public: + NavController(Controller* mainController, Floorplan::IndoorMap* im); + virtual ~NavController() { if (running) {stop();} } - /** ctor */ - NavController(Controller* mainController, Grid* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) { - - // filter init - std::unique_ptr> init(new PFInit(grid)); - - // estimation - //std::unique_ptr> estimation(new K::ParticleFilterEstimationWeightedAverage()); - std::unique_ptr> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5)); - - // resampling - std::unique_ptr> resample(new NodeResampling(*grid)); - //std::unique_ptr> resample(new K::ParticleFilterResamplingSimple()); - //std::unique_ptr> resample(new K::ParticleFilterResamplingPercent(0.05)); - //std::unique_ptr resample(new RegionalResampling()); - - // eval and transition - wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - std::unique_ptr> eval(new PFEval(grid, wifiModel)); - std::unique_ptr> transition(new PFTrans(grid)); - - // setup the filter - pf = std::unique_ptr>(new K::ParticleFilter(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->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); - //SensorFactory::get().getActivity().addListener(this); - - // hacky.. but we need to call this one from the main thread! - //mainController->getMapView()->showParticles(pf->getParticles()); - qRegisterMetaType(); - - } - /** attach a new event listener */ void addListener(NavControllerListener* l) { listeners.push_back(l); } - void start() { +public: - Assert::isFalse(running, "already started!"); - running = true; - curCtrl.resetAfterTransition(); // ensure we start empty ;) - tFilter = std::thread(&NavController::filterUpdateLoop, this); - tDisplay = std::thread(&NavController::updateMapViewLoop, this); + virtual void stop() {;} - // start all sensors - SensorFactory::get().getAccelerometer().start(); - SensorFactory::get().getGyroscope().start(); - SensorFactory::get().getBarometer().start(); - SensorFactory::get().getWiFi().start(); - -#ifndef ANDROID - // #include - // run with - // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin - // show with - // kcachegrind callgrind.out.xxxx - CALLGRIND_START_INSTRUMENTATION; -#endif - - } - - void stop() { - Assert::isTrue(running, "not started!"); - running = false; - tFilter.join(); - tDisplay.join(); - } - - - void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override { - (void) sensor; - (void) data; - (void) ts; - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override { - (void) sensor; - (void) ts; - (void) data; - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override { - (void) sensor; - (void) ts; - curObs.wifi = data; - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override { - (void) sensor; - (void) ts; - curObs.gps = data; - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override { - (void) sensor; - (void) ts; - curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition - gotSensorData(ts); - } - - void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override { - (void) sensor; - (void) ts; - curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition - gotSensorData(ts); - } - -// void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override { -// (void) sensor; -// (void) ts; -// curCtrl.activity = data.curActivity; -// curObs.activity = data.curActivity; -// debugActivity(data.curActivity); -// gotSensorData(ts); -// } + virtual void start() = 0; int cameraMode = 0; void toggleCamera() { cameraMode = (cameraMode + 1) % 3; } -private: - - /** called when any sensor has received new data */ - void gotSensorData(const Timestamp ts) { - curObs.currentTime = ts; - if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} - } - - -// void debugActivity(const ActivityData& activity) { -// QString act; -// switch(activity.curActivity) { -// case ActivityButterPressure::Activity::STAY: act = "STAY"; break; -// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; -// case ActivityButterPressure::Activity::UP: act = "UP"; break; -// default: act = "???"; break; -// } -// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); -// } - - /** particle-filter update loop */ - void filterUpdateLoop() { - - - while(running && !Settings::Filter::useMainThread) { - -// // fixed update rate based on the systems time -> LIVE! even for offline data -// const Timestamp ts1 = Timestamp::fromUnixTime(); -// doUpdate(); -// const Timestamp ts2 = Timestamp::fromUnixTime(); -// const Timestamp needed = ts2-ts1; -// 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)); } - - } - } - - Timestamp lastTransition; - - /** check whether its time for a filter update, and if so, execute the update and return true */ - bool filterUpdateIfNeeded() { - - 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) { - - // 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; - - } else { - - return false; - - } - - } - - - MyState curEst; - DijkstraPath pathToDest; - - /** perform a filter-update (called from a background-loop) */ - void filterUpdate() { - - //lastEst = curEst; - curEst = pf->update(&curCtrl, curObs); - //Log::add("Nav", "cur est: " + curEst.position.asString()); - - // inform listeners about the new estimation - for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());} - - 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.position.inMeter()); - 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(curEst.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(); - - } - - - const int display_ms = Settings::MapView3D::msPerFrame.ms(); - - /** UI update loop */ - void 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)); - } - } - - Point3 curPosFast; - Point3 curPosSlow; - - /** update the map-view (called from within a background-loop) */ - void updateMapView() { - - const float kappa1 = display_ms / 1000.0f; - const float kappa2 = kappa1 * 0.7; - - const float myHeight_m = 1.80; - - curPosFast = curPosFast * (1-kappa1) + curEst.position.inMeter() * (kappa1); - curPosSlow = curPosSlow * (1-kappa2) + curEst.position.inMeter() * (kappa2); - - const Point3 dir = (curPosFast - curPosSlow).normalized(); - const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized(); - - // how to update the camera - if (cameraMode == 0) { - mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir); - } else if (cameraMode == 1) { - mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2); - } else if (cameraMode == 2) { - const Point3 spectator = curPosFast + Point3(0,0,25) - dir*15; - const Point3 spectatorDir = (curPosFast - spectator).normalized(); - mainController->getMapView3D()->setLookEye(spectator); - mainController->getMapView3D()->setLookDir(spectatorDir); - } - - mainController->getMapView3D()->setClipAbove(curEst.position.inMeter().z + 2); - mainController->getMapView3D()->setCurrentEstimation(curEst.position.inMeter(), dir); - mainController->getMapView2D()->setCurrentEstimation(curEst.position.inMeter(), dir); - - } + void updateMapView(); }; diff --git a/nav/State.h b/nav/Observation.h similarity index 52% rename from nav/State.h rename to nav/Observation.h index 9b95b39..bdd7e2b 100644 --- a/nav/State.h +++ b/nav/Observation.h @@ -1,47 +1,9 @@ #ifndef STATE_H #define STATE_H -#include -#include -#include -#include -#include - #include #include - -struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading { - - - /** ctor */ - MyState(const int x_cm, const int y_cm, const int z_cm) : WalkState(GridPoint(x_cm, y_cm, z_cm)), WalkStateHeading(Heading(0), 0) { - ; - } - - MyState() : WalkState(GridPoint()), WalkStateHeading(Heading(0), 0) { - ; - } - - MyState& operator += (const MyState& o) { - position += o.position; - return *this; - } - - MyState& operator /= (const float val) { - position /= val; - return *this; - } - - MyState operator * (const float val) const { - MyState copy = *this; - copy.position = copy.position * val; - return copy; - } - - - - -}; +#include /** observed sensor data */ struct MyObservation { diff --git a/nav/RegionalResampling.h b/nav/RegionalResampling.h deleted file mode 100644 index 7646006..0000000 --- a/nav/RegionalResampling.h +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef REGIONALRESAMPLING_H -#define REGIONALRESAMPLING_H - -#include -#include "State.h" - -class RegionalResampling : public K::ParticleFilterResampling { - -public: - - float maxDist = 12.5; - - RegionalResampling() {;} - - void resample(std::vector>& particles) override { - - Point3 sum; - for (const K::Particle& p : particles) { - sum += p.state.position.inMeter(); - } - const Point3 avg = sum / particles.size(); - - std::vector> next; - for (const K::Particle& p : particles) { - const float dist = p.state.position.inMeter().getDistance(avg); - if (rand() % 6 != 0) {continue;} - if (dist < maxDist) {next.push_back(p);} - } - - // cumulate - std::vector> copy = particles; - double cumWeight = 0; - for ( K::Particle& p : copy) { - cumWeight += p.weight; - p.weight = cumWeight; - } - - // draw missing particles - const int missing = particles.size() - next.size(); - for (int i = 0; i < missing; ++i) { - next.push_back(draw(copy, cumWeight)); - } - - std::swap(next, particles); - - } - - std::minstd_rand gen; - - /** draw one particle according to its weight from the copy vector */ - const K::Particle& draw(std::vector>& copy, const double cumWeight) { - - // generate random values between [0:cumWeight] - std::uniform_real_distribution dist(0, cumWeight); - - // draw a random value between [0:cumWeight] - const float rand = dist(gen); - - // search comparator (cumWeight is ordered -> use binary search) - auto comp = [] (const K::Particle& s, const float d) {return s.weight < d;}; - auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); - return *it; - - } - -}; - -#endif // REGIONALRESAMPLING_H diff --git a/nav/grid/Filter.h b/nav/grid/Filter.h new file mode 100644 index 0000000..ab43b50 --- /dev/null +++ b/nav/grid/Filter.h @@ -0,0 +1,253 @@ +#ifndef FILTER_H +#define FILTER_H + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "../Observation.h" +#include "State.h" +#include "Node.h" +#include "NodeResampling.h" +#include "../Settings.h" + +#include +#include + +namespace GridBased { + + class PFInit : public K::ParticleFilterInitializer { + + private: + + Grid* grid; + + public: + + PFInit(Grid* grid) : grid(grid) { + + } + + virtual void initialize(std::vector>& particles) override { + + std::minstd_rand gen; + std::uniform_int_distribution distIdx(0, grid->getNumNodes()-1); + std::uniform_real_distribution distHead(0, 2*M_PI); + + + for (K::Particle& 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 + } + + // // fix position + heading + // for (K::Particle& 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 K::ParticleFilterTransition { + + public: + + /** local, static control-data COPY */ + MyControl ctrl; + + Grid* grid; + GridWalker walker; + + WalkModuleFavorZ modFavorZ; + WalkModuleHeadingControl modHeading; + WalkModuleNodeImportance modImportance; + WalkModuleFollowDestination modDestination; + WalkModuleActivityControl modActivity; + + NodeResampling resampler; + + std::minstd_rand gen; + + public: + + PFTrans(Grid* 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); + + + if (Settings::destination != GridPoint(0,0,0)) { + //walker.addModule(&modDestination); + modDestination.setDestination(grid->getNodeFor(Settings::destination)); + } + + } + + + + void transition(std::vector>& particles, const MyControl* _ctrl) override { + + // local copy!! observation might be changed async outside!! (will really produces crashes!) + this->ctrl = *_ctrl; + ((MyControl*)_ctrl)->resetAfterTransition(); + + std::normal_distribution noise(0, Settings::IMU::stepSigma); + + // sanity check + Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + + //for (K::Particle& p : particles) { + #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)); + + K::Particle& 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");} + + } + + } + + }; + + class PFEval : public K::ParticleFilterEvaluation { + + Grid* grid; + + WiFiModelLogDistCeiling& wifiModel; + + + //WiFiObserverFree wiFiProbability; // free-calculation + WiFiObserverGrid wiFiProbability; // grid-calculation + + // smartphone is 1.3 meter above ground + const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); + + public: + + PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : + grid(grid), wifiModel(wifiModel), + //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free + wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid + + + } + + double getStairProb(const K::Particle& p, const Activity act) { + + const float kappa = 0.75; + + 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::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; + + } + + double evaluation(std::vector>& particles, const MyObservation& _observation) override { + + double sum = 0; + + // 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(); + + 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!"); + + #pragma omp parallel for num_threads(3) + for (int i = 0; i < Settings::numParticles; ++i) { + + K::Particle& p = particles[i]; + + // 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); + + + //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; + + 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; + + } + + return sum; + + } + + }; + +} + + + +#endif // FILTER_H diff --git a/nav/grid/NavControllerGrid.cpp b/nav/grid/NavControllerGrid.cpp new file mode 100644 index 0000000..59a33a1 --- /dev/null +++ b/nav/grid/NavControllerGrid.cpp @@ -0,0 +1,266 @@ +#include "NavControllerGrid.h" + +#include +#include +#include +#include + +#include "Settings.h" +#include "RegionalResampling.h" +#include "NodeResampling.h" + +#include "../ui/debug/SensorDataWidget.h" +#include "../ui/map/3D/MapView3D.h" +#include "../ui/map/2D/MapView2D.h" +#include "../ui/debug/InfoWidget.h" + +#include "../Controller.h" + +Q_DECLARE_METATYPE(const void*) + +GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { + + // filter init + std::unique_ptr> init(new PFInit(grid)); + + // estimation + //std::unique_ptr> estimation(new K::ParticleFilterEstimationWeightedAverage()); + std::unique_ptr> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5)); + + // resampling + std::unique_ptr> resample(new NodeResampling(*grid)); + //std::unique_ptr> resample(new K::ParticleFilterResamplingSimple()); + //std::unique_ptr> resample(new K::ParticleFilterResamplingPercent(0.05)); + //std::unique_ptr resample(new RegionalResampling()); + + // eval and transition + wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + std::unique_ptr> eval(new PFEval(grid, wifiModel)); + std::unique_ptr> transition(new PFTrans(grid)); + + // setup the filter + pf = std::unique_ptr>(new K::ParticleFilter(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->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); + //SensorFactory::get().getActivity().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); + + // start all sensors + SensorFactory::get().getAccelerometer().start(); + SensorFactory::get().getGyroscope().start(); + SensorFactory::get().getBarometer().start(); + SensorFactory::get().getWiFi().start(); + +#ifndef ANDROID + // #include + // 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(); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) { + (void) sensor; + (void) data; + (void) ts; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) { + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) { + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) { + (void) sensor; + (void) ts; + curObs.wifi = data; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) { + (void) sensor; + (void) ts; + curObs.gps = data; + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) { + (void) sensor; + (void) ts; + curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition + gotSensorData(ts); +} + +void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) { + (void) sensor; + (void) ts; + curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition + gotSensorData(ts); +} + +/** called when any sensor has received new data */ +void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { + curObs.currentTime = ts; + if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} +} + +// void debugActivity(const ActivityData& activity) { +// QString act; +// switch(activity.curActivity) { +// case ActivityButterPressure::Activity::STAY: act = "STAY"; break; +// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; +// case ActivityButterPressure::Activity::UP: act = "UP"; break; +// default: act = "???"; break; +// } +// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); +// } + + /** particle-filter update loop */ + void GridBased::NavControllerGrid::filterUpdateLoop() { + + + while(running && !Settings::Filter::useMainThread) { + +// // fixed update rate based on the systems time -> LIVE! even for offline data +// const Timestamp ts1 = Timestamp::fromUnixTime(); +// doUpdate(); +// const Timestamp ts2 = Timestamp::fromUnixTime(); +// const Timestamp needed = ts2-ts1; +// 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)); } + + } + } + + + + /** 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; + + // 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; + + 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 { + + return false; + + } + + } + + + + /** 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()); + + // 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"); + + // 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(); + + } + + + + /** 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)); + } + } + diff --git a/nav/grid/NavControllerGrid.h b/nav/grid/NavControllerGrid.h new file mode 100644 index 0000000..5be5778 --- /dev/null +++ b/nav/grid/NavControllerGrid.h @@ -0,0 +1,89 @@ +#ifndef NAVCONTROLLERGRID_H +#define NAVCONTROLLERGRID_H + +#include "../sensors/AccelerometerSensor.h" +#include "../sensors/GyroscopeSensor.h" +#include "../sensors/BarometerSensor.h" +#include "../sensors/WiFiSensor.h" +#include "../sensors/SensorFactory.h" +#include "../sensors/StepSensor.h" +#include "../sensors/TurnSensor.h" + +#include +#include + +#include "State.h" +#include "Filter.h" +#include "../Controller.h" + +#include "../NavController.h" + + +namespace GridBased { + + class NavControllerGrid : public NavController { + + private: + + Grid* grid; + WiFiModelLogDistCeiling wifiModel; + + std::unique_ptr> pf; + + DijkstraPath pathToDest; + + MyObservation curObs; + MyControl curCtrl; + + + public: + + NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid); + + + void start() override; + + void stop() override; + + + void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override ; + + void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; + + // void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; + + + private: + + /** called when any sensor has received new data */ + void gotSensorData(const Timestamp ts); + + // void debugActivity(const ActivityData& activity); + + /** 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(); + + /** perform a filter-update (called from a background-loop) */ + void filterUpdate(); + + /** UI update loop */ + void updateMapViewLoop(); + + }; + +} + +#endif // NAVCONTROLLERGRID_H diff --git a/nav/Node.h b/nav/grid/Node.h similarity index 100% rename from nav/Node.h rename to nav/grid/Node.h diff --git a/nav/NodeResampling.h b/nav/grid/NodeResampling.h similarity index 100% rename from nav/NodeResampling.h rename to nav/grid/NodeResampling.h diff --git a/nav/grid/RegionalResampling.h b/nav/grid/RegionalResampling.h new file mode 100644 index 0000000..21a0994 --- /dev/null +++ b/nav/grid/RegionalResampling.h @@ -0,0 +1,72 @@ +#ifndef REGIONALRESAMPLING_H +#define REGIONALRESAMPLING_H + +#include +#include "State.h" + +namespace GridBased { + + class RegionalResampling : public K::ParticleFilterResampling { + + public: + + float maxDist = 12.5; + + RegionalResampling() {;} + + void resample(std::vector>& particles) override { + + Point3 sum; + for (const K::Particle& p : particles) { + sum += p.state.position.inMeter(); + } + const Point3 avg = sum / particles.size(); + + std::vector> next; + for (const K::Particle& p : particles) { + const float dist = p.state.position.inMeter().getDistance(avg); + if (rand() % 6 != 0) {continue;} + if (dist < maxDist) {next.push_back(p);} + } + + // cumulate + std::vector> copy = particles; + double cumWeight = 0; + for ( K::Particle& p : copy) { + cumWeight += p.weight; + p.weight = cumWeight; + } + + // draw missing particles + const int missing = particles.size() - next.size(); + for (int i = 0; i < missing; ++i) { + next.push_back(draw(copy, cumWeight)); + } + + std::swap(next, particles); + + } + + std::minstd_rand gen; + + /** draw one particle according to its weight from the copy vector */ + const K::Particle& draw(std::vector>& copy, const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const K::Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); + return *it; + + } + + }; + +} + +#endif // REGIONALRESAMPLING_H diff --git a/nav/grid/State.h b/nav/grid/State.h new file mode 100644 index 0000000..5113e14 --- /dev/null +++ b/nav/grid/State.h @@ -0,0 +1,44 @@ +#ifndef GRID_STATE_H +#define GRID_STATE_H + +#include +#include +#include +#include +#include + +namespace GridBased { + + struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading { + + + /** ctor */ + MyState(const int x_cm, const int y_cm, const int z_cm) : WalkState(GridPoint(x_cm, y_cm, z_cm)), WalkStateHeading(Heading(0), 0) { + ; + } + + MyState() : WalkState(GridPoint()), WalkStateHeading(Heading(0), 0) { + ; + } + + MyState& operator += (const MyState& o) { + position += o.position; + return *this; + } + + MyState& operator /= (const float val) { + position /= val; + return *this; + } + + MyState operator * (const float val) const { + MyState copy = *this; + copy.position = copy.position * val; + return copy; + } + + }; + +} + +#endif // GRID_STATE_H diff --git a/nav/mesh/FilterMesh.h b/nav/mesh/FilterMesh.h new file mode 100644 index 0000000..fdfabbf --- /dev/null +++ b/nav/mesh/FilterMesh.h @@ -0,0 +1,220 @@ +#ifndef FILTERMESH_ +#define FILTERMESH_ + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "State.h" +#include "../../Settings.h" + +#include +#include + +namespace MeshBased { + + class PFInit : public K::ParticleFilterInitializer { + + private: + + NM::NavMesh* mesh; + + public: + + PFInit(NM::NavMesh* mesh) : mesh(mesh) { + + } + + virtual void initialize(std::vector>& particles) override { + + std::minstd_rand gen; + std::uniform_real_distribution distHead(0, 2*M_PI); + + NM::NavMeshRandom rnd = mesh->getRandom(); + + for (K::Particle& p : particles) { + p.state.loc = rnd.draw(); + p.state.heading = Heading(distHead(gen)); // random heading + p.weight = 1.0 / particles.size(); // equal weight + } + + // // fix position + heading + // for (K::Particle& 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 K::ParticleFilterTransition { + + public: + + // local, static control-data COPY + MyControl ctrl; + + Grid* grid; + GridWalker walker; + + WalkModuleFavorZ modFavorZ; + WalkModuleHeadingControl modHeading; + WalkModuleNodeImportance modImportance; + WalkModuleFollowDestination modDestination; + WalkModuleActivityControl modActivity; + + NodeResampling resampler; + + std::minstd_rand gen; + + public: + + PFTrans(Grid* 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); + + + if (Settings::destination != GridPoint(0,0,0)) { + //walker.addModule(&modDestination); + modDestination.setDestination(grid->getNodeFor(Settings::destination)); + } + + } + + + + void transition(std::vector>& particles, const MyControl* _ctrl) override { + + // local copy!! observation might be changed async outside!! (will really produces crashes!) + this->ctrl = *_ctrl; + ((MyControl*)_ctrl)->resetAfterTransition(); + + std::normal_distribution noise(0, Settings::IMU::stepSigma); + + // sanity check + Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); + + //for (K::Particle& p : particles) { + #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)); + + K::Particle& 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");} + + } + + } + + }; + + class PFEval : public K::ParticleFilterEvaluation { + + Grid* grid; + + WiFiModelLogDistCeiling& wifiModel; + + + //WiFiObserverFree wiFiProbability; // free-calculation + WiFiObserverGrid wiFiProbability; // grid-calculation + + // smartphone is 1.3 meter above ground + const Point3 person = Point3(0,0,Settings::smartphoneAboveGround); + + public: + + PFEval(Grid* grid, WiFiModelLogDistCeiling& wifiModel) : + grid(grid), wifiModel(wifiModel), + //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free + wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid + + + } + + double evaluation(std::vector>& particles, const MyObservation& _observation) override { + + double sum = 0; + + // 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(); + + 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!"); + + #pragma omp parallel for num_threads(3) + for (int i = 0; i < Settings::numParticles; ++i) { + + K::Particle& p = particles[i]; + + // 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); + + + //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; + + 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; + + } + + return sum; + + } + + }; + + */ +} + +#endif // FILTERMESH_ diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp new file mode 100644 index 0000000..2f9e61b --- /dev/null +++ b/nav/mesh/NavControllerMesh.cpp @@ -0,0 +1,290 @@ +#include "NavControllerMesh.h" + +#include "../ui/debug/SensorDataWidget.h" +#include "../ui/map/3D/MapView3D.h" +#include "../ui/map/2D/MapView2D.h" +#include "../ui/debug/InfoWidget.h" + +#include +#include + +#include "State.h" +#include "FilterMesh.h" +#include "Controller.h" +#include "../NavControllerListener.h" + +#include +#include +#include +#include + +#include +#include +#include + +//#ifndef ANDROID +//#include +//#endif + +#include "Settings.h" + +Q_DECLARE_METATYPE(const void*) + +/** ctor */ +MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh) : + NavController(mainController, im), navMesh(navMesh), wifiModel(im) { + + // filter init + std::unique_ptr> init(new MeshBased::PFInit(navMesh)); + +// // estimation +// //std::unique_ptr> estimation(new K::ParticleFilterEstimationWeightedAverage()); +// std::unique_ptr> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5)); + +// // resampling +// std::unique_ptr> resample(new NodeResampling(*grid)); +// //std::unique_ptr> resample(new K::ParticleFilterResamplingSimple()); +// //std::unique_ptr> resample(new K::ParticleFilterResamplingPercent(0.05)); +// //std::unique_ptr resample(new RegionalResampling()); + +// // eval and transition +// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); +// std::unique_ptr> eval(new PFEval(grid, wifiModel)); +// std::unique_ptr> transition(new PFTrans(grid)); + +// // setup the filter +// pf = std::unique_ptr>(new K::ParticleFilter(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->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); + //SensorFactory::get().getActivity().addListener(this); + + // hacky.. but we need to call this one from the main thread! + //mainController->getMapView()->showParticles(pf->getParticles()); + qRegisterMetaType(); + +} + + +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); + + // start all sensors + SensorFactory::get().getAccelerometer().start(); + SensorFactory::get().getGyroscope().start(); + SensorFactory::get().getBarometer().start(); + SensorFactory::get().getWiFi().start(); + +//#ifndef ANDROID +// // #include +// // run with +// // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin +// // show with +// // kcachegrind callgrind.out.xxxx +// CALLGRIND_START_INSTRUMENTATION; +//#endif + +} + +void MeshBased::NavControllerMesh::stop() { + Assert::isTrue(running, "not started!"); + running = false; + tFilter.join(); + tDisplay.join(); +} + + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) { + (void) sensor; + (void) data; + (void) ts; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) { + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) { + (void) sensor; + (void) ts; + (void) data; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) { + (void) sensor; + (void) ts; + curObs.wifi = data; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) { + (void) sensor; + (void) ts; + curObs.gps = data; + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) { + (void) sensor; + (void) ts; + curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition + gotSensorData(ts); +} + +void MeshBased::NavControllerMesh::onSensorData(Sensor* 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 NavControllerMesh::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { +// (void) sensor; +// (void) ts; +// curCtrl.activity = data.curActivity; +// curObs.activity = data.curActivity; +// debugActivity(data.curActivity); +// gotSensorData(ts); +// } + +/** called when any sensor has received new data */ +void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { + curObs.currentTime = ts; + if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} +} + +// void debugActivity(const ActivityData& activity) { +// QString act; +// switch(activity.curActivity) { +// case ActivityButterPressure::Activity::STAY: act = "STAY"; break; +// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; +// case ActivityButterPressure::Activity::UP: act = "UP"; break; +// default: act = "???"; break; +// } +// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); +// } + + /** particle-filter update loop */ + void MeshBased::NavControllerMesh::filterUpdateLoop() { + + + while(running && !Settings::Filter::useMainThread) { + +// // fixed update rate based on the systems time -> LIVE! even for offline data +// const Timestamp ts1 = Timestamp::fromUnixTime(); +// doUpdate(); +// const Timestamp ts2 = Timestamp::fromUnixTime(); +// const Timestamp needed = ts2-ts1; +// 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)); } + + } + } + + Timestamp lastTransition; + + /** 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; + + // 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; + + 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 { + + return false; + + } + + } + + DijkstraPath pathToDest; + + /** perform a filter-update (called from a background-loop) */ + void MeshBased::NavControllerMesh::filterUpdate() { + +// //lastEst = curEst; +// curEst = pf->update(&curCtrl, curObs); +// //Log::add("Nav", "cur est: " + curEst.position.asString()); + +// // inform listeners about the new estimation +// for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());} + +// 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.position.inMeter()); +// 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(curEst.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 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)); + } + } diff --git a/nav/mesh/NavControllerMesh.h b/nav/mesh/NavControllerMesh.h new file mode 100644 index 0000000..3d7c6ec --- /dev/null +++ b/nav/mesh/NavControllerMesh.h @@ -0,0 +1,88 @@ +#ifndef NAVCONTROLLERMESH_H +#define NAVCONTROLLERMESH_H + +#include "../sensors/AccelerometerSensor.h" +#include "../sensors/GyroscopeSensor.h" +#include "../sensors/BarometerSensor.h" +#include "../sensors/WiFiSensor.h" +#include "../sensors/SensorFactory.h" +#include "../sensors/StepSensor.h" +#include "../sensors/TurnSensor.h" + +#include +#include + +#include +#include + +//#include "State.h" +#include "FilterMesh.h" + +#include "../Controller.h" +#include "../NavController.h" + +namespace MeshBased { + + class NavControllerMesh : public NavController { + + private: + + NM::NavMesh* navMesh; + WiFiModelLogDistCeiling wifiModel; + + std::unique_ptr> pf; + + MyObservation curObs; + MyControl curCtrl; + + public: + + NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh* navMesh); + + + void start() override; + + void stop() override; + + + void onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) override; + + void onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) override; + + // void onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) override ; + + + private: + + /** called when any sensor has received new data */ + void gotSensorData(const Timestamp ts); + + // void debugActivity(const ActivityData& activity); + + /** 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(); + + /** perform a filter-update (called from a background-loop) */ + void filterUpdate(); + + /** UI update loop */ + void updateMapViewLoop(); + + }; + +} + +#endif // NAVCONTROLLERMESH_H diff --git a/nav/mesh/State.h b/nav/mesh/State.h new file mode 100644 index 0000000..e46bbf9 --- /dev/null +++ b/nav/mesh/State.h @@ -0,0 +1,45 @@ +#ifndef MESH_STATE_H +#define MESH_STATE_H + +#include +#include +#include + +namespace MeshBased { + + struct MyState { + + NM::NavMeshLocation loc; + Heading heading; + + /** ctor */ + MyState() : loc(), heading(0) { + ; + } + + /** ctor */ + MyState(NM::NavMeshLocation loc, Heading h) : loc(loc), heading(h) { + ; + } + +// MyState& operator += (const MyState& o) { +// position += o.position; +// return *this; +// } + +// MyState& operator /= (const float val) { +// position /= val; +// return *this; +// } + +// MyState operator * (const float val) const { +// MyState copy = *this; +// copy.position = copy.position * val; +// return copy; +// } + + }; + +} + +#endif // MESH_STATE_H diff --git a/ui/map/2D/ColorPoints2D.h b/ui/map/2D/ColorPoints2D.h index 0d477e1..ca0ec30 100644 --- a/ui/map/2D/ColorPoints2D.h +++ b/ui/map/2D/ColorPoints2D.h @@ -8,8 +8,8 @@ #include #include -#include "../nav/Node.h" -#include "../nav/State.h" +#include "../nav/grid/Node.h" +#include "../nav/Observation.h" /** * debug color points diff --git a/ui/map/2D/MapView2D.cpp b/ui/map/2D/MapView2D.cpp index 3fa85e7..5a096b4 100644 --- a/ui/map/2D/MapView2D.cpp +++ b/ui/map/2D/MapView2D.cpp @@ -111,7 +111,7 @@ void MapView2D::setMap(WiFiCalibrationDataModel* mdl, Floorplan::IndoorMap* map) } -void MapView2D::showParticles(const std::vector>* particles) { +void MapView2D::showParticles(const std::vector>* particles) { this->colorPoints->setFromParticles(*particles); } diff --git a/ui/map/2D/MapView2D.h b/ui/map/2D/MapView2D.h index f85b12d..56b09e3 100644 --- a/ui/map/2D/MapView2D.h +++ b/ui/map/2D/MapView2D.h @@ -12,6 +12,8 @@ #include #include +#include "nav/grid/State.h" + namespace Floorplan { class IndoorMap; } @@ -99,11 +101,11 @@ public: /** NOTE: must be called from Qt's main thread! */ Q_INVOKABLE void showParticles(const void* particles) { - showParticles((const std::vector>*) particles); + showParticles((const std::vector>*) particles); } /** NOTE: must be called from Qt's main thread! */ - void showParticles(const std::vector>* particles); + void showParticles(const std::vector>* particles); diff --git a/ui/map/2D/Path2D.h b/ui/map/2D/Path2D.h index 763f54c..cc72627 100644 --- a/ui/map/2D/Path2D.h +++ b/ui/map/2D/Path2D.h @@ -8,7 +8,7 @@ #include #include -#include "../nav/Node.h" +#include "../nav/grid/Node.h" class Path2D : public Renderable2D { diff --git a/ui/map/3D/MapView3D.h b/ui/map/3D/MapView3D.h index d56372b..da3847b 100644 --- a/ui/map/3D/MapView3D.h +++ b/ui/map/3D/MapView3D.h @@ -16,7 +16,9 @@ #include "elements/ColorPoints.h" #include "elements/Object.h" -#include "../nav/State.h" +#include "../nav/Observation.h" +#include "../nav/grid/State.h" + namespace Floorplan { class IndoorMap; @@ -122,11 +124,11 @@ public: /** NOTE: must be called from Qt's main thread! */ Q_INVOKABLE void showParticles(const void* particles) { - showParticles((const std::vector>*) particles); + showParticles((const std::vector>*) particles); } /** NOTE: must be called from Qt's main thread! */ - void showParticles(const std::vector>* particles) { + void showParticles(const std::vector>* particles) { this->colorPoints->setFromParticles(*particles); } diff --git a/ui/map/3D/elements/ColorPoints.h b/ui/map/3D/elements/ColorPoints.h index 7d2c2a0..bfdc681 100644 --- a/ui/map/3D/elements/ColorPoints.h +++ b/ui/map/3D/elements/ColorPoints.h @@ -11,7 +11,7 @@ #include "../Renderable.h" #include "../../../../Settings.h" -#include "../../../../nav/Node.h" +#include "../../../../nav/grid/Node.h" class ColorPoints : public Renderable { diff --git a/yasmin.pro b/yasmin.pro index 6d9dc33..b805239 100644 --- a/yasmin.pro +++ b/yasmin.pro @@ -66,6 +66,7 @@ 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 \ @@ -80,7 +81,10 @@ SOURCES += \ ui/map/3D/MapView3D.cpp \ ui/map/2D/MapView2D.cpp \ tools/calibration/WiFiCalibrationScanDialog.cpp \ - ui/debug/PlotGPS.cpp + ui/debug/PlotGPS.cpp \ + nav/NavController.cpp \ + nav/mesh/NavControllerMesh.cpp \ + nav/grid/NavControllerGrid.cpp RESOURCES += qml.qrc @@ -151,21 +155,16 @@ HEADERS += \ ui/debug/plot/PlottWidget.h \ ui/debug/PlotTurns.h \ ui/debug/PlotWiFiScan.h \ - nav/State.h \ - nav/Filter.h \ - nav/Node.h \ sensors/linux/WiFiSensorLinuxC.h \ sensors/offline/SensorFactoryOffline.h \ sensors/dummy/SensorFactoryDummy.h \ sensors/android/SensorFactoryAndroid.h \ Settings.h \ - nav/RegionalResampling.h \ sensors/offline/AllInOneSensor.h \ sensors/ActivitySensor.h \ ui/LoggerUI.h \ ui/debug/InfoWidget.h \ ui/UIHelper.h \ - nav/NodeResampling.h \ ui/map/3D/MapView3D.h \ ui/map/2D/MapView2D.h \ ui/map/2D/Floor2D.h \ @@ -193,7 +192,18 @@ HEADERS += \ sensors/dummy/CompassSensorDummy.h \ sensors/dummy/GPSSensorDummy.h \ ui/debug/PlotGPS.h \ - sensors/SensorWriter.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 DISTFILES += \ android-sources/src/MyActivity.java \