refactoring to add nav mesh
This commit is contained in:
k-a-z-u
16
nav/CurEst.h
Normal file
16
nav/CurEst.h
Normal 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
248
nav/Filter.h
@@ -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
46
nav/NavController.cpp
Normal 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);
|
||||
|
||||
}
|
||||
@@ -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 <Indoor/Assertions.h>
|
||||
#include <thread>
|
||||
|
||||
#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 <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 "CurEst.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"
|
||||
|
||||
Q_DECLARE_METATYPE(const void*)
|
||||
|
||||
|
||||
class Controller;
|
||||
|
||||
class NavController :
|
||||
public SensorListener<AccelerometerData>,
|
||||
@@ -50,344 +53,55 @@ class NavController :
|
||||
{
|
||||
|
||||
|
||||
private:
|
||||
protected:
|
||||
|
||||
Controller* mainController;
|
||||
Grid<MyGridNode>* grid;
|
||||
WiFiModelLogDistCeiling wifiModel;
|
||||
Floorplan::IndoorMap* im;
|
||||
|
||||
MyObservation curObs;
|
||||
MyControl curCtrl;
|
||||
|
||||
bool running = false;
|
||||
std::thread tFilter;
|
||||
std::thread tDisplay;
|
||||
|
||||
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
|
||||
|
||||
/** the estimated path */
|
||||
std::vector<Point3> estPath;
|
||||
|
||||
/** all listeners */
|
||||
std::vector<NavControllerListener*> 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<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 */
|
||||
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 <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);
|
||||
// }
|
||||
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<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) */
|
||||
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();
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -1,47 +1,9 @@
|
||||
#ifndef 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/gps/GPSData.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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
#include <Indoor/sensors/activity/Activity.h>
|
||||
|
||||
/** observed sensor data */
|
||||
struct MyObservation {
|
||||
@@ -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
253
nav/grid/Filter.h
Normal 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
266
nav/grid/NavControllerGrid.cpp
Normal 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
89
nav/grid/NavControllerGrid.h
Normal 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
|
||||
72
nav/grid/RegionalResampling.h
Normal file
72
nav/grid/RegionalResampling.h
Normal 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
44
nav/grid/State.h
Normal 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
220
nav/mesh/FilterMesh.h
Normal 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
290
nav/mesh/NavControllerMesh.cpp
Normal 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
88
nav/mesh/NavControllerMesh.h
Normal 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
45
nav/mesh/State.h
Normal 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
|
||||
Reference in New Issue
Block a user