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
289 lines
11 KiB
C++
289 lines
11 KiB
C++
#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, WiFiModel* wifiModel) :
|
|
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
|
|
|
|
// filter init
|
|
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
|
|
|
|
// estimation
|
|
std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>());
|
|
//std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
|
|
|
|
// resampling
|
|
//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<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
|
|
|
|
// eval and transition
|
|
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
|
|
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
|
|
|
|
// 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);
|
|
|
|
// 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 MeshBased::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 Activity::STANDING: act = "STAY"; break;
|
|
// case Activity::WALKING_DOWN: act = "DOWN"; break;
|
|
// case Activity::WALKING_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));
|
|
}
|
|
}
|