a lot!!! of changes
added main menu added debug display many debug widgets for plotting live data worked on android live sensors added offline-data sensor feeding some dummy data sensors worked on the map display added ui debug for grid-points, particles and weights added a cool dude to display the estimation added real filtering based on the Indoor components c++11 fixes for android compilation online and offline filtering support new resampling technique for testing map loading via dialog
This commit is contained in:
kazu
299
nav/NavController.h
Normal file
299
nav/NavController.h
Normal file
@@ -0,0 +1,299 @@
|
||||
#ifndef NAVCONTROLLER_H
|
||||
#define NAVCONTROLLER_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 "../ui/debug/SensorDataWidget.h"
|
||||
#include "../ui/map/MapView.h"
|
||||
|
||||
#include <Indoor/Assertions.h>
|
||||
#include <thread>
|
||||
|
||||
#include "State.h"
|
||||
#include "Filter.h"
|
||||
#include "Controller.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"
|
||||
|
||||
Q_DECLARE_METATYPE(const void*)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class NavController :
|
||||
public SensorListener<AccelerometerData>,
|
||||
public SensorListener<GyroscopeData>,
|
||||
public SensorListener<BarometerData>,
|
||||
public SensorListener<WiFiMeasurements>,
|
||||
public SensorListener<GPSData>,
|
||||
public SensorListener<StepData>,
|
||||
public SensorListener<TurnData> {
|
||||
|
||||
private:
|
||||
|
||||
Controller* mainController;
|
||||
Grid<MyGridNode>* grid;
|
||||
WiFiModelLogDistCeiling wifiModel;
|
||||
Floorplan::IndoorMap* im;
|
||||
|
||||
MyObservation curObs;
|
||||
MyControl curCtrl;
|
||||
|
||||
bool running = false;
|
||||
std::thread tUpdate;
|
||||
std::thread tDisplay;
|
||||
|
||||
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
|
||||
|
||||
public:
|
||||
|
||||
virtual ~NavController() {
|
||||
if (running) {stop();}
|
||||
}
|
||||
|
||||
NavController(Controller* mainController, Grid<MyGridNode>* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) {
|
||||
|
||||
wifiModel.loadAPs(im, Settings::wifiTXP, Settings::wifiEXP, Settings::wifiWAF);
|
||||
|
||||
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);
|
||||
|
||||
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
|
||||
|
||||
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
|
||||
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.1));
|
||||
|
||||
//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
|
||||
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.10));
|
||||
std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
|
||||
|
||||
|
||||
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(wifiModel));
|
||||
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid, &curCtrl));
|
||||
|
||||
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(1.0);
|
||||
|
||||
}
|
||||
|
||||
void start() {
|
||||
Assert::isFalse(running, "already started!");
|
||||
running = true;
|
||||
tUpdate = std::thread(&NavController::update, this);
|
||||
tDisplay = std::thread(&NavController::display, this);
|
||||
}
|
||||
|
||||
void stop() {
|
||||
Assert::isTrue(running, "not started!");
|
||||
running = false;
|
||||
tUpdate.join();
|
||||
tDisplay.join();
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
|
||||
(void) sensor;
|
||||
curObs.currentTime = ts;
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
|
||||
(void) sensor;
|
||||
curObs.currentTime = ts;
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
|
||||
(void) sensor;
|
||||
curObs.currentTime = ts;
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
|
||||
(void) sensor;
|
||||
(void) ts;
|
||||
curObs.currentTime = ts;
|
||||
curObs.wifi = data;
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
|
||||
(void) sensor;
|
||||
(void) ts;
|
||||
curObs.currentTime = ts;
|
||||
curObs.gps = data;
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override {
|
||||
(void) sensor;
|
||||
(void) ts;
|
||||
curObs.currentTime = ts;
|
||||
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
|
||||
}
|
||||
|
||||
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override {
|
||||
(void) sensor;
|
||||
(void) ts;
|
||||
curObs.currentTime = ts;
|
||||
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
|
||||
}
|
||||
|
||||
int cameraMode = 0;
|
||||
void toggleCamera() {
|
||||
cameraMode = (cameraMode + 1) % 3;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** particle-filter update loop */
|
||||
void update() {
|
||||
|
||||
Timestamp lastTransition;
|
||||
|
||||
while(running) {
|
||||
|
||||
// // 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()));
|
||||
|
||||
// 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 > Timestamp::fromMS(500)) {
|
||||
doUpdate();
|
||||
lastTransition = curObs.currentTime;
|
||||
} else {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
MyState curEst;
|
||||
//MyState lastEst;
|
||||
|
||||
void doUpdate() {
|
||||
|
||||
//lastEst = curEst;
|
||||
curEst = pf->update(&curCtrl, curObs);
|
||||
|
||||
// hacky.. but we need to call this one from the main thread!
|
||||
//mainController->getMapView()->showParticles(pf->getParticles());
|
||||
qRegisterMetaType<const void*>();
|
||||
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
|
||||
|
||||
|
||||
PFTrans* trans = (PFTrans*)pf->getTransition();
|
||||
const MyGridNode* node = grid->getNodePtrFor(curEst.position);
|
||||
if (node) {
|
||||
const DijkstraPath<MyGridNode> path = trans->modDestination.getShortestPath(*node);
|
||||
// mainController->getMapView()->showGridImportance();
|
||||
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "setPath", Qt::QueuedConnection, Q_ARG(const void*, &path)), "call failed");
|
||||
}
|
||||
|
||||
/*
|
||||
static K::Gnuplot gp;
|
||||
K::GnuplotSplot plot;
|
||||
K::GnuplotSplotElementLines lines; plot.add(&lines);
|
||||
K::GnuplotSplotElementPoints points; plot.add(&points);
|
||||
K::GnuplotSplotElementPoints best; plot.add(&best); best.setPointSize(2); best.setColorHex("#0000ff");
|
||||
|
||||
for (const K::Particle<MyState>& p : pf->getParticles()) {
|
||||
const Point3 pos = p.state.position.inMeter();
|
||||
points.add(K::GnuplotPoint3(pos.x, pos.y, pos.z));
|
||||
}
|
||||
|
||||
for (const Floorplan::Floor* f : im->floors) {
|
||||
for (const Floorplan::FloorOutlinePolygon* polygon : f->outline) {
|
||||
for (int i = 0; i < polygon->poly.points.size(); ++i) {
|
||||
const Point2 p1 = polygon->poly.points[i];
|
||||
const Point2 p2 = polygon->poly.points[(i+1)%polygon->poly.points.size()];
|
||||
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
|
||||
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
|
||||
lines.addSegment(gp1, gp2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
K::GnuplotPoint3 gpBest(curEst.position.x_cm/100.0f, curEst.position.y_cm/100.0f, curEst.position.z_cm/100.0f);
|
||||
best.add(gpBest);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
*/
|
||||
|
||||
|
||||
}
|
||||
|
||||
const int display_ms = 50;
|
||||
|
||||
/** UI update loop */
|
||||
void display() {
|
||||
while(running) {
|
||||
doDisplay();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
|
||||
}
|
||||
}
|
||||
|
||||
Point3 curPosFast;
|
||||
Point3 curPosSlow;
|
||||
|
||||
|
||||
|
||||
void doDisplay() {
|
||||
|
||||
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();
|
||||
|
||||
if (cameraMode == 0) {
|
||||
mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
|
||||
} else if (cameraMode == 1) {
|
||||
mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
|
||||
} else if (cameraMode == 2) {
|
||||
const Point3 spectator = curPosFast + Point3(0,0,20) - dir*15;
|
||||
const Point3 spectatorDir = (curPosFast - spectator).normalized();
|
||||
mainController->getMapView()->setLookEye(spectator);
|
||||
mainController->getMapView()->setLookDir(spectatorDir);
|
||||
}
|
||||
|
||||
mainController->getMapView()->setCurrentEstimation(curPosFast, dir);
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // NAVCONTROLLER_H
|
||||
Reference in New Issue
Block a user