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

updated sensors and filter to current code version

Browse Source 
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
This commit is contained in:
toni
2018-07-12 18:39:27 +02:00
parent b4a1a3d969
commit 625f5fe04d
22 changed files with 325 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
Controller.cpp
View File

@@ -20,6 +20,7 @@
#include <Indoor/sensors/radio/WiFiGridEstimator.h> #include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h> #include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/model/WiFiModel.h>
#include <Indoor/Assertions.h> #include <Indoor/Assertions.h>
@@ -189,6 +190,55 @@ void buildGridOnce(Grid<MyGridNode>* grid, Floorplan::IndoorMap* map, const std:
} }
void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& wifiModelFile){
// ask questions
const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "WiFi", "Use Fingerprints for WiFiCalibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use APs from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No);
// WiFi setup
wifiModel = new WiFiModelLogDistCeiling(map);
if (replyWiFiFP == QMessageBox::Yes) {
std::ifstream inp(wifiModelFile, std::ifstream::binary);
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Log::add("Controller", "Create new WiFiModel");
WiFiCalibrationDataModel mdl(fpFile);
Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : mdl.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry);
}
wifiModel->saveXML(wifiModelFile);
} else {
Log::add("Controller", "Use existing WifiModel");
// load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map);
wifiModel = fac.loadXML(wifiModelFile);
}
} else {
Log::add("Controller", "Read AP from File");
// load all APs from the floorplan and use same TXP/EXP/WAF for all of them
((WiFiModelLogDistCeiling*)wifiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(wifiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}
}
void Controller::on3DButton() { void Controller::on3DButton() {
static bool use3D = false; static bool use3D = false;
@@ -266,6 +316,7 @@ void Controller::loadNavMesh(QDir dir) {
QFile fMap(dir.path() + "/map.xml"); QFile fMap(dir.path() + "/map.xml");
QFile fGrid(dir.path() + "/grid.dat"); QFile fGrid(dir.path() + "/grid.dat");
QFile fWiFiFP(dir.path() + "/wifi_fp.dat"); QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
QFile fWiFiModel(dir.path() + "/wifimodel.dat");
Assert::isTrue(fMap.exists(), "map.xml missing"); Assert::isTrue(fMap.exists(), "map.xml missing");
@@ -274,6 +325,9 @@ void Controller::loadNavMesh(QDir dir) {
im = Floorplan::Reader::readFromString(str.toStdString()); im = Floorplan::Reader::readFromString(str.toStdString());
const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); const std::string sWiFiFP = fWiFiFP.fileName().toStdString();
const std::string sWiFiModel = fWiFiModel.fileName().toStdString();
buildWiFiModelOnce(wifiModel, im, sWiFiFP, sWiFiModel);
// create navmesh // create navmesh
if (navMesh) {delete navMesh; navMesh = nullptr;} if (navMesh) {delete navMesh; navMesh = nullptr;}
@@ -283,7 +337,7 @@ void Controller::loadNavMesh(QDir dir) {
// create a new navigator // create a new navigator
if (nav) {delete nav; nav = nullptr;} if (nav) {delete nav; nav = nullptr;}
nav = new MeshBased::NavControllerMesh(this, im, navMesh); nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP); WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);

2
Controller.h
View File

@@ -21,6 +21,7 @@ class MyGridNode;
class InfoWidget; class InfoWidget;
class MapView3D; class MapView3D;
class MapView2D; class MapView2D;
class WiFiModel;
namespace NM { namespace NM {
template <typename T> class NavMesh; template <typename T> class NavMesh;
@@ -78,6 +79,7 @@ private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh = nullptr; NM::NavMesh<NM::NavMeshTriangle>* navMesh = nullptr;
NavController* nav = nullptr; NavController* nav = nullptr;
Floorplan::IndoorMap* im = nullptr; Floorplan::IndoorMap* im = nullptr;
WiFiModel* wifiModel = nullptr;
}; };

2
Settings.h
View File

@@ -39,7 +39,7 @@ namespace Settings {
namespace WiFiModel { namespace WiFiModel {
constexpr float sigma = 13.0; constexpr float sigma = 13.0; //TODO: im Museum hatten wir 8.0
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -48; constexpr float TXP = -48;

4
nav/NavController.h
View File

@@ -48,8 +48,8 @@ class NavController :
public SensorListener<WiFiMeasurements>, public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>, public SensorListener<GPSData>,
public SensorListener<StepData>, public SensorListener<StepData>,
public SensorListener<TurnData> public SensorListener<TurnData>,
//public SensorListener<ActivityData> public SensorListener<ActivityData>
{ {

39
nav/grid/Filter.h
View File

@@ -1,13 +1,18 @@
#ifndef FILTER_H #ifndef FILTER_H
#define FILTER_H #define FILTER_H
#include <KLib/math/filter/particles/ParticleFilter.h> #include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h> #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h> #include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
@@ -31,7 +36,7 @@
namespace GridBased { namespace GridBased {
class PFInit : public K::ParticleFilterInitializer<MyState> { class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private: private:
@@ -43,14 +48,14 @@ namespace GridBased {
} }
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override { virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1); std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
std::uniform_real_distribution<float> distHead(0, 2*M_PI); std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) { for (SMC::Particle<MyState>& p : particles) {
const int idx = distIdx(gen); const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx]; const MyGridNode& node = (*grid)[idx];
p.state.position = node; // random position p.state.position = node; // random position
@@ -59,7 +64,7 @@ namespace GridBased {
} }
// // fix position + heading // // fix position + heading
// for (K::Particle<MyState>& p : particles) { // for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000; //// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx]; //// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
@@ -71,7 +76,7 @@ namespace GridBased {
}; };
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> { class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public: public:
@@ -110,7 +115,7 @@ namespace GridBased {
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl; this->ctrl = *_ctrl;
@@ -121,14 +126,14 @@ namespace GridBased {
// sanity check // sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) { //for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic //#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
double prob; double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob); p.state = walker.getDestination(*grid, p.state, dist_m, prob);
@@ -147,7 +152,7 @@ namespace GridBased {
}; };
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> { class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
@@ -170,7 +175,7 @@ namespace GridBased {
} }
double getStairProb(const K::Particle<MyState>& p, const Activity act) { double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75; const float kappa = 0.75;
@@ -195,7 +200,7 @@ namespace GridBased {
} }
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override { double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0; double sum = 0;
@@ -215,7 +220,7 @@ namespace GridBased {
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
// WiFi free // WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi)); //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));

27
nav/grid/NavControllerGrid.cpp
View File

@@ -21,25 +21,25 @@ Q_DECLARE_METATYPE(const void*)
GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) {
// filter init // filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::unique_ptr<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation // estimation
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>()); //std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)); std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling // resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid)); 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<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05)); //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling()); //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition // eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); 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<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid)); std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter // setup the filter
pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init))); 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->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval)); pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation)); pf->setEstimation(std::move(estimation));
@@ -56,7 +56,7 @@ GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floo
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getTurns().addListener(this);
//SensorFactory::get().getActivity().addListener(this); SensorFactory::get().getActivity().addListener(this);
} }
@@ -141,6 +141,15 @@ void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const
gotSensorData(ts); 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 */ /** called when any sensor has received new data */
void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts; curObs.currentTime = ts;

4
nav/grid/NavControllerGrid.h
View File

@@ -28,7 +28,7 @@ namespace GridBased {
Grid<MyGridNode>* grid; Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel; WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf; std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest; DijkstraPath<MyGridNode> pathToDest;
@@ -60,7 +60,7 @@ namespace GridBased {
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ; void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private: private:

12
nav/grid/NodeResampling.h
View File

@@ -6,7 +6,7 @@
#include <random> #include <random>
#include <Indoor/grid/Grid.h> #include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
/** /**
@@ -16,12 +16,12 @@
* O(log(n)) per particle * O(log(n)) per particle
*/ */
template <typename State, typename Node> template <typename State, typename Node>
class NodeResampling : public K::ParticleFilterResampling<State> { class NodeResampling : public SMC::ParticleFilterResampling<State> {
private: private:
/** this is a copy of the particle-set to draw from it */ /** this is a copy of the particle-set to draw from it */
std::vector<K::Particle<State>> particlesCopy; std::vector<SMC::Particle<State>> particlesCopy;
/** random number generator */ /** random number generator */
std::minstd_rand gen; std::minstd_rand gen;
@@ -35,7 +35,7 @@
gen.seed(1234); gen.seed(1234);
} }
void resample(std::vector<K::Particle<State>>& particles) override { void resample(std::vector<SMC::Particle<State>>& particles) override {
// compile-time sanity checks // compile-time sanity checks
// TODO: this solution requires EXPLICIT overloading which is bad... // TODO: this solution requires EXPLICIT overloading which is bad...
@@ -99,7 +99,7 @@
private: private:
/** draw one particle according to its weight from the copy vector */ /** draw one particle according to its weight from the copy vector */
const K::Particle<State>& draw(const double cumWeight) { const SMC::Particle<State>& draw(const double cumWeight) {
// generate random values between [0:cumWeight] // generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, cumWeight); std::uniform_real_distribution<float> dist(0, cumWeight);
@@ -108,7 +108,7 @@
const float rand = dist(gen); const float rand = dist(gen);
// search comparator (cumWeight is ordered -> use binary search) // search comparator (cumWeight is ordered -> use binary search)
auto comp = [] (const K::Particle<State>& s, const float d) {return s.weight < d;}; auto comp = [] (const SMC::Particle<State>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
return *it; return *it;

20
nav/grid/RegionalResampling.h
View File

@@ -1,12 +1,12 @@
#ifndef REGIONALRESAMPLING_H #ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H #define REGIONALRESAMPLING_H
#include <KLib/math/filter/particles/ParticleFilter.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
#include "State.h" #include "State.h"
namespace GridBased { namespace GridBased {
class RegionalResampling : public K::ParticleFilterResampling<MyState> { class RegionalResampling : public SMC::ParticleFilterResampling<MyState> {
public: public:
@@ -14,25 +14,25 @@ namespace GridBased {
RegionalResampling() {;} RegionalResampling() {;}
void resample(std::vector<K::Particle<MyState>>& particles) override { void resample(std::vector<SMC::Particle<MyState>>& particles) override {
Point3 sum; Point3 sum;
for (const K::Particle<MyState>& p : particles) { for (const SMC::Particle<MyState>& p : particles) {
sum += p.state.position.inMeter(); sum += p.state.position.inMeter();
} }
const Point3 avg = sum / particles.size(); const Point3 avg = sum / particles.size();
std::vector<K::Particle<MyState>> next; std::vector<SMC::Particle<MyState>> next;
for (const K::Particle<MyState>& p : particles) { for (const SMC::Particle<MyState>& p : particles) {
const float dist = p.state.position.inMeter().getDistance(avg); const float dist = p.state.position.inMeter().getDistance(avg);
if (rand() % 6 != 0) {continue;} if (rand() % 6 != 0) {continue;}
if (dist < maxDist) {next.push_back(p);} if (dist < maxDist) {next.push_back(p);}
} }
// cumulate // cumulate
std::vector<K::Particle<MyState>> copy = particles; std::vector<SMC::Particle<MyState>> copy = particles;
double cumWeight = 0; double cumWeight = 0;
for ( K::Particle<MyState>& p : copy) { for ( SMC::Particle<MyState>& p : copy) {
cumWeight += p.weight; cumWeight += p.weight;
p.weight = cumWeight; p.weight = cumWeight;
} }
@@ -50,7 +50,7 @@ namespace GridBased {
std::minstd_rand gen; std::minstd_rand gen;
/** draw one particle according to its weight from the copy vector */ /** draw one particle according to its weight from the copy vector */
const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) { const SMC::Particle<MyState>& draw(std::vector<SMC::Particle<MyState>>& copy, const double cumWeight) {
// generate random values between [0:cumWeight] // generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, cumWeight); std::uniform_real_distribution<float> dist(0, cumWeight);
@@ -59,7 +59,7 @@ namespace GridBased {
const float rand = dist(gen); const float rand = dist(gen);
// search comparator (cumWeight is ordered -> use binary search) // search comparator (cumWeight is ordered -> use binary search)
auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;}; auto comp = [] (const SMC::Particle<MyState>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
return *it; return *it;

189
nav/mesh/FilterMesh.h
View File

@@ -1,13 +1,18 @@
#ifndef FILTERMESH_ #ifndef FILTERMESH_
#define FILTERMESH_ #define FILTERMESH_
#include <KLib/math/filter/particles/ParticleFilter.h> #include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h> #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h> #include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
@@ -18,6 +23,7 @@
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h> #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include "State.h" #include "State.h"
#include "../Observation.h"
#include "../../Settings.h" #include "../../Settings.h"
#include <omp.h> #include <omp.h>
@@ -25,33 +31,33 @@
namespace MeshBased { namespace MeshBased {
class PFInit : public K::ParticleFilterInitializer<MyState> { class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private: private:
NM::NavMesh<NM::NavMeshTriangle>* mesh; const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public: public:
PFInit(NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) { PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
} }
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override { virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI); std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom(); NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (K::Particle<MyState>& p : particles) { for (SMC::Particle<MyState>& p : particles) {
p.state.loc = rnd.draw(); p.state.loc = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight p.weight = 1.0 / particles.size(); // equal weight
} }
// // fix position + heading // // fix position + heading
// for (K::Particle<MyState>& p : particles) { // for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000; //// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx]; //// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap // const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
@@ -63,107 +69,109 @@ namespace MeshBased {
}; };
/*
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> { class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public: public:
using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
// local, static control-data COPY // local, static control-data COPY
MyControl ctrl; MyControl ctrl;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public: public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) { PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
//walker.addModule(&modFavorZ); // how to evaluate drawn points
walker.addModule(&modHeading); //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addModule(&modImportance); //walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
walker.addModule(&modActivity); //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
} }
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!) // local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl; this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition(); ((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma); // walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
//#pragma omp atomic // how to walk
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen)); NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.loc;
K::Particle<MyState>& p = particles[i]; params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
double prob; if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
p.state = walker.getDestination(*grid, p.state, dist_m, prob); params.stepSizes.stepSizeFloor_m = 0.1;
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15); params.stepSizes.stepSizeStair_m = 0.1;
//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
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state
p.weight *= res.probability;
p.state.loc = res.location;
p.state.heading = res.heading;
} }
} }
}; };
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid; class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModelLogDistCeiling& wifiModel; WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
//WiFiObserverFree wiFiProbability; // free-calculation const float kappa = 0.75;
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground switch (act) {
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
case Activity::WALKING:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
public: public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) : //TODO: Was ist hier besser? Im Museum hatten wir das unterste.
grid(grid), wifiModel(wifiModel), //PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free //PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
}
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0; double sum = 0;
@@ -171,41 +179,27 @@ namespace MeshBased {
const MyObservation observation = _observation; const MyObservation observation = _observation;
// vap-grouping // vap-grouping
const int numAP1 = observation.wifi.entries.size(); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check // sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!"); Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// assign weights
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) { for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
K::Particle<MyState>& p = particles[i];
// 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);
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
const double pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity); const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1; const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
p.weight *= prob; // NOTE: keeps the weight returned by the transition step! const double prob = pWifi * pStair * pGPS;
//p.weight = prob; // does NOT keep the weights returned by the transition step
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");} if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic #pragma omp atomic
sum += p.weight; sum += p.weight;
} }
return sum; return sum;
@@ -214,7 +208,6 @@ namespace MeshBased {
}; };
*/
} }
#endif // FILTERMESH_ #endif // FILTERMESH_

70
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,37 +31,35 @@
Q_DECLARE_METATYPE(const void*) Q_DECLARE_METATYPE(const void*)
/** ctor */ /** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh) : MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(im) { NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init // filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new MeshBased::PFInit(navMesh)); std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// // estimation // estimation
// //std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>()); std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>());
// std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)); //std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// // resampling // 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<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>()); //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05));
// //std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05)); std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// // eval and transition // eval and transition
// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
// std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel)); std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// // setup the filter // setup the filter
// pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init))); 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->setTransition(std::move(transition));
// pf->setEvaluation(std::move(eval)); pf->setEvaluation(std::move(eval));
// pf->setEstimation(std::move(estimation)); pf->setEstimation(std::move(estimation));
// pf->setResampling(std::move(resample)); pf->setResampling(std::move(resample));
// pf->setNEffThreshold(0.85); //before 0.75, edit by toni pf->setNEffThreshold(0.85); //before 0.75, edit by toni
// //pf->setNEffThreshold(0.65); // still too low? //pf->setNEffThreshold(0.65); // still too low?
// //pf->setNEffThreshold(0.25); // too low //pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors // attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this); SensorFactory::get().getAccelerometer().addListener(this);
@@ -70,7 +68,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getTurns().addListener(this);
//SensorFactory::get().getActivity().addListener(this); SensorFactory::get().getActivity().addListener(this);
// hacky.. but we need to call this one from the main thread! // hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles()); //mainController->getMapView()->showParticles(pf->getParticles());
@@ -161,14 +159,14 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const
gotSensorData(ts); gotSensorData(ts);
} }
// void NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) { void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
// (void) sensor; (void) sensor;
// (void) ts; (void) ts;
// curCtrl.activity = data.curActivity; curCtrl.activity = data.curActivity;
// curObs.activity = data.curActivity; curObs.activity = data.curActivity;
// debugActivity(data.curActivity); //debugActivity(data.curActivity);
// gotSensorData(ts); gotSensorData(ts);
// } }
/** called when any sensor has received new data */ /** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
@@ -179,9 +177,9 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
// void debugActivity(const ActivityData& activity) { // void debugActivity(const ActivityData& activity) {
// QString act; // QString act;
// switch(activity.curActivity) { // switch(activity.curActivity) {
// case ActivityButterPressure::Activity::STAY: act = "STAY"; break; // case Activity::STANDING: act = "STAY"; break;
// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; // case Activity::WALKING_DOWN: act = "DOWN"; break;
// case ActivityButterPressure::Activity::UP: act = "UP"; break; // case Activity::WALKING_UP: act = "UP"; break;
// default: act = "???"; break; // default: act = "???"; break;
// } // }
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); // Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");

9
nav/mesh/NavControllerMesh.h
View File

@@ -8,6 +8,7 @@
#include "../sensors/SensorFactory.h" #include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h" #include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h" #include "../sensors/TurnSensor.h"
#include "../sensors/ActivitySensor.h"
#include <Indoor/navMesh/NavMeshLocation.h> #include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h> #include <Indoor/navMesh/NavMesh.h>
@@ -28,16 +29,16 @@ namespace MeshBased {
private: private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh; NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModelLogDistCeiling wifiModel; WiFiModel* wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf; std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs; MyObservation curObs;
MyControl curCtrl; MyControl curCtrl;
public: public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh); NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
void start() override; void start() override;
@@ -59,7 +60,7 @@ namespace MeshBased {
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override; void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ; void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private: private:

28
nav/mesh/State.h
View File

@@ -22,24 +22,26 @@ namespace MeshBased {
; ;
} }
// MyState& operator += (const MyState& o) { MyState& operator += (const MyState& o) {
// position += o.position; loc.pos += o.loc.pos;
// return *this; return *this;
// } }
// MyState& operator /= (const float val) { MyState& operator /= (const float val) {
// position /= val; loc.pos /= val;
// return *this; return *this;
// } }
// MyState operator * (const float val) const { MyState operator * (const float val) const {
// MyState copy = *this; MyState copy = *this;
// copy.position = copy.position * val; copy.loc.pos = copy.loc.pos * val;
// return copy; return copy;
// } }
}; };
} }
#endif // MESH_STATE_H #endif // MESH_STATE_H

20
sensors/ActivitySensor.h
View File

@@ -1,16 +1,16 @@
#ifndef BAROMETERACTIVITYSENSOR_H #ifndef BAROMETERACTIVITYSENSOR_H
#define BAROMETERACTIVITYSENSOR_H #define BAROMETERACTIVITYSENSOR_H
#include <Indoor/sensors/pressure/ActivityButterPressure.h> #include <Indoor/sensors/activity/ActivityDetector.h>
#include <Indoor/misc/Debug.h> #include <Indoor/misc/Debug.h>
#include "BarometerSensor.h" #include "BarometerSensor.h"
#include "AccelerometerSensor.h" #include "AccelerometerSensor.h"
#include <iostream> #include <iostream>
struct ActivityData { struct ActivityData {
ActivityButterPressure::Activity curActivity; Activity curActivity;
ActivityData(const ActivityButterPressure::Activity act) : curActivity(act) {;} ActivityData(const Activity act) : curActivity(act) {;}
ActivityData() : curActivity(ActivityButterPressure::Activity::STAY) {;} ActivityData() : curActivity(Activity::STANDING) {;}
}; };
/** /**
@@ -23,7 +23,7 @@ class ActivitySensor :
private: private:
ActivityButterPressure act; ActivityDetector act;
ActivityData data; ActivityData data;
BarometerSensor& baro; BarometerSensor& baro;
@@ -60,16 +60,16 @@ public:
return; return;
} }
this->data.curActivity = act.add(ts, data); act.add(ts, data);
informListeners(ts, this->data); this->data.curActivity = act.get();
informListeners(ts, this->data);
} }
virtual void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override { virtual void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor; (void) sensor;
(void) ts;
(void) data; act.add(ts, data);
// TODO!
} }
}; };

12
sensors/SensorFactory.h
View File

@@ -30,7 +30,7 @@
#include "StepSensor.h" #include "StepSensor.h"
#include "TurnSensor.h" #include "TurnSensor.h"
//#include "ActivitySensor.h" #include "ActivitySensor.h"
class SensorFactory { class SensorFactory {
@@ -90,11 +90,11 @@ public:
return turns; return turns;
} }
// /** get the Activity sensor */ /** get the Activity sensor */
// ActivitySensor& getActivity() { ActivitySensor& getActivity() {
// static ActivitySensor activity(getBarometer(), getAccelerometer()); static ActivitySensor activity(getBarometer(), getAccelerometer());
// return activity; return activity;
// } }
}; };

6
ui/map/2D/ColorPoints2D.h
View File

@@ -7,7 +7,7 @@
#include <Indoor/grid/Grid.h> #include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/Particle.h> #include <Indoor/smc/Particle.h>
#include "../nav/grid/Node.h" #include "../nav/grid/Node.h"
#include "../nav/Observation.h" #include "../nav/Observation.h"
@@ -59,13 +59,13 @@ public:
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
template <typename T> void setFromParticles(const std::vector<K::Particle<T>>& particles) { template <typename T> void setFromParticles(const std::vector<SMC::Particle<T>>& particles) {
points.clear(); points.clear();
// group particles by grid-point // group particles by grid-point
std::unordered_map<GridPoint, float> weights; std::unordered_map<GridPoint, float> weights;
for (const K::Particle<T>& p : particles) { for (const SMC::Particle<T>& p : particles) {
const GridPoint gp = p.state.position; const GridPoint gp = p.state.position;
if (weights.find(gp) != weights.end()) {continue;} if (weights.find(gp) != weights.end()) {continue;}
weights[gp] += p.weight; weights[gp] += p.weight;

2
ui/map/2D/HasSelectableNodes.h
View File

@@ -2,7 +2,7 @@
#define HASSELECTABLENODES_H #define HASSELECTABLENODES_H
#include <vector> #include <vector>
#include <KLib/geo/Point2.h> #include <Indoor/geo/Point2.h>
class HasSelectableNodes { class HasSelectableNodes {

2
ui/map/2D/MapView2D.cpp
View File

@@ -111,7 +111,7 @@ void MapView2D::setMap(WiFiCalibrationDataModel* mdl, Floorplan::IndoorMap* map)
} }
void MapView2D::showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles) { void MapView2D::showParticles(const std::vector<SMC::Particle<GridBased::MyState>>* particles) {
this->colorPoints->setFromParticles(*particles); this->colorPoints->setFromParticles(*particles);
} }

6
ui/map/2D/MapView2D.h
View File

@@ -28,7 +28,7 @@ class ColorPoints2D;
class Path2D; class Path2D;
template <typename T> class DijkstraPath; template <typename T> class DijkstraPath;
namespace K { namespace SMC {
template <typename T> class Particle; template <typename T> class Particle;
} }
class MyState; class MyState;
@@ -101,11 +101,11 @@ public:
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
Q_INVOKABLE void showParticles(const void* particles) { Q_INVOKABLE void showParticles(const void* particles) {
showParticles((const std::vector<K::Particle<GridBased::MyState>>*) particles); showParticles((const std::vector<SMC::Particle<GridBased::MyState>>*) particles);
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles); void showParticles(const std::vector<SMC::Particle<GridBased::MyState>>* particles);

4
ui/map/3D/MapView3D.h
View File

@@ -124,11 +124,11 @@ public:
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
Q_INVOKABLE void showParticles(const void* particles) { Q_INVOKABLE void showParticles(const void* particles) {
showParticles((const std::vector<K::Particle<GridBased::MyState>>*) particles); showParticles((const std::vector<SMC::Particle<GridBased::MyState>>*) particles);
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
void showParticles(const std::vector<K::Particle<GridBased::MyState>>* particles) { void showParticles(const std::vector<SMC::Particle<GridBased::MyState>>* particles) {
this->colorPoints->setFromParticles(*particles); this->colorPoints->setFromParticles(*particles);
} }

8
ui/map/3D/elements/ColorPoints.h
View File

@@ -3,7 +3,7 @@
#include <Indoor/floorplan/v2/Floorplan.h> #include <Indoor/floorplan/v2/Floorplan.h>
#include <KLib/math/filter/particles/ParticleFilter.h> #include <Indoor/smc/filtering/ParticleFilter.h>
#include "../gl/GLHelper.h" #include "../gl/GLHelper.h"
#include "../gl/GLPoints.h" #include "../gl/GLPoints.h"
@@ -62,7 +62,7 @@ public:
} }
/** NOTE: must be called from Qt's main thread! */ /** NOTE: must be called from Qt's main thread! */
template <typename T> void setFromParticles(const std::vector<K::Particle<T>>& particles) { template <typename T> void setFromParticles(const std::vector<SMC::Particle<T>>& particles) {
points.clear(); points.clear();
@@ -71,7 +71,7 @@ public:
// group particles by grid-point // group particles by grid-point
std::unordered_map<GridPoint, float> weights; std::unordered_map<GridPoint, float> weights;
for (const K::Particle<T>& p : particles) { for (const SMC::Particle<T>& p : particles) {
const GridPoint gp = p.state.position; const GridPoint gp = p.state.position;
if (weights.find(gp) != weights.end()) {continue;} if (weights.find(gp) != weights.end()) {continue;}
weights[gp] += p.weight; weights[gp] += p.weight;
@@ -97,7 +97,7 @@ public:
} }
// for (const K::Particle<T>& p : particles) { // for (const SMC::Particle<T>& p : particles) {
// const GridPoint gp = p.state.position; // const GridPoint gp = p.state.position;
// const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f); // swap z and y // const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f); // swap z and y
// const QColor color = Qt::blue; // const QColor color = Qt::blue;