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

added per-floor optimization

Browse Source
This commit is contained in:
k-a-z-u
2018-05-16 13:05:17 +02:00
parent 4b90ba9ca4
commit 1e52b377dc
3 changed files with 89 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Settings.h
View File

@@ -56,13 +56,14 @@ namespace Settings {
constexpr float WAF = -11.0; constexpr float WAF = -11.0;
const bool optimize = true; const bool optimize = true;
const bool useRegionalOpt = true;
// how to perform VAP grouping. see // how to perform VAP grouping. see
// - calibration in Controller.cpp // - calibration in Controller.cpp
// - eval in Filter.h // - eval in Filter.h
// NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM
} }
namespace BeaconModel { namespace BeaconModel {
@@ -83,9 +84,13 @@ namespace Settings {
constexpr bool useMainThread = false; // perform filtering in the main thread constexpr bool useMainThread = false; // perform filtering in the main thread
} }
const std::string mapDir = "../map/"; // const std::string mapDir = "../map/";
const std::string dataDir = "../measurements/"; // const std::string dataDir = "../measurements/";
const std::string errorDir = dataDir + "results/"; // const std::string errorDir = dataDir + "results/";
const std::string mapDir = "/apps/museum/maps/";
const std::string dataDir = "/apps/";
const std::string errorDir = dataDir + "museum/results/";
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup { struct DataSetup {

76
main.cpp
View File

@@ -20,7 +20,11 @@
#include <Indoor/sensors/imu/MotionDetection.h> #include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h> #include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h> #include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/model/WiFiModels.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h> #include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerPerFloor.h>
#include <Indoor/math/stats/Statistics.h> #include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
@@ -55,32 +59,67 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// wifi // wifi
WiFiModelLogDistCeiling WiFiModel(map); // WiFiModelLogDistCeiling WiFiModel(map);
// WiFiModelPerFloor WiFiModelPerFloor(map);
// WiFiModelPerBBox WiFiModelPerBBox(map);
WiFiModel* WiFiModel = nullptr;
// with optimization // with optimization
if(Settings::WiFiModel::optimize){ if(Settings::WiFiModel::optimize){
if (!inp.good() || (inp.peek()&&0) || inp.eof()) { if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.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);
WiFiModel.addAP(ap.mac, entry);
}
WiFiModel.saveXML(setup.wifiModel); Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
if (Settings::WiFiModel::useRegionalOpt) {
// use a regional optimization scheme (one per floor)
WiFiOptimizerPerFloor opt(map);
// add all fingerprints to the optimizer (optimizer will add them to the correct floor/model)
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
WiFiModel = opt.optimizeAll();
WiFiModel->saveXML(setup.wifiModel);
} else {
// use one model per AP for the whole map
WiFiModel = new WiFiModelLogDistCeiling(map);
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.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(setup.wifiModel);
}
} else { } else {
WiFiModel.loadXML(setup.wifiModel);
// load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map);
WiFiModel = fac.loadXML(setup.wifiModel);
} }
} else { } else {
// without optimization // without optimization
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); WiFiModel = new WiFiModelLogDistCeiling(map);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); ((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");
} }
@@ -117,8 +156,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const int numParticles = 5000; const int numParticles = 5000;
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position //auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel); auto eval = std::make_unique<MyPFEval>(*WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh, WiFiModel); auto trans = std::make_unique<MyPFTrans>(mesh, *WiFiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>(); //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>(); //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
@@ -212,7 +251,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.setGroundTruth(gtPos); plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos); plot.addEstimationNode(est.pos.pos);
plot.setActivity((int) act.get()); plot.setActivity((int) act.get());
//plot.plot();
plot.plot();
// error calc // error calc
float err_m = gtPos.getDistance(est.pos.pos); float err_m = gtPos.getDistance(est.pos.pos);

39
navMesh/filter.h
View File

@@ -172,10 +172,10 @@ public:
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> { class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>; using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
MyNavMeshWalk walker; MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer; WiFiQualityAnalyzer analyzer;
@@ -219,20 +219,20 @@ public:
} }
// divergence between eval and transition // divergence between eval and transition
std::vector<SMC::Particle<MyState>> wifiParticles; std::vector<SMC::Particle<MyState>> wifiParticles;
NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom(); NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
for(int i = 0; i < 10000; ++i){ for(int i = 0; i < 10000; ++i){
NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw(); NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs); double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight); SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
wifiParticles.push_back(tmpParticle); wifiParticles.push_back(tmpParticle);
} }
MyState wifiEstimate = estimator.estimate(wifiParticles); MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld // fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos); const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
//const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);; //const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
//std::cout << "KLD: " << kld << std::endl; //std::cout << "KLD: " << kld << std::endl;
@@ -262,8 +262,8 @@ public:
double deltaUnblockable = 0.01; double deltaUnblockable = 0.01;
// walk // walk
//MyNavMeshWalk::ResultEntry res = walker.getOne(params); //MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi); MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state // assign back to particle's state
p.weight *= res.probability; p.weight *= res.probability;
@@ -310,7 +310,10 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
public: public:
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} // FRANK
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override { virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
@@ -318,8 +321,8 @@ public:
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) { for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& p = particles[i];
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);