290 lines
11 KiB
C++
290 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;
|
|
|
|
const Timestamp diff = curObs.currentTime - lastTransition;
|
|
if (curCtrl.numStepsSinceLastTransition > 0){
|
|
|
|
// 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;
|
|
|
|
const Timestamp ts1 = Timestamp::fromUnixTime();
|
|
filterUpdate();
|
|
const Timestamp ts2 = Timestamp::fromUnixTime();
|
|
const Timestamp tsDiff = ts2-ts1;
|
|
const QString filterTime = QString::number(diff.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;
|
|
MyState sCurEst = pf->update(&curCtrl, curObs);
|
|
curEst.pos_m = sCurEst.loc.pos;
|
|
curEst.head = sCurEst.heading;
|
|
|
|
// 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");
|
|
|
|
//TODO: Implement destination for Mesh
|
|
// 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));
|
|
}
|
|
}
|