625f5fe04d
removed KLib stuff added new activity filter is uncommand! at the moment, the app is not able to load new maps and breaks using old maps
276 lines
10 KiB
C++
276 lines
10 KiB
C++
#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<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
|
|
|
|
// estimation
|
|
//std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MyState>());
|
|
std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
|
|
|
|
// resampling
|
|
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
|
|
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
|
|
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::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<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
|
|
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
|
|
|
|
// setup the filter
|
|
pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::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);
|
|
}
|
|
|
|
void GridBased::NavControllerGrid::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 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));
|
|
}
|
|
}
|
|
|