From fdbd9845845701842ea8c3cfb11150caf18aa5d1 Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 18 Apr 2017 11:18:37 +0200 Subject: [PATCH] change simple transition model added klb transition models added debugging output --- code/CMakeLists.txt | 2 +- code/Plotti.h | 22 ++++- code/Settings.h | 2 +- code/filter/KLB.h | 219 ++++++++++++------------------------------ code/filter/Logic.h | 93 +++++++++++------- code/filter/Structs.h | 9 +- code/main.cpp | 69 ++++++++----- 7 files changed, 190 insertions(+), 226 deletions(-) diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index bbde724..3d69b24 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -58,7 +58,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O2 + #-O2 -march=native -DWITH_TESTS diff --git a/code/Plotti.h b/code/Plotti.h index 6166b13..11769dd 100644 --- a/code/Plotti.h +++ b/code/Plotti.h @@ -34,7 +34,8 @@ struct Plotti { K::GnuplotSplotElementLines pStairs; K::GnuplotSplotElementPoints pAPs; K::GnuplotSplotElementPoints pInterest; - K::GnuplotSplotElementPoints pParticles; + K::GnuplotSplotElementPoints pParticles1; + K::GnuplotSplotElementPoints pParticles2; K::GnuplotSplotElementPoints pNormal1; K::GnuplotSplotElementPoints pNormal2; K::GnuplotSplotElementColorPoints pDistributation1; @@ -51,7 +52,8 @@ struct Plotti { splot.add(&pColorPoints); pColorPoints.setPointSize(0.6); splot.add(&pDistributation1); pDistributation1.setPointSize(0.6); splot.add(&pDistributation2); pDistributation2.setPointSize(0.6); - splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f); + splot.add(&pParticles1); pParticles1.setColorHex("#0000ff"); pParticles1.setPointSize(0.4f); + splot.add(&pParticles2); pParticles2.setColorHex("#ff00ff"); pParticles2.setPointSize(0.4f); splot.add(&pNormal1); pNormal1.setColorHex("#ff00ff"); pNormal1.setPointSize(0.4f); splot.add(&pNormal2); pNormal2.setColorHex("#00aaff"); pNormal2.setPointSize(0.4f); splot.add(&pFloor); @@ -295,16 +297,26 @@ struct Plotti { } } - template void addParticles(const std::vector>& particles) { - pParticles.clear(); + template void addParticles1(const std::vector>& particles) { + pParticles1.clear(); int i = 0; for (const K::Particle& p : particles) { if (++i % 25 != 0) {continue;} K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); - pParticles.add(pos / 100.0f); + pParticles1.add(pos / 100.0f); } } + template void addParticles2(const std::vector>& particles) { + pParticles2.clear(); + int i = 0; + for (const K::Particle& p : particles) { + if (++i % 25 != 0) {continue;} + K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); + pParticles2.add(pos / 100.0f); + } + } + void show() { gp.draw(splot); gp.flush(); diff --git a/code/Settings.h b/code/Settings.h index 385dcf6..df81cf4 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -23,7 +23,7 @@ namespace Settings { namespace Mixing { //Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1); - const double lambda = 0.05; + const double lambda = 0.01; } namespace IMU { diff --git a/code/filter/KLB.h b/code/filter/KLB.h index 70fa915..2747282 100644 --- a/code/filter/KLB.h +++ b/code/filter/KLB.h @@ -118,172 +118,71 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability{ -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(); + const double lambda; //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); + Distribution::Uniform uniRand = Distribution::Uniform(-0.1, 0.1); + + /** ctor */ + ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;} + + virtual Eigen::MatrixXd update(std::vector>& modes) override { + + Assert::equal(modes[0].getParticles().size(), modes[1].getParticles().size(), "Particle.size() differs!"); + + // create eigen matrix for posterior and wifi + Eigen::MatrixXd mParticle(modes[0].getParticles().size(), 3); + Eigen::MatrixXd mWifi(modes[1].getParticles().size(), 3); + + #pragma omp parallel for num_threads(6) + for(int i = 0; i < modes[0].getParticles().size(); ++i){ + mParticle(i,0) = (modes[0].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw(); + mParticle(i,1) = (modes[0].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw(); + mParticle(i,2) = (modes[0].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw(); + + mWifi(i,0) = (modes[1].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw(); + mWifi(i,1) = (modes[1].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw(); + mWifi(i,2) = (modes[1].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw(); + } + + // create normal distributions + Eigen::VectorXd meanParticle(3); + Point3 estParticle = modes[0].getEstimation().position.inMeter(); + meanParticle << estParticle.x, estParticle.y, estParticle.z; + Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mParticle, meanParticle); + + Eigen::VectorXd meanWifi(3); + Point3 estWifi = modes[1].getEstimation().position.inMeter(); + meanWifi << estWifi.x, estWifi.y, estWifi.z; + Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mWifi, meanWifi); + + // get kld + double kld = Divergence::KullbackLeibler::getMultivariateGauss(normParticle, normWifi); + + if(kld > 20){ + std::cout << "STTTTTOOOOOOP" << std::endl; + } + + + // debugging global variable + __KLD = kld; + + //exp. distribution + double expKld = std::exp(-lambda * kld); + + Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!"); + + //create the matrix + Eigen::MatrixXd m(2,2); + m << expKld, 1- expKld, 0, 1; + + return m; - //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/filter/Logic.h b/code/filter/Logic.h index 349e73a..2132e15 100644 --- a/code/filter/Logic.h +++ b/code/filter/Logic.h @@ -72,8 +72,9 @@ private: struct PFInit : public K::ParticleFilterInitializer { Grid& grid; + int mode; - PFInit(Grid& grid) : grid(grid) {;} + PFInit(Grid& grid, int mode) : grid(grid), mode(mode) {;} virtual void initialize(std::vector>& particles) override { for (K::Particle& p : particles) { @@ -85,6 +86,9 @@ struct PFInit : public K::ParticleFilterInitializer { p.state.relativePressure = 0; // start with a relative pressure of 0 p.weight = 1.0 / particles.size(); // equal weight + //for debugging + p.state.curMode = mode; + } } @@ -123,56 +127,77 @@ struct PFInitFixed : public K::ParticleFilterInitializer { struct PFTransSimple : public K::ParticleFilterTransition{ Grid& grid; - std::minstd_rand gen; + + // define the noise + Distribution::Normal noise_cm = Distribution::Normal(0.0, Settings::IMU::stepLength * 2.0 * 100.0); + Distribution::Normal height = Distribution::Normal(0.0, 600.0); + + // draw randomly from a vector random_selector<> rand; + + // draw from 0 - 1 Distribution::Uniform uniRand = Distribution::Uniform(0,1); + /** ctor */ PFTransSimple(Grid& grid) : grid(grid) {} virtual void transition(std::vector>& particles, const MyControl* control) override { - std::normal_distribution noise_cm(0.0, Settings::IMU::stepLength * 2.0 * 100.0); + + int noNewPositionCounter = 0; #pragma omp parallel for num_threads(6) for (int i = 0; i < Settings::numParticles; ++i) { K::Particle& p = particles[i]; - // if neighboring node is a staircase, we have a 0.8 chance to walk them. - GridPoint tmp = grid.getNodeFor(p.state.position); - MyNode tmpNode(tmp); - int numNeigbors = grid.getNumNeighbors(tmpNode); +// // if neighboring node is a staircase, we have a 0.8 chance to walk them. +// GridPoint tmp = grid.getNodeFor(p.state.position); +// MyNode tmpNode(tmp); +// int numNeigbors = grid.getNumNeighbors(tmpNode); - std::vector zNodes; - for(int i = 0; i < numNeigbors; ++i){ +// std::vector zNodes; +// for(int i = 0; i < numNeigbors; ++i){ - //if neighbor is stair (1) or elevator (2) - MyNode curNode = grid.getNeighbor(tmpNode, i); - if(curNode.getType() == 1 || curNode.getType() == 2){ - zNodes.push_back(curNode); - } - } +// //if neighbor is stair (1) or elevator (2) +// MyNode curNode = grid.getNeighbor(tmpNode, i); +// if(curNode.getType() == 1 || curNode.getType() == 2){ +// zNodes.push_back(curNode); +// } +// } - float height = 0.0; - if(!zNodes.empty()){ +// float height = 0.0; +// if(!zNodes.empty()){ - if(uniRand.draw() > 0.3){ - //get a random height from all the neighbors on stairs or elevators - height = rand(zNodes).z_cm - p.state.position.z_cm; - }else{ - //do nothin - } +// if(uniRand.draw() > 0.3){ +// //get a random height from all the neighbors on stairs or elevators +// height = rand(zNodes).z_cm - p.state.position.z_cm; +// }else{ +// //do nothin +// } - } +// } - GridPoint noisePt(noise_cm(gen), noise_cm(gen), height); + + double diffHeight = p.state.position.z_cm + height.draw(); + + if() + + + GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw()); GridPoint newPosition = p.state.position + noisePt; - if(grid.hasNodeFor(newPosition)){ - p.state.position = newPosition; - }else{ - //no new position! - } + p.state.position = grid.getNearestNode(newPosition); + +// if(grid.hasNodeFor(newPosition)){ +// p.state.position = newPosition; +// }else{ +// //no new position! +// #pragma omp atomic +// noNewPositionCounter++; +// } } + +// std::cout << noNewPositionCounter << std::endl; } }; @@ -214,7 +239,9 @@ struct PFTrans : public K::ParticleFilterTransition { std::normal_distribution noise(0, Settings::IMU::stepSigma); - for (K::Particle& p : particles) { + #pragma omp parallel for num_threads(6) + for (int i = 0; i < Settings::numParticles; ++i) { + K::Particle& p = particles[i]; // save old position p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); @@ -259,7 +286,7 @@ struct PFEval : public K::ParticleFilterEvaluation { inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const { //const MyNode& node = grid.getNodeFor(point); - return wiFiProbability.getProbability(point.inMeter(), observation.currentTime, vapWifi); + return wiFiProbability.getProbability(point.inMeter() + Point3(0,0,1.3), observation.currentTime, vapWifi); } /** probability for BEACONS */ @@ -304,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation { double sum = 0; const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); - #pragma omp parallel for num_threads(3) + #pragma omp parallel for num_threads(6) for (int i = 0; i < Settings::numParticles; ++i) { K::Particle& p = particles[i]; diff --git a/code/filter/Structs.h b/code/filter/Structs.h index 13dc5e4..8477dd2 100644 --- a/code/filter/Structs.h +++ b/code/filter/Structs.h @@ -28,10 +28,13 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre GridPoint positionOld; - MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} + int curMode; + + MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;} + MyState& operator += (const MyState& o) { this->position += o.position; return *this; @@ -40,8 +43,8 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre this->position /= d; return *this; } - MyState operator * (const double d) const { - return MyState(this->position*d); + MyState operator * (const double d) const { + return MyState(this->position*d); } bool belongsToRegion(const MyState& o) const { return position.inMeter().getDistance(o.position.inMeter()) < 3.0; diff --git a/code/main.cpp b/code/main.cpp index f5bc5c5..894b5a9 100755 --- a/code/main.cpp +++ b/code/main.cpp @@ -166,12 +166,11 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa //init the mode filters std::vector> modes; - - std::shared_ptr> init(new PFInit(grid)); //std::shared_ptr> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)); // mode 1 - K::ParticleFilterMixing mode1(Settings::numParticles, init, Settings::Mode1::modeProbability); + std::shared_ptr> initMode1(new PFInit(grid, 1)); + K::ParticleFilterMixing mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability); mode1.setTransition(std::shared_ptr(new PFTrans(grid, &ctrl))); mode1.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); mode1.setResampling(std::shared_ptr>(new K::ParticleFilterResamplingSimple())); @@ -181,7 +180,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa modes.push_back(mode1); // mode 2 - K::ParticleFilterMixing mode2(Settings::numParticles, init, Settings::Mode2::modeProbability); + std::shared_ptr> initMode2(new PFInit(grid, 2)); + K::ParticleFilterMixing mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability); mode2.setTransition(std::shared_ptr(new PFTransSimple(grid))); mode2.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); mode2.setResampling(std::shared_ptr>(new K::ParticleFilterResamplingSimple())); @@ -197,7 +197,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa K::InteractingMultipleModelParticleFilter IMMAPF(modes, transitionProbabilityMatrix); IMMAPF.setMixingSampler(std::unique_ptr>(new K::MixingSamplerDivergency())); IMMAPF.setJointEstimation(std::unique_ptr>(new K::JointEstimationPosteriorOnly())); - IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransition(grid, Settings::Mixing::lambda))); + //IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransition(grid, Settings::Mixing::lambda))); + IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda))); Timestamp lastTimestamp = Timestamp::fromMS(0); @@ -299,8 +300,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa plot.setEst(estPos); plot.setGT(gtPos); + plot.addParticles1(IMMAPF.getModes()[0].getParticles()); + plot.addParticles2(IMMAPF.getModes()[1].getParticles()); plot.addEstimationNode(estPos); - plot.addParticles(IMMAPF.getModes()[0].getParticles()); plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter()); //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; @@ -308,10 +310,34 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa //plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n"; int minutes = static_cast(ts.sec()) / 60; plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast(static_cast(ts.sec())%60) << "'\n"; - if(Settings::useKLB){ - plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n"; + plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n"; + plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n"; + + plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n"; + plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n"; + + int ones = 0; + int twos = 0; + for(int i = 0; i < IMMAPF.getModes()[0].getParticles().size(); ++i){ + + int mode1 = IMMAPF.getModes()[0].getParticles()[i].state.curMode; + int mode2 = IMMAPF.getModes()[1].getParticles()[i].state.curMode; + + if(mode1 == 1){ + ++ones; + } else { + ++twos; + } + + if(mode2 == 1){ + ++ones; + } else { + ++twos; + } } - plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n"; + + plot.gp << "set label 1007 at screen 0.90, 0.04 'Part. Mode1:" << ones << "'\n"; + plot.gp << "set label 1008 at screen 0.90, 0.02 'Part. Mode2:" << twos << "'\n"; // error between GT and estimation float err_m = gtPos.getDistance(estPos); @@ -362,24 +388,22 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa K::GnuplotPlotElementLines lines; //save as screenshot for klb - if(Settings::useKLB){ - std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t); - gp << "set terminal png size 1280,720\n"; - gp << "set output '" << path << "_shennendistance.png'\n"; + std::string path = evalDir + "/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){ + for(int i=0; i < kld_data.size()-1; ++i){ - K::GnuplotPoint2 p1(i, kld_data[i]); - K::GnuplotPoint2 p2(i+1, kld_data[i+1]); + K::GnuplotPoint2 p1(i, kld_data[i]); + K::GnuplotPoint2 p2(i+1, kld_data[i+1]); - lines.addSegment(p1, p2); - } - - plotkld.add(&lines); - gp.draw(plotkld); - gp.flush(); + lines.addSegment(p1, p2); } + plotkld.add(&lines); + gp.draw(plotkld); + gp.flush(); + std::cout << "finished" << std::endl; sleep(1); @@ -399,7 +423,6 @@ int main(int argc, char** argv) { // run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); // //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); - Settings::useKLB = true; //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1); run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1); run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);