FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

current revision

Browse Source
This commit is contained in:
FrankE
2016-09-28 12:16:45 +02:00
parent 075d8bb633
commit d47322e73b
90 changed files with 8228 additions and 606 deletions
Download Patch File Download Diff File Expand all files Collapse all files

150
nav/Filter.h
View File

@@ -12,18 +12,21 @@
#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/WalkModuleButterActivity.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>
class PFInit : public K::ParticleFilterInitializer<MyState> {
private:
@@ -42,17 +45,20 @@ public:
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 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);
// }
@@ -65,28 +71,34 @@ 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;
WalkModuleButterActivity<MyGridNode, MyState> modBarometer;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid, MyControl* ctrl) : grid(grid), modHeading(ctrl, Settings::turnSigma), modDestination(*grid) {
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
walker.addModule(&modFavorZ);
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
walker.addModule(&modImportance);
walker.addModule(&modBarometer);
walker.addModule(&modDestination);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
@@ -94,16 +106,52 @@ public:
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
std::normal_distribution<float> noise(0, Settings::stepSigma);
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
for (K::Particle<MyState>& p : particles) {
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::stepLength + noise(gen));
p.state = walker.getDestination(*grid, p.state, dist_m);
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
double probSum = 0;
// seems OK
// float sum = 0;
// for (int i = 0; i < 1000; ++i) {
// float val = noise(gen);
// sum += std::abs(val);
// }
//Log::add("123", "sum: " + std::to_string(sum));
//Log::add("123", std::to_string(Timestamp::fromRunningTime().ms()) + ": " + std::to_string(ctrl.numStepsSinceLastTransition));
//for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(2)
for (int i = 0; i < (int) particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
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
if (p.weight != p.weight) {throw Exception("nan");}
probSum += prob;
//p.weight = Distribution::Exponential<double>::getProbability(5.0, prob);
}
((MyControl*)control)->resetAfterTransition();
// const double avgProb = probSum / particles.size();
// const double threshold = avgProb * 0.15;
// for (int i = 0; i < (int) particles.size(); ++i) {
// K::Particle<MyState>& p = particles[i];
// p.weight = (p.weight > threshold) ? (1.0) : (0.01); // downvote all transitions below the threshold
// //p.weight = 1;
// }
}
@@ -111,12 +159,51 @@ public:
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
WiFiObserverFree wiFiProbability;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// how to perform VAP grouping. also see calibration in Controller.cpp
VAPGrouper vg = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public:
PFEval(WiFiModelLogDistCeiling& wifiModel) : wifiModel(wifiModel), wiFiProbability(Settings::wifiSigma, wifiModel) {
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 ActivityButterPressure::Activity act) {
const float kappa = 0.75;
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
case ActivityButterPressure::Activity::STAY:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case ActivityButterPressure::Activity::UP:
case ActivityButterPressure::Activity::DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
@@ -124,18 +211,31 @@ public:
double sum = 0;
// smartphone is 1.3 meter above ground
const Point3 person(0,0,Settings::smartphoneAboveGround);
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// vap-grouping
const WiFiMeasurements wifiObs = vg.group(_observation.wifi);
for (K::Particle<MyState>& p : particles) {
const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, observation.wifi);
// 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);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS;
p.weight = prob;
sum += prob;
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
sum += p.weight;
if (p.weight != p.weight) {throw Exception("nan");}
}
return sum;

291
nav/NavController.h
View File

@@ -10,7 +10,8 @@
#include "../sensors/TurnSensor.h"
#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/MapView.h"
#include "../ui/map/3D/MapView3D.h"
#include "../ui/debug/InfoWidget.h"
#include <Indoor/Assertions.h>
#include <thread>
@@ -18,24 +19,25 @@
#include "State.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"
Q_DECLARE_METATYPE(const void*)
class NavController :
public SensorListener<AccelerometerData>,
public SensorListener<GyroscopeData>,
@@ -43,7 +45,9 @@ class NavController :
public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>,
public SensorListener<StepData>,
public SensorListener<TurnData> {
public SensorListener<TurnData>,
public SensorListener<ActivityData> {
private:
@@ -56,106 +60,164 @@ private:
MyControl curCtrl;
bool running = false;
std::thread tUpdate;
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;
public:
virtual ~NavController() {
if (running) {stop();}
}
/** ctor */
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);
// 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.1));
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.10));
std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
//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));
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid, &curCtrl));
// 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(1.0);
pf->setNEffThreshold(0.75);
//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() {
Assert::isFalse(running, "already started!");
running = true;
tUpdate = std::thread(&NavController::update, this);
tDisplay = std::thread(&NavController::display, this);
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavController::filterUpdateLoop, this);
tDisplay = std::thread(&NavController::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 stop() {
Assert::isTrue(running, "not started!");
running = false;
tUpdate.join();
tFilter.join();
tDisplay.join();
}
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor;
curObs.currentTime = ts;
(void) data;
(void) ts;
gotSensorData(ts);
}
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
(void) sensor;
curObs.currentTime = ts;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
(void) sensor;
curObs.currentTime = ts;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor;
(void) ts;
curObs.currentTime = ts;
curObs.wifi = data;
gotSensorData(ts);
}
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
(void) sensor;
(void) ts;
curObs.currentTime = ts;
curObs.gps = data;
gotSensorData(ts);
}
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
gotSensorData(ts);
}
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
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);
}
int cameraMode = 0;
@@ -165,12 +227,29 @@ public:
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 update() {
void filterUpdateLoop() {
Timestamp lastTransition;
while(running) {
while(running && !Settings::Filter::useMainThread) {
// // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -180,82 +259,94 @@ private:
// 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));
}
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; std::cout << "ts:" << curObs.currentTime << " avg:" << (avgSum/avgCount) << std::endl;
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;
} else {
return false;
}
}
MyState curEst;
//MyState lastEst;
DijkstraPath<MyGridNode> pathToDest;
void doUpdate() {
/** 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());
// 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");
// 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) {
const DijkstraPath<MyGridNode> path = trans->modDestination.getShortestPath(*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();
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;
const int display_ms = Settings::MapView::msPerFrame.ms();
/** UI update loop */
void display() {
void updateMapViewLoop() {
while(running) {
doDisplay();
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));
}
}
@@ -264,8 +355,8 @@ private:
Point3 curPosSlow;
void doDisplay() {
/** 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;
@@ -278,19 +369,21 @@ private:
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->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
} else if (cameraMode == 1) {
mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
mainController->getMapView3D()->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 spectator = curPosFast + Point3(0,0,25) - dir*15;
const Point3 spectatorDir = (curPosFast - spectator).normalized();
mainController->getMapView()->setLookEye(spectator);
mainController->getMapView()->setLookDir(spectatorDir);
mainController->getMapView3D()->setLookEye(spectator);
mainController->getMapView3D()->setLookDir(spectatorDir);
}
mainController->getMapView()->setCurrentEstimation(curPosFast, dir);
mainController->getMapView3D()->setClipAbove(curEst.position.inMeter().z + 2);
mainController->getMapView3D()->setCurrentEstimation(curEst.position.inMeter(), dir);
mainController->getMapView2D()->setCurrentEstimation(curEst.position.inMeter(), dir);
}

15
nav/NavControllerListener.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef NAVCONTROLLERLISTENER_H
#define NAVCONTROLLERLISTENER_H
#include <Indoor/geo/Point3.h>
class NavControllerListener {
public:
/** a new position estimation is available */
virtual void onNewEstimation(const Point3 pos_m) = 0;
};
#endif // NAVCONTROLLERLISTENER_H

10
nav/Node.h
View File

@@ -4,11 +4,15 @@
#include <Indoor/grid/Grid.h>
#include <Indoor/sensors/radio/WiFiGridNode.h>
struct MyGridNode : public GridNode, public GridPoint {//, public WiFiGridNode<10> {
struct MyGridNode : public GridNode, public GridPoint, public WiFiGridNode<20> {
float navImportance;
float getNavImportance() const { return navImportance; }
float walkImportance;
float getWalkImportance() const { return walkImportance; }
/** empty ctor */
MyGridNode() : GridPoint(-1, -1, -1) {;}
@@ -17,11 +21,11 @@ struct MyGridNode : public GridNode, public GridPoint {//, public WiFiGridNode<1
static void staticDeserialize(std::istream& inp) {
//WiFiGridNode::staticDeserialize(inp);
WiFiGridNode::staticDeserialize(inp);
}
static void staticSerialize(std::ostream& out) {
//WiFiGridNode::staticSerialize(out);
WiFiGridNode::staticSerialize(out);
}
};

123
nav/NodeResampling.h Normal file
View File

@@ -0,0 +1,123 @@
#ifndef NODERESAMPLING_H
#define NODERESAMPLING_H
#include <algorithm>
#include <random>
#include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h>
/**
* uses simple probability resampling by drawing particles according
* to their current weight.
* HOWEVER: after drawing them, do NOT use them directly, but replace them with a neighbor
* O(log(n)) per particle
*/
template <typename State, typename Node>
class NodeResampling : public K::ParticleFilterResampling<State> {
private:
/** this is a copy of the particle-set to draw from it */
std::vector<K::Particle<State>> particlesCopy;
/** random number generator */
std::minstd_rand gen;
Grid<Node>& grid;
public:
/** ctor */
NodeResampling(Grid<Node>& grid) : grid(grid) {
gen.seed(1234);
}
void resample(std::vector<K::Particle<State>>& particles) override {
// compile-time sanity checks
// TODO: this solution requires EXPLICIT overloading which is bad...
//static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
const uint32_t cnt = (uint32_t) particles.size();
// equal weight for all particles. sums up to 1.0
const double equalWeight = 1.0 / (double) cnt;
// ensure the copy vector has the same size as the real particle vector
particlesCopy.resize(cnt);
// swap both vectors
particlesCopy.swap(particles);
// calculate cumulative weight
double cumWeight = 0;
for (uint32_t i = 0; i < cnt; ++i) {
cumWeight += particlesCopy[i].weight;
particlesCopy[i].weight = cumWeight;
}
// std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
// std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
std::normal_distribution<float> distTurn(0.0, +0.03);
// now draw from the copy vector and fill the original one
// with the resampled particle-set
for (uint32_t i = 0; i < cnt; ++i) {
// slight chance to get a truely random node as particle
// mainly for testing
// if (distNewOne(gen) < 0.005) {
// particles[i].state.position = grid[distRndNode(gen)];
// particles[i].weight = equalWeight;
// continue;
// }
// normal redraw procedure
particles[i] = draw(cumWeight);
particles[i].weight = equalWeight;
const Node* n = grid.getNodePtrFor(particles[i].state.position);
if (n == nullptr) {continue;} // should not happen!
for (int j = 0; j < 2; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &grid.getNeighbor(*n, idx);
}
particles[i].state.position = *n;
particles[i].state.heading.direction += distTurn(gen);
}
}
private:
/** draw one particle according to its weight from the copy vector */
const K::Particle<State>& draw(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<State>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
return *it;
}
};
#endif // NODERESAMPLING_H

12
nav/State.h
View File

@@ -2,7 +2,7 @@
#define STATE_H
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleButterActivity.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>
@@ -10,7 +10,7 @@
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/gps/GPSData.h>
struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading, public WalkStateBarometerActivity {
struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading {
/** ctor */
@@ -52,6 +52,10 @@ struct MyObservation {
/** gps measurements */
GPSData gps;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
/** time of evaluation */
Timestamp currentTime;
@@ -66,6 +70,10 @@ struct MyControl {
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
ActivityButterPressure::Activity activity;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;