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

refactoring to add nav mesh

Browse Source
This commit is contained in:
k-a-z-u
2018-07-11 19:04:42 +02:00
parent bb974d3871
commit b4a1a3d969
27 changed files with 1581 additions and 712 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
Controller.cpp
View File

@@ -11,6 +11,9 @@
#include <Indoor/grid/factory/v2/GridFactory.h> #include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h> #include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshFactory.h>
#include <Indoor/floorplan/v2/Floorplan.h> #include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h> #include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h> #include <Indoor/floorplan/v2/FloorplanHelper.h>
@@ -34,6 +37,8 @@
#include "tools/calibration/WiFiCalibrationDataModel.h" #include "tools/calibration/WiFiCalibrationDataModel.h"
#include "nav/NavController.h" #include "nav/NavController.h"
#include "nav/grid/NavControllerGrid.h"
#include "nav/mesh/NavControllerMesh.h"
Controller::Controller() { Controller::Controller() {
@@ -196,12 +201,19 @@ void Controller::on3DButton() {
void Controller::onLoadButton() { void Controller::onLoadButton() {
// pick a map to load // pick a map to load
QDir dir = LoadSetupDialog::pickSetupFolder(); QDir dir = LoadSetupDialog::pickSetupFolder();
// cancelled? // cancelled?
if (dir.path() == ".") { return; } if (dir.path() == ".") { return; }
loadNavMesh(dir);
}
void Controller::loadGrid(QDir dir) {
QFile fMap(dir.path() + "/map.xml"); QFile fMap(dir.path() + "/map.xml");
QFile fGrid(dir.path() + "/grid.dat"); QFile fGrid(dir.path() + "/grid.dat");
QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
@@ -213,14 +225,6 @@ void Controller::onLoadButton() {
QString str = QString(fMap.readAll()); QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString()); 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(); const std::string sGrid = fGrid.fileName().toStdString();
std::ifstream inp(sGrid, std::ifstream::binary); std::ifstream inp(sGrid, std::ifstream::binary);
//Assert::isTrue(inp.good(), "failed to open grid.dat"); //Assert::isTrue(inp.good(), "failed to open grid.dat");
@@ -240,7 +244,7 @@ void Controller::onLoadButton() {
// create a new navigator // create a new navigator
if (nav) {delete nav; nav = nullptr;} if (nav) {delete nav; nav = nullptr;}
nav = new NavController(this, grid, im); nav = new GridBased::NavControllerGrid(this, im, grid);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);
@@ -255,6 +259,44 @@ void Controller::onLoadButton() {
// attach ipin step logger // attach ipin step logger
//nav->addListener(sl); //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<NM::NavMeshTriangle> 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);
} }

12
Controller.h
View File

@@ -1,6 +1,9 @@
#ifndef CONTROLLER_H #ifndef CONTROLLER_H
#define CONTROLLER_H #define CONTROLLER_H
#include <QDir>
#include "misc/fixc11.h" #include "misc/fixc11.h"
//#include "ipin/Scaler.h" //#include "ipin/Scaler.h"
//#include "ipin/StepLoggerWrapper.h" //#include "ipin/StepLoggerWrapper.h"
@@ -19,6 +22,11 @@ class InfoWidget;
class MapView3D; class MapView3D;
class MapView2D; class MapView2D;
namespace NM {
template <typename T> class NavMesh;
class NavMeshTriangle;
}
namespace Floorplan { namespace Floorplan {
class IndoorMap; class IndoorMap;
} }
@@ -61,9 +69,13 @@ private slots:
private: private:
void loadGrid(QDir dir);
void loadNavMesh(QDir dir);
MainWindow* mainWindow; MainWindow* mainWindow;
Grid<MyGridNode>* grid = nullptr; Grid<MyGridNode>* grid = nullptr;
NM::NavMesh<NM::NavMeshTriangle>* navMesh = nullptr;
NavController* nav = nullptr; NavController* nav = nullptr;
Floorplan::IndoorMap* im = nullptr; Floorplan::IndoorMap* im = nullptr;

22
Settings.h
View File

@@ -6,6 +6,7 @@
#include <Indoor/sensors/radio/VAPGrouper.h> #include <Indoor/sensors/radio/VAPGrouper.h>
#include <QFile> #include <QFile>
#include <QDir>
namespace Settings { namespace Settings {
@@ -80,11 +81,22 @@ namespace Settings {
// qDebug(folder.c_str()); // qDebug(folder.c_str());
// return folder; // return folder;
//return "/storage/sdcard1/YASMIN/"; //return "/storage/sdcard1/YASMIN/";
const std::string f1 = "/storage/sdcard1/YASMIN/"; //const std::string f1 = "/storage/sdcard1/YASMIN/";
const std::string f2 = "/sdcard/YASMIN/"; //const std::string f2 = "/sdcard/YASMIN/";
if (QFile(f1.c_str()).exists()) {return f1;} //if (QFile(f1.c_str()).exists()) {return f1;}
if (QFile(f2.c_str()).exists()) {return f2;} //if (QFile(f2.c_str()).exists()) {return f2;}
throw Exception("data folder missing. one of those:\n" + f1 + "\n" + 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 #else
//return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/"; //return "/home/toni/Documents/programme/localization/YASMIN/YASMIN_DATA/";
return "/apps/android/workspace/YASMIN_DATA/"; return "/apps/android/workspace/YASMIN_DATA/";

16
nav/CurEst.h Normal file
View File

@@ -0,0 +1,16 @@
#ifndef CUREST_H
#define CUREST_H
#include <Indoor/geo/Point3.h>
#include <Indoor/geo/Heading.h>
struct CurEst {
Point3 pos_m;
Heading head;
CurEst() : pos_m(0,0,0), head(0) {;}
};
#endif // CUREST_H

248
nav/Filter.h
View File

@@ -1,248 +0,0 @@
#ifndef FILTER_H
#define FILTER_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.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/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include "State.h"
#include "Node.h"
#include "NodeResampling.h"
#include "../Settings.h"
#include <omp.h>
#include <future>
class PFInit : public K::ParticleFilterInitializer<MyState> {
private:
Grid<MyGridNode>* grid;
public:
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
p.state.position = node; // random position
p.state.heading.direction = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// for (K::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 K::ParticleFilterTransition<MyState, MyControl> {
public:
/** local, static control-data COPY */
MyControl ctrl;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
}
void transition(std::vector<K::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();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& 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<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
}
};
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double getStairProb(const K::Particle<MyState>& 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<K::Particle<MyState>>& 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<MyState>& 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

46
nav/NavController.cpp Normal file
View File

@@ -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<const void*>();
}
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);
}

366
nav/NavController.h
View File

@@ -9,34 +9,37 @@
#include "../sensors/StepSensor.h" #include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h" #include "../sensors/TurnSensor.h"
#include "../ui/debug/SensorDataWidget.h" //#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/3D/MapView3D.h" //#include "../ui/map/3D/MapView3D.h"
#include "../ui/debug/InfoWidget.h" //#include "../ui/debug/InfoWidget.h"
#include <Indoor/Assertions.h> #include <Indoor/Assertions.h>
#include <thread> #include <thread>
#include "State.h" #include "Observation.h"
#include "Filter.h" #include "../Settings.h"
#include "Controller.h"
//#include "Filter.h"
//#include "Controller.h"
//#include "NavControllerListener.h"
//#include <KLib/misc/gnuplot/Gnuplot.h>
//#include <KLib/misc/gnuplot/GnuplotSplot.h>
//#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
//#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
////#ifndef ANDROID
////#include <valgrind/callgrind.h>
////#endif
//#include "Settings.h"
//#include "RegionalResampling.h"
//#include "NodeResampling.h"
#include "NavControllerListener.h" #include "NavControllerListener.h"
#include "CurEst.h"
#include <KLib/misc/gnuplot/Gnuplot.h> class Controller;
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#ifndef ANDROID
#include <valgrind/callgrind.h>
#endif
#include "Settings.h"
#include "RegionalResampling.h"
#include "NodeResampling.h"
Q_DECLARE_METATYPE(const void*)
class NavController : class NavController :
public SensorListener<AccelerometerData>, public SensorListener<AccelerometerData>,
@@ -50,344 +53,55 @@ class NavController :
{ {
private: protected:
Controller* mainController; Controller* mainController;
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
Floorplan::IndoorMap* im; Floorplan::IndoorMap* im;
MyObservation curObs;
MyControl curCtrl;
bool running = false; bool running = false;
std::thread tFilter; std::thread tFilter;
std::thread tDisplay; std::thread tDisplay;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
/** the estimated path */ /** the estimated path */
std::vector<Point3> estPath; std::vector<Point3> estPath;
/** all listeners */ /** all listeners */
std::vector<NavControllerListener*> listeners; std::vector<NavControllerListener*> listeners;
/** display stuff */
const int display_ms = Settings::MapView3D::msPerFrame.ms();
Point3 curPosFast;
Point3 curPosSlow;
CurEst curEst;
Timestamp lastTransition;
public: public:
NavController(Controller* mainController, Floorplan::IndoorMap* im);
virtual ~NavController() { virtual ~NavController() {
if (running) {stop();} if (running) {stop();}
} }
/** ctor */
NavController(Controller* mainController, Grid<MyGridNode>* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter
pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample));
pf->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<const void*>();
}
/** attach a new event listener */ /** attach a new event listener */
void addListener(NavControllerListener* l) { void addListener(NavControllerListener* l) {
listeners.push_back(l); listeners.push_back(l);
} }
void start() { public:
Assert::isFalse(running, "already started!"); virtual void stop() {;}
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavController::filterUpdateLoop, this);
tDisplay = std::thread(&NavController::updateMapViewLoop, this);
// start all sensors virtual void start() = 0;
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
#ifndef ANDROID
// #include <valgrind/callgrind.h>
// run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with
// kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION;
#endif
}
void stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void onSensorData(Sensor<StepData>* 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<TurnData>* 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<ActivityData>* 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);
// }
int cameraMode = 0; int cameraMode = 0;
void toggleCamera() { void toggleCamera() {
cameraMode = (cameraMode + 1) % 3; 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<MyGridNode> 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) */ /** update the map-view (called from within a background-loop) */
void updateMapView() { 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);
}
}; };

40
nav/State.h → nav/Observation.h
View File

@@ -1,47 +1,9 @@
#ifndef STATE_H #ifndef STATE_H
#define STATE_H #define STATE_H
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h> #include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/gps/GPSData.h> #include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/activity/Activity.h>
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;
}
};
/** observed sensor data */ /** observed sensor data */
struct MyObservation { struct MyObservation {

68
nav/RegionalResampling.h
View File

@@ -1,68 +0,0 @@
#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include "State.h"
class RegionalResampling : public K::ParticleFilterResampling<MyState> {
public:
float maxDist = 12.5;
RegionalResampling() {;}
void resample(std::vector<K::Particle<MyState>>& particles) override {
Point3 sum;
for (const K::Particle<MyState>& p : particles) {
sum += p.state.position.inMeter();
}
const Point3 avg = sum / particles.size();
std::vector<K::Particle<MyState>> next;
for (const K::Particle<MyState>& 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<K::Particle<MyState>> copy = particles;
double cumWeight = 0;
for ( K::Particle<MyState>& 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<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> 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<MyState>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
return *it;
}
};
#endif // REGIONALRESAMPLING_H

253
nav/grid/Filter.h Normal file
View File

@@ -0,0 +1,253 @@
#ifndef FILTER_H
#define FILTER_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.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/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include "../Observation.h"
#include "State.h"
#include "Node.h"
#include "NodeResampling.h"
#include "../Settings.h"
#include <omp.h>
#include <future>
namespace GridBased {
class PFInit : public K::ParticleFilterInitializer<MyState> {
private:
Grid<MyGridNode>* grid;
public:
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
p.state.position = node; // random position
p.state.heading.direction = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// for (K::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 K::ParticleFilterTransition<MyState, MyControl> {
public:
/** local, static control-data COPY */
MyControl ctrl;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
}
void transition(std::vector<K::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();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& 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<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
}
};
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double getStairProb(const K::Particle<MyState>& 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<K::Particle<MyState>>& 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<MyState>& 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

266
nav/grid/NavControllerGrid.cpp Normal file
View File

@@ -0,0 +1,266 @@
#include "NavControllerGrid.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#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<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter
pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample));
pf->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 <valgrind/callgrind.h>
// run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with
// kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION;
#endif
}
void GridBased::NavControllerGrid::stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void GridBased::NavControllerGrid::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
/** 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));
}
}

89
nav/grid/NavControllerGrid.h Normal file
View File

@@ -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 <Indoor/Assertions.h>
#include <thread>
#include "State.h"
#include "Filter.h"
#include "../Controller.h"
#include "../NavController.h"
namespace GridBased {
class NavControllerGrid : public NavController {
private:
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest;
MyObservation curObs;
MyControl curCtrl;
public:
NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);
void start() override;
void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:
/** 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

0
nav/Node.h → nav/grid/Node.h
View File

0
nav/NodeResampling.h → nav/grid/NodeResampling.h
View File

72
nav/grid/RegionalResampling.h Normal file
View File

@@ -0,0 +1,72 @@
#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include "State.h"
namespace GridBased {
class RegionalResampling : public K::ParticleFilterResampling<MyState> {
public:
float maxDist = 12.5;
RegionalResampling() {;}
void resample(std::vector<K::Particle<MyState>>& particles) override {
Point3 sum;
for (const K::Particle<MyState>& p : particles) {
sum += p.state.position.inMeter();
}
const Point3 avg = sum / particles.size();
std::vector<K::Particle<MyState>> next;
for (const K::Particle<MyState>& 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<K::Particle<MyState>> copy = particles;
double cumWeight = 0;
for ( K::Particle<MyState>& 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<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> 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<MyState>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
return *it;
}
};
}
#endif // REGIONALRESAMPLING_H

44
nav/grid/State.h Normal file
View File

@@ -0,0 +1,44 @@
#ifndef GRID_STATE_H
#define GRID_STATE_H
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
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

220
nav/mesh/FilterMesh.h Normal file
View File

@@ -0,0 +1,220 @@
#ifndef FILTERMESH_
#define FILTERMESH_
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.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/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include "State.h"
#include "../../Settings.h"
#include <omp.h>
#include <future>
namespace MeshBased {
class PFInit : public K::ParticleFilterInitializer<MyState> {
private:
NM::NavMesh<NM::NavMeshTriangle>* mesh;
public:
PFInit(NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (K::Particle<MyState>& 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<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 K::ParticleFilterTransition<MyState, MyControl> {
public:
// local, static control-data COPY
MyControl ctrl;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
}
void transition(std::vector<K::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();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& 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<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
}
};
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double evaluation(std::vector<K::Particle<MyState>>& 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<MyState>& 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_

290
nav/mesh/NavControllerMesh.cpp Normal file
View File

@@ -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 <Indoor/Assertions.h>
#include <thread>
#include "State.h"
#include "FilterMesh.h"
#include "Controller.h"
#include "../NavControllerListener.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshTriangle.h>
#include <Indoor/floorplan/v2/Floorplan.h>
//#ifndef ANDROID
//#include <valgrind/callgrind.h>
//#endif
#include "Settings.h"
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh) :
NavController(mainController, im), navMesh(navMesh), wifiModel(im) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new MeshBased::PFInit(navMesh));
// // estimation
// //std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
// std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// // resampling
// std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
// //std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
// //std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
// //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// // eval and transition
// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
// std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
// std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// // setup the filter
// pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
// pf->setTransition(std::move(transition));
// pf->setEvaluation(std::move(eval));
// pf->setEstimation(std::move(estimation));
// pf->setResampling(std::move(resample));
// pf->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<const void*>();
}
void MeshBased::NavControllerMesh::start() {
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
//#ifndef ANDROID
// // #include <valgrind/callgrind.h>
// // run with
// // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// // show with
// // kcachegrind callgrind.out.xxxx
// CALLGRIND_START_INSTRUMENTATION;
//#endif
}
void MeshBased::NavControllerMesh::stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
// void NavControllerMesh::onSensorData(Sensor<ActivityData>* 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<MyGridNode> 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));
}
}

88
nav/mesh/NavControllerMesh.h Normal file
View File

@@ -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 <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/Assertions.h>
#include <thread>
//#include "State.h"
#include "FilterMesh.h"
#include "../Controller.h"
#include "../NavController.h"
namespace MeshBased {
class NavControllerMesh : public NavController {
private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs;
MyControl curCtrl;
public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh);
void start() override;
void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:
/** 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

45
nav/mesh/State.h Normal file
View File

@@ -0,0 +1,45 @@
#ifndef MESH_STATE_H
#define MESH_STATE_H
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshTriangle.h>
#include <Indoor/geo/Heading.h>
namespace MeshBased {
struct MyState {
NM::NavMeshLocation<NM::NavMeshTriangle> loc;
Heading heading;
/** ctor */
MyState() : loc(), heading(0) {
;
}
/** ctor */
MyState(NM::NavMeshLocation<NM::NavMeshTriangle> 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

4
ui/map/2D/ColorPoints2D.h
View File

@@ -8,8 +8,8 @@
#include <Indoor/grid/Grid.h> #include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/Particle.h> #include <KLib/math/filter/particles/Particle.h>
#include "../nav/Node.h" #include "../nav/grid/Node.h"
#include "../nav/State.h" #include "../nav/Observation.h"
/** /**
* debug color points * debug color points

2
ui/map/2D/MapView2D.cpp
View File

@@ -111,7 +111,7 @@ void MapView2D::setMap(WiFiCalibrationDataModel* mdl, Floorplan::IndoorMap* map)
} }
void MapView2D::showParticles(const std::vector<K::Particle<MyState>>* particles) { void MapView2D::showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles) {
this->colorPoints->setFromParticles(*particles); this->colorPoints->setFromParticles(*particles);
} }

6
ui/map/2D/MapView2D.h
View File

@@ -12,6 +12,8 @@
#include <Indoor/nav/dijkstra/DijkstraPath.h> #include <Indoor/nav/dijkstra/DijkstraPath.h>
#include <Indoor/geo/Point3.h> #include <Indoor/geo/Point3.h>
#include "nav/grid/State.h"
namespace Floorplan { namespace Floorplan {
class IndoorMap; class IndoorMap;
} }
@@ -99,11 +101,11 @@ public:
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
Q_INVOKABLE void showParticles(const void* particles) { Q_INVOKABLE void showParticles(const void* particles) {
showParticles((const std::vector<K::Particle<MyState>>*) particles); showParticles((const std::vector<K::Particle<GridBased::MyState>>*) particles);
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<K::Particle<MyState>>* particles); void showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles);

2
ui/map/2D/Path2D.h
View File

@@ -8,7 +8,7 @@
#include <Indoor/nav/dijkstra/DijkstraPath.h> #include <Indoor/nav/dijkstra/DijkstraPath.h>
#include <Indoor/grid/Grid.h> #include <Indoor/grid/Grid.h>
#include "../nav/Node.h" #include "../nav/grid/Node.h"
class Path2D : public Renderable2D { class Path2D : public Renderable2D {

8
ui/map/3D/MapView3D.h
View File

@@ -16,7 +16,9 @@
#include "elements/ColorPoints.h" #include "elements/ColorPoints.h"
#include "elements/Object.h" #include "elements/Object.h"
#include "../nav/State.h" #include "../nav/Observation.h"
#include "../nav/grid/State.h"
namespace Floorplan { namespace Floorplan {
class IndoorMap; class IndoorMap;
@@ -122,11 +124,11 @@ public:
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
Q_INVOKABLE void showParticles(const void* particles) { Q_INVOKABLE void showParticles(const void* particles) {
showParticles((const std::vector<K::Particle<MyState>>*) particles); showParticles((const std::vector<K::Particle<GridBased::MyState>>*) particles);
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<K::Particle<MyState>>* particles) { void showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles) {
this->colorPoints->setFromParticles(*particles); this->colorPoints->setFromParticles(*particles);
} }

2
ui/map/3D/elements/ColorPoints.h
View File

@@ -11,7 +11,7 @@
#include "../Renderable.h" #include "../Renderable.h"
#include "../../../../Settings.h" #include "../../../../Settings.h"
#include "../../../../nav/Node.h" #include "../../../../nav/grid/Node.h"
class ColorPoints : public Renderable { class ColorPoints : public Renderable {

24
yasmin.pro
View File

@@ -66,6 +66,7 @@ SOURCES += \
main.cpp \ main.cpp \
lib/gpc/gpc.cpp \ lib/gpc/gpc.cpp \
../Indoor/lib/tinyxml/tinyxml2.cpp \ ../Indoor/lib/tinyxml/tinyxml2.cpp \
../Indoor/lib/Recast/*.cpp\
ui/menu/MainMenu.cpp \ ui/menu/MainMenu.cpp \
ui/MainWindow.cpp \ ui/MainWindow.cpp \
Controller.cpp \ Controller.cpp \
@@ -80,7 +81,10 @@ SOURCES += \
ui/map/3D/MapView3D.cpp \ ui/map/3D/MapView3D.cpp \
ui/map/2D/MapView2D.cpp \ ui/map/2D/MapView2D.cpp \
tools/calibration/WiFiCalibrationScanDialog.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 RESOURCES += qml.qrc
@@ -151,21 +155,16 @@ HEADERS += \
ui/debug/plot/PlottWidget.h \ ui/debug/plot/PlottWidget.h \
ui/debug/PlotTurns.h \ ui/debug/PlotTurns.h \
ui/debug/PlotWiFiScan.h \ ui/debug/PlotWiFiScan.h \
nav/State.h \
nav/Filter.h \
nav/Node.h \
sensors/linux/WiFiSensorLinuxC.h \ sensors/linux/WiFiSensorLinuxC.h \
sensors/offline/SensorFactoryOffline.h \ sensors/offline/SensorFactoryOffline.h \
sensors/dummy/SensorFactoryDummy.h \ sensors/dummy/SensorFactoryDummy.h \
sensors/android/SensorFactoryAndroid.h \ sensors/android/SensorFactoryAndroid.h \
Settings.h \ Settings.h \
nav/RegionalResampling.h \
sensors/offline/AllInOneSensor.h \ sensors/offline/AllInOneSensor.h \
sensors/ActivitySensor.h \ sensors/ActivitySensor.h \
ui/LoggerUI.h \ ui/LoggerUI.h \
ui/debug/InfoWidget.h \ ui/debug/InfoWidget.h \
ui/UIHelper.h \ ui/UIHelper.h \
nav/NodeResampling.h \
ui/map/3D/MapView3D.h \ ui/map/3D/MapView3D.h \
ui/map/2D/MapView2D.h \ ui/map/2D/MapView2D.h \
ui/map/2D/Floor2D.h \ ui/map/2D/Floor2D.h \
@@ -193,7 +192,18 @@ HEADERS += \
sensors/dummy/CompassSensorDummy.h \ sensors/dummy/CompassSensorDummy.h \
sensors/dummy/GPSSensorDummy.h \ sensors/dummy/GPSSensorDummy.h \
ui/debug/PlotGPS.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 += \ DISTFILES += \
android-sources/src/MyActivity.java \ android-sources/src/MyActivity.java \