From 9b21e5627d6f37be00627bfdbade08c2a43429f3 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 30 Mar 2017 18:52:49 +0200 Subject: [PATCH] too many things... sorry :D --- code/Settings.h | 6 +- code/filter/KLB.h | 221 ++++++++++++++++++++++++++++++++++++++++++++++ code/main.cpp | 98 +++++++++++--------- 3 files changed, 281 insertions(+), 44 deletions(-) create mode 100644 code/filter/KLB.h diff --git a/code/Settings.h b/code/Settings.h index 6ee7a1f..d1f1b4f 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -7,7 +7,7 @@ namespace Settings { - const int numParticles = 5000; + const int numParticles = 10000; namespace IMU { const float turnSigma = 2.5; // 3.5 @@ -46,7 +46,7 @@ namespace Settings { const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); } - namespace WiFiModelOptimized { + namespace WiFiModel_{ constexpr float sigma = 8.0; /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ constexpr float TXP = -64.5905; @@ -95,7 +95,7 @@ namespace Settings { } namespace Paths_IPIN2017 { - const std::vector path1 = {0, 1, 2, 3, 4, 5, 6, 7, 700, 8, 9, 10}; + const std::vector path1 = {0, 1, 2, 3, 4, 5, 6, 700, 7, 8, 9, 10}; const std::vector path2 = {11, 12, 3, 2, 1, 0, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 11}; const std::vector path3 = {31, 32, 33, 34, 35, 36, 37, 38}; } diff --git a/code/filter/KLB.h b/code/filter/KLB.h new file mode 100644 index 0000000..36df974 --- /dev/null +++ b/code/filter/KLB.h @@ -0,0 +1,221 @@ +#ifndef KLB_H +#define KLB_H + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +//#include + +#include +#include +#include + +#include "Structs.h" + +#include "../Plotti.h" +#include "Logic.h" +#include "../Settings.h" + +static double getKernelDensityProbability(std::vector>& particles, MyState state, std::vector>& samplesWifi){ + + Distribution::KernelDensity parzen([&](MyState state){ + int size = particles.size(); + double prob = 0; + + #pragma omp parallel for reduction(+:prob) num_threads(6) + for(int i = 0; i < size; ++i){ + double distance = particles[i].state.position.getDistanceInCM(state.position); + prob += Distribution::Normal::getProbability(0, 100, distance) * particles[i].weight; + } + + return prob; + ;}); + + std::vector probsWifiV; + std::vector probsParticleV; + + //just for plottingstuff + std::vector> samplesParticles; + + const int step = 4; + int i = 0; + for(K::Particle particle : samplesWifi){ + if(++i % step != 0){continue;} + MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm)); + + double probiParticle = parzen.getProbability(state); + probsParticleV.push_back(probiParticle); + + double probiwifi = particle.weight; + probsWifiV.push_back(probiwifi); + + //samplesParticles.push_back(K::Particle(state, probiParticle)); + } + + //make vectors + Eigen::Map probsWifi(&probsWifiV[0], probsWifiV.size()); + Eigen::Map probsParticle(&probsParticleV[0], probsParticleV.size()); + + //get divergence + //double kld = Divergence::KullbackLeibler::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS); + double kld = Divergence::JensenShannon::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS); + + //plotti + //plot.debugDistribution1(samplesWifi); + //plot.debugDistribution1(samplesParticles); + + + //estimate the mean + //K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); + //const MyState estWifi = estimateWifi.estimate(samplesWifi); + //plot.addEstimationNodeSmoothed(estWifi.position.inMeter()); + + return kld; +} + + +static double kldFromMultivariatNormal(std::vector>& particles, MyState state, std::vector>& particleWifi){ + //kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi + //todo put this in depletionhelper.h + + Point3 estPos = state.position.inMeter(); + + //this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and + //no inverse matrix is possible + std::mt19937_64 rng; + // initialize the random number generator with time-dependent seed + uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)}; + rng.seed(ss); + // initialize a uniform distribution between -0.0001 and 0.0001 + std::uniform_real_distribution unif(-0.0001, 0.0001); + + //create a gauss dist for the current particle approx. + Eigen::MatrixXd m(particles.size(), 3); + for(int i = 0; i < particles.size(); ++i){ + m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng); + m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng); + m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng); + } + + Eigen::VectorXd mean(3); + mean << estPos.x, estPos.y, estPos.z; + + Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean); + + //create a gauss dist for wifi + Eigen::MatrixXd covWifi(3,3); + covWifi << Settings::WiFiModel::sigma, 0, 0, + 0, Settings::WiFiModel::sigma, 0, + 0, 0, 0.01; + +// //calc wi-fi prob for every node and get mean vector +// WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model); +// const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi); + +// std::vector allNodes = grid.getNodes(); +// std::vector> particleWifi; + +// //problem! dadurch das ich nur die nodes nehme, verschiebt sich der mittelwert natürlich in die mitte des gebäudes und nicht an den rand +// //muss also die verteilung über mehr nodes oder sampling erstellen!! mittelwert fehler!!!! +// //#pragma omp parallel for num_threads(6) +// for(MyNode node : allNodes){ +// double prob = wiFiProbability.getProbability(node, ts, wifiObs); +// K::Particle tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob); +// //#pragma omp critical +// particleWifi.push_back(tmp); +// } + +// std::vector floors; +// floors.push_back(0.0); +// floors.push_back(4.0); +// floors.push_back(7.4); +// floors.push_back(10.8); + +// #pragma omp parallel for num_threads(6) +// for(int x = -20; x < 100; ++x){ +// for(int y = -20; y < 75; ++y){ +// for(double z : floors){ +// double X = x;// / 10.0; +// double Y = y;// / 10.0; +// double Z = z;// / 10.0; + +// Point3 pt(X,Y,Z); +// double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), ts, wifiObs); +// K::Particle tmp (MyState(GridPoint(X * 100.0, Y * 100.0, Z * 100.0)), prob); + +// #pragma omp critical +// particleWifi.push_back(tmp); +// } +// } +// } + + //estimate the mean + K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); + const MyState estWifi = estimateWifi.estimate(particleWifi); + + //get matrix with wifi particles +// Eigen::MatrixXd mW(particleWifi.size(), 3); +// for(int i = 0; i < particleWifi.size(); ++i){ +// mW(i,0) = particleWifi[i].state.position.x_cm / 100.0; +// mW(i,1) = particleWifi[i].state.position.y_cm / 100.0; +// mW(i,2) = estWifi.position.z_cm / 100.0; +// } + + Eigen::VectorXd meanWifi(3); + meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0; + Distribution::NormalDistributionN normWifi(meanWifi, covWifi); + + //Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mW, meanWifi); + + //get the kld distance + double kld = Divergence::KullbackLeibler::getMultivariateGauss(normParticle, normWifi); + + //plot.debugDistribution1(particleWifi); + + //plot.drawNormalN1(normParticle); + //plot.drawNormalN2(normWifi); + + //plot.addEstimationNodeSmoothed(estWifi.position.inMeter()); + + return kld; +} + + + +#endif // KLB_H diff --git a/code/main.cpp b/code/main.cpp index 347b781..56f8909 100755 --- a/code/main.cpp +++ b/code/main.cpp @@ -100,7 +100,7 @@ struct Data { Floorplan::IndoorMap* MyState::map; -void run(DataSetup setup, int numFile, std::string folder) { +void run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { std::vector kld_data; @@ -143,7 +143,7 @@ void run(DataSetup setup, int numFile, std::string folder) { Offline::FileReader fr(setup.training[numFile]); //interpolator for ground truth - Interpolator gtInterpolator = fr.getGroundTruthPath(map, Settings::Paths_IPIN2017::path1); + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); //gnuplot plot Plotti plot; @@ -165,19 +165,23 @@ void run(DataSetup setup, int numFile, std::string folder) { //filter init //std::unique_ptr init = K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); - //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 1080.0f), 90.0f))); + //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f))); pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); pf.setEvaluation(std::unique_ptr(new PFEval(WiFiModel, beaconModel, grid))); //resampling - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.4))); + //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingDivergence())); + + pf.setNEffThreshold(0.95); //estimation //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.95))); + pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5))); //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); @@ -191,6 +195,10 @@ void run(DataSetup setup, int numFile, std::string folder) { K::Statistics errorStats; + //calc wi-fi prob for every node and get mean vector + WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, WiFiModel); + + //file writing for error data long int t = static_cast(time(NULL)); std::ofstream errorFile; @@ -241,17 +249,34 @@ void run(DataSetup setup, int numFile, std::string folder) { obs.currentTime = ts; - MyState est = pf.update(&ctrl, obs); + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi); + + std::vector allNodes = grid.getNodes(); + std::vector> particleWifi; + for(MyNode node : allNodes){ + double prob = wiFiProbability.getProbability(node, ts, wifiObs); + K::Particle tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob); + particleWifi.push_back(tmp); + } + + if(kld_data.empty()){ + kld_data.push_back(0.0); + } + + std::function>&, MyState, std::vector>&)> kldFunc = getKernelDensityProbability; + //std::function>&, MyState, std::vector>&)> kldFunc = kldFromMultivariatNormal; + + double kld = 0.0; + MyState est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld); Point3 estPos = est.position.inMeter(); //double kld = getKernelDensityProbability(pf, WiFiModel, obs, grid, ts, plot); - double kld = kldFromMultivariatNormal(pf, estPos, WiFiModel, obs, grid, ts, plot); + //double kld = kldFromMultivariatNormal(pf, estPos, particleWifi, plot); kld_data.push_back(kld); //current ground truth position Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); - /** plotting stuff */ plot.pInterest.clear(); @@ -289,25 +314,6 @@ void run(DataSetup setup, int numFile, std::string folder) { // reset control ctrl.resetAfterTransition(); - - //plot image files -// for(int i = 0; i < map->floors.size(); ++i){ -// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), i); -// plot.show(); -// usleep(10*10); -// } - -// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), 90); -// plot.show(); - -// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++), 0); -// plot.show(); - -// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t++)); -// plot.show(); - -// //reset to qt window -// plot.gp << "set terminal qt size 800,600\n"; } } @@ -323,20 +329,20 @@ void run(DataSetup setup, int numFile, std::string folder) { plot.saveToFile(plotFile); plotFile.close(); -// for(int i = 0; i < map->floors.size(); ++i){ -// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); -// plot.show(); -// usleep(1000*10); -// } + for(int i = 0; i < map->floors.size(); ++i){ + plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); + plot.show(); + usleep(10*10); + } -// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); -// plot.show(); + plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); + plot.show(); -// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); -// plot.show(); + plot.printSideView("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); + plot.show(); -// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); -// plot.show(); + plot.printOverview("/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.show(); //draw kld @@ -344,6 +350,11 @@ void run(DataSetup setup, int numFile, std::string folder) { K::GnuplotPlot plotkld; K::GnuplotPlotElementLines lines; + //save as screenshot + std::string path = "/home/toni/Documents/programme/localization/IPIN2017/code/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t); + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_shennendistance.png'\n"; + for(int i=0; i < kld_data.size()-1; ++i){ K::GnuplotPoint2 p1(i, kld_data[i]); @@ -353,11 +364,11 @@ void run(DataSetup setup, int numFile, std::string folder) { } plotkld.add(&lines); - gp.draw(plotkld); gp.flush(); - sleep(1000); + std::cout << "finished" << std::endl; + sleep(1); } @@ -367,7 +378,12 @@ int main(int argc, char** argv) { //run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor //for(int i = 0; i < 5; ++i){ - run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2 + //run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2 + //run(data.IPIN2017, 1, "ipin2017"); + run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); + run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2); + run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); + run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2); //} }