diff --git a/code/Settings.h b/code/Settings.h index d1f1b4f..d864b73 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -7,6 +7,8 @@ namespace Settings { + const bool useKLB = true; + const int numParticles = 10000; namespace IMU { diff --git a/code/filter/Logic.h b/code/filter/Logic.h index 737afd4..ba1b8a8 100644 --- a/code/filter/Logic.h +++ b/code/filter/Logic.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ struct PFInit : public K::ParticleFilterInitializer { p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading p.state.heading.error = 0; p.state.relativePressure = 0; // start with a relative pressure of 0 + p.weight = 1.0 / particles.size(); // equal weight } } @@ -75,7 +77,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer { p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading p.state.heading.error = 0; p.state.relativePressure = 0; // start with a relative pressure of 0 - + p.weight = 1.0 / particles.size(); // equal weight } } @@ -96,6 +98,8 @@ struct PFTrans : public K::ParticleFilterTransition { WalkModuleFavorZ modFavorZ; //WalkModulePreventVisited modPreventVisited; + //WalkModuleActivityControl modActivity; + std::minstd_rand gen; @@ -104,7 +108,7 @@ struct PFTrans : public K::ParticleFilterTransition { walker.addModule(&modHead); //walker.addModule(&modHeadMises); //walker.addModule(&modSpread); // might help in some situations! keep in mind! - + //walker.addModule(&modActivity); //walker.addModule(&modHeadUgly); //walker.addModule(&modImportance); //walker.addModule(&modFavorZ); @@ -178,18 +182,45 @@ struct PFEval : public K::ParticleFilterEvaluation { return Distribution::Normal::getProbability(static_cast(hPa), 0.10, static_cast(observation.relativePressure)); } + double getStairProb(const K::Particle& p, const ActivityButterPressure::Activity act) { + + const float kappa = 0.75; + + const MyNode& gn = grid.getNodeFor(p.state.position); + switch (act) { + + case ActivityButterPressure::Activity::STAY: + if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} + if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} + {return 1-kappa;} + + case ActivityButterPressure::Activity::UP: + case ActivityButterPressure::Activity::DOWN: + if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} + if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} + {return 1-kappa;} + + } + + return 1.0; + + } + virtual double evaluation(std::vector>& particles, const MyObs& observation) override { double sum = 0; const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); - for (K::Particle& p : particles) { + #pragma omp parallel for num_threads(3) + for (int i = 0; i < Settings::numParticles; ++i) { + K::Particle& p = particles[i]; // Point3 pos_m = p.state.position.inMeter(); // Point3 posOld_m = p.state.positionOld.inMeter(); const double pWifi = getWIFI(observation, wifiObs, p.state.position); - const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure); + const double pBaroPressure = getStairProb(p, observation.activity); + //const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure); //const double pBeacon = getBEACON(observation, p.state.position); //small checks @@ -199,6 +230,8 @@ struct PFEval : public K::ParticleFilterEvaluation { const double prob = pBaroPressure * pWifi; p.weight = prob; + + #pragma omp atomic sum += (prob); } diff --git a/code/filter/Structs.h b/code/filter/Structs.h index b79694d..a79641c 100644 --- a/code/filter/Structs.h +++ b/code/filter/Structs.h @@ -18,6 +18,8 @@ #include #include +#include + struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ { static Floorplan::IndoorMap* map; @@ -77,6 +79,9 @@ struct MyObs { /** wifi measurements */ WiFiMeasurements wifi; + /** detected activity */ + ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY; + /** beacon measurements */ BeaconMeasurements beacons; diff --git a/code/main.cpp b/code/main.cpp index 56f8909..3c98c50 100755 --- a/code/main.cpp +++ b/code/main.cpp @@ -170,11 +170,13 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa 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::ParticleFilterResamplingPercent(0.4))); - //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingDivergence())); - + if(Settings::useKLB){ + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingDivergence())); + } else { + //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.setNEffThreshold(0.95); @@ -190,6 +192,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa StepDetection sd; TurnDetection td; MotionDetection md; + ActivityButterPressure act; RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); @@ -236,6 +239,10 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa obs.relativePressure = relBaro.getPressureRealtiveToStart(); obs.sigmaPressure = relBaro.getSigma(); + //activity recognition + obs.activity = act.add(ts, fr.getBarometer()[e.idx].data); + //activity for transition + } else if (e.type == Offline::Sensor::LIN_ACC) { md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); @@ -249,31 +256,39 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa obs.currentTime = ts; - const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi); + MyState est; + if(Settings::useKLB){ - 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); + 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); + } + + double kld = 0.0; + + //set probability distributions. + //std::function>&, MyState, std::vector>&)> kldFunc = getKernelDensityProbability; + std::function>&, MyState, std::vector>&)> kldFunc = kldFromMultivariatNormal; + + //update filter + est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld); + + kld_data.push_back(kld); + } else { + est = pf.update(&ctrl, obs); } - 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, particleWifi, plot); - kld_data.push_back(kld); - //current ground truth position Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); @@ -298,8 +313,10 @@ 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"; - plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld << "'\n"; - //plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n"; + if(Settings::useKLB){ + plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n"; + } + plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n"; // error between GT and estimation float err_m = gtPos.getDistance(estPos); @@ -309,7 +326,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa plot.show(); usleep(10*10); - lastTimestamp = ts; // reset control @@ -350,23 +366,25 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa 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"; + //save as screenshot for klb + if(Settings::useKLB){ + 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){ + 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); + lines.addSegment(p1, p2); + } + + plotkld.add(&lines); + gp.draw(plotkld); + gp.flush(); } - plotkld.add(&lines); - gp.draw(plotkld); - gp.flush(); - std::cout << "finished" << std::endl; sleep(1); @@ -380,7 +398,7 @@ int main(int argc, char** argv) { //for(int i = 0; i < 5; ++i){ //run(data.IPIN2017, 0, "ipin2017"); // Nexus Path2 //run(data.IPIN2017, 1, "ipin2017"); - run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); + //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);