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

aktuell spielerein

Browse Source
This commit is contained in:
toni
2018-08-01 10:08:33 +02:00
parent 1e52b377dc
commit 38042f8dad
5 changed files with 145 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -75,7 +75,7 @@ endif()
add_definitions(
# -DWITH_TESTS
-DWITH_ASSERTIONS
# -DWITH_DEBUG_LOG
-DWITH_DEBUG_LOG
# -DWITH_DEBUG_PLOT
# -D_GLIBCXX_DEBUG
)

4
Plotty.h
View File

@@ -526,6 +526,10 @@ public:
gp.flush();
}
void closeStream(){
gp.close();
}
void saveToFile(std::ofstream& stream){
gp.draw(splot);
stream << "set terminal x11 size 2000,1500\n";

25
Settings.h
View File

@@ -9,7 +9,7 @@ namespace Settings {
bool useKLB = false;
const int numParticles = 5000;
const int numParticles = 1000;
const int numBSParticles = 50;
namespace IMU {
@@ -41,6 +41,11 @@ namespace Settings {
const Point2 bandwidth(100,100);
}
namespace KDE3D {
const Point3 bandwidth(1, 1, 1);
const Point3 gridSize(0.2, 0.2, 1); // in meter
}
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination
@@ -56,7 +61,7 @@ namespace Settings {
constexpr float WAF = -11.0;
const bool optimize = true;
const bool useRegionalOpt = true;
const bool useRegionalOpt = false;
// how to perform VAP grouping. see
// - calibration in Controller.cpp
@@ -84,13 +89,13 @@ namespace Settings {
constexpr bool useMainThread = false; // perform filtering in the main thread
}
// const std::string mapDir = "../map/";
// const std::string dataDir = "../measurements/";
// const std::string errorDir = dataDir + "results/";
const std::string mapDir = "../map/";
const std::string dataDir = "../measurements/";
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/";
// 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, ...) */
struct DataSetup {
@@ -164,7 +169,7 @@ namespace Settings {
DataSetup Path3 = {
mapDir + "map42_ap_path3.xml",
mapDir + "map47_ap_path3.xml",
{
dataDir + "museum/Nexus/Path3_4519.csv",
@@ -178,7 +183,7 @@ namespace Settings {
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat",
45
46
};
} data;

106
main.cpp
View File

@@ -28,6 +28,7 @@
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
#include <sys/stat.h>
@@ -45,6 +46,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats;
//calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
double distance = 0;
for(int i = 1; i < gtEntries.size(); ++i){
distance += gtEntries[i].value.getDistance(gtEntries[i-1].value);
}
std::cout << "Distance of Path: " << distance << std::endl;
// error file
const long int t = static_cast<long int>(time(NULL));
auto evalDir = std::experimental::filesystem::path(Settings::errorDir);
@@ -63,7 +73,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// WiFiModelPerFloor WiFiModelPerFloor(map);
// WiFiModelPerBBox WiFiModelPerBBox(map);
WiFiModel* WiFiModel = nullptr;
WiFiModel* wifiModel = nullptr;
// with optimization
if(Settings::WiFiModel::optimize){
@@ -76,22 +86,23 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// use a regional optimization scheme (one per floor)
WiFiOptimizerPerFloor opt(map);
WiFiOptimizer::PerFloor opt(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
WiFiOptimizer::LogDistCeiling ldc(map, Settings::WiFiModel::vg_calib, WiFiOptimizer::Mode::QUALITY);
// 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);
wifiModel = opt.optimizeAll(ldc.MIN_2_FPS);
wifiModel->saveXML(setup.wifiModel);
} else {
/** NOTE: Funktioniert fürs Museum VIEL VIEL Besser */
// use one model per AP for the whole map
WiFiModel = new WiFiModelLogDistCeiling(map);
wifiModel = new WiFiModelLogDistCeiling(map);
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
@@ -99,10 +110,10 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
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);
((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry);
}
WiFiModel->saveXML(setup.wifiModel);
wifiModel->saveXML(setup.wifiModel);
}
@@ -111,15 +122,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// load WiFiModel from file. The factory will create the correct instance
//WiFiModel->loadXML(setup.wifiModel);
WiFiModelFactory fac(map);
WiFiModel = fac.loadXML(setup.wifiModel);
wifiModel = fac.loadXML(setup.wifiModel);
}
} else {
// without optimization
WiFiModel = new WiFiModelLogDistCeiling(map);
((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");
wifiModel = new WiFiModelLogDistCeiling(map);
((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");
}
@@ -156,15 +167,16 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const int numParticles = 5000;
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(*WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh, *WiFiModel);
auto eval = std::make_unique<MyPFEval>(*wifiModel);
auto trans = std::make_unique<MyPFTrans>(mesh, *wifiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKDE<MyState, MyNavMeshTriangle>>(mesh, Settings::KDE3D::gridSize, Settings::KDE3D::bandwidth);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
@@ -187,6 +199,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
int i = 0;
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
@@ -248,19 +262,35 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.showParticles(pf.getParticles());
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
//plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.setActivity((int) act.get());
plot.plot();
//plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
// float err_m = gtPos.getDistance(est.pos.pos);
// errorStats.add(err_m);
// errorFile << ts.ms() << " " << err_m << "\n";
//error calc with penalty for wrong floor
double errorFactor = 3.0;
Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z);
Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z);
float err_m = gtPosError.getDistance(estError);
errorStats.add(err_m);
errorFile << ts.ms() << " " << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
plot.gp << "set terminal png size 1280,720\n";
plot.gp.setOutput("/tmp/videoSparkasse/" + std::to_string(i++) + ".png");
int degree = ((30 - i) % 360);
if (degree < 0){degree += 360;}
plot.gp << "set view 63,"<< degree << "\n";
plot.gp << "set autoscale xy\n";
plot.gp << "set autoscale z\n";
plot.plot();
}
}
@@ -289,6 +319,33 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot();
plot.closeStream();
// //save file for radius_sub
// std::ofstream radiusFile;
// radiusFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_radius.csv");
// //provide the maximum value for radius_sub (kld * wifiquality)
// double max = *std::max_element(((MyPFTrans*) pf.getTransition())->listRadiusSub.begin() , ((MyPFTrans*) pf.getTransition())->listRadiusSub.end());
// std::cout << "Max Value radius_sub: " << max << std::endl;
// double sum = 0;
// int pos = ((MyPFTrans*) pf.getTransition())->listRadiusSub.size() * 0.75;
// double quantilValue = 0;
// int num = 0;
// for(double val : ((MyPFTrans*) pf.getTransition())->listRadiusSub){
// sum += val;
// if(num == pos){quantilValue = val;}
// ++num;
// radiusFile << val << "\n";
// }
// double avg = sum / num;
// std::cout << "Average radius_sub: " << avg << std::endl;
// std::cout << "75 Quantil radius_sub: " << quantilValue << std::endl;
// radiusFile.close();
return errorStats;
}
@@ -300,7 +357,8 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
std::string evaluationName = "museum/tmp";
//Path2_Museum_KDE_SimpleResampling
std::string evaluationName = "museum/videoSparkasse";
for(int i = 0; i < 1; ++i){
@@ -330,7 +388,7 @@ int main(int argc, char** argv) {
// statsQuantil.add(tmp.getQuantile(0.75));
// }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
for(int j = 0; j < 1; ++j){
tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());

70
navMesh/filter.h
View File

@@ -23,6 +23,7 @@
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include <Indoor/navMesh/NavMeshRandom.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
@@ -71,6 +72,11 @@ struct MyState {
return pos.pos.y;
}
float getZ() {
return pos.pos.z;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
@@ -175,24 +181,28 @@ class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer;
WiFiObserverFree wifiProbability;
const double lambda = 0.0003;
const double lambda = 0.03;
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
public:
//std::vector<double> listRadiusSub;
MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
walker(mesh),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
// how to evaluate drawn points
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
@@ -206,33 +216,33 @@ public:
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// wifi and quality of wifi
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
if(!wifiObs.entries.empty()){
analyzer.add(wifiObs);
}
float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){
qualityWifi = 1.0;
} else if(qualityWifi == 0) {
qualityWifi = 0.00000001;
}
// // wifi and quality of wifi
// const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
// if(!wifiObs.entries.empty()){
// analyzer.add(wifiObs);
// }
// float qualityWifi = analyzer.getQuality();
// if(std::isnan(qualityWifi)){
// qualityWifi = 1.0;
// } else if(qualityWifi == 0) {
// qualityWifi = 0.00000001;
// }
// divergence between eval and transition
std::vector<SMC::Particle<MyState>> wifiParticles;
NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
for(int i = 0; i < 10000; ++i){
// // divergence between eval and transition
// std::vector<SMC::Particle<MyState>> wifiParticles;
// NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
// for(int i = 0; i < 10000; ++i){
NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
wifiParticles.push_back(tmpParticle);
}
// NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
// double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
// SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
// wifiParticles.push_back(tmpParticle);
// }
MyState wifiEstimate = estimator.estimate(wifiParticles);
// MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
// // fake kld
// const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
//const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
//std::cout << "KLD: " << kld << std::endl;
@@ -241,6 +251,8 @@ public:
//update wifi
//walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
// listRadiusSub.push_back(kld * qualityWifi);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
@@ -262,8 +274,8 @@ public:
double deltaUnblockable = 0.01;
// walk
//MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
//MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state
p.weight *= res.probability;