From de2570cc0c61cc6de263a64ecb95a2cccf396d41 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 24 Jan 2018 11:26:26 +0100 Subject: [PATCH] added nav-mesh support via demo --- CMakeLists.txt | 7 +- main.cpp | 474 +-------------------------------------------- mainToni.h | 494 +++++++++++++++++++++++++++++++++++++++++++++++ navMesh/filter.h | 184 ++++++++++++++++++ navMesh/main.h | 94 +++++++++ navMesh/mesh.h | 32 +++ 6 files changed, 810 insertions(+), 475 deletions(-) create mode 100644 mainToni.h create mode 100644 navMesh/filter.h create mode 100644 navMesh/main.h create mode 100644 navMesh/mesh.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e98ce8..f473d22 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,14 +41,15 @@ FILE(GLOB SOURCES ./*/*.cpp ./*/*/*.cpp ./*/*/*/*.cpp - ../../Indoor/lib/tinyxml/tinyxml2.cpp + ../Indoor/lib/tinyxml/tinyxml2.cpp + ../Indoor/lib/Recast/*.cpp ) # system specific compiler flags ADD_DEFINITIONS( - -std=gnu++11 + #-std=gnu++14 -Wall -Werror=return-type @@ -58,7 +59,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O2 + # -O2 -march=native -DWITH_TESTS diff --git a/main.cpp b/main.cpp index 0bcd66b..b8895b9 100755 --- a/main.cpp +++ b/main.cpp @@ -1,479 +1,9 @@ -#include -#include "filter/Structs.h" -#include "filter/KLB.h" -#include "Plotti.h" -#include "filter/Logic.h" -#include "Settings.h" -#include -#include - -#include -#include -#include -#include - -#include - -//frank -//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/"; -//const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/"; - -//toni -const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/"; -//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/"; -const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/"; -//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/"; -const std::string errorDir = dataDir + "results/"; - -/** describes one dataset (map, training, parameter-estimation, ...) */ -struct DataSetup { - std::string map; - std::vector training; - std::string wifiParams; - int minWifiOccurences; - VAPGrouper::Mode vapMode; - std::string grid; -}; - -/** all configured datasets */ -struct Data { - - DataSetup SecondFloorOnly = { - - mapDir + "SHL_Stock_2_01.xml", - - { - dataDir + "Path1_1.csv", - dataDir + "Path2_1.csv", - dataDir + "Path3_1.csv", - }, - - mapDir + "wifi_fp_all.dat", - 40, - VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - mapDir + "grid_Stock_2_01.dat" - }; - - DataSetup FloorOneToThree = { - - mapDir + "SHL_Stock_1-3_03.xml", - - { - dataDir + "Path4_0.csv", - dataDir + "Path5_0.csv", - dataDir + "Path6_0.csv", - }, - - mapDir + "wifi_fp_all.dat", - 40, - VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - mapDir + "grid_Stock_1-3_03.dat" - }; - - DataSetup MotionAxisTest = { - - mapDir + "SHL40_noElevator.xml", - - { - dataDir + "Path0_0.csv" - }, - - mapDir + "wifi_fp_all.dat", - 40, - VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - mapDir + "grid_SHL40_noElevator.dat" - }; - -} data; - -Floorplan::IndoorMap* MyState::map; - -Stats::Statistics run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { - - std::vector kld_data; - - // load the floorplan - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - MyState::map = map; - - WiFiModelLogDistCeiling WiFiModel(map); - WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); - Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); - - //Wi-Fi model new -// WiFiModelFactory factory(map); -// WiFiModel* wifimodel= factory.loadXML("/home/toni/Documents/programme/localization/data/wifi/model/eachOptParPos_multimodel.xml"); -// Assert::isFalse(wifimodel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); - - BeaconModelLogDistCeiling beaconModel(map); - beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); - //Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml"); - - - // build the grid - std::ifstream inp(setup.grid, std::ifstream::binary); - Grid grid(Settings::Grid::gridSize_cm); - - // grid.dat empty? -> build one and save it - if (!inp.good() || (inp.peek()&&0) || inp.eof()) { - std::ofstream onp; - onp.open(setup.grid); - GridFactory factory(grid); - factory.build(map); - - // add node-importance - Importance::addImportance(grid); - - grid.write(onp); - } else { - grid.read(inp); - } - - // stamp WiFi signal-strengths onto the grid - WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); - - // reading file - Offline::FileReader fr(setup.training[numFile]); - - //interpolator for ground truth - Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); - - //gnuplot plot - Plotti plot; - plot.addFloors(map); - plot.addOutline(map); - plot.addStairs(map); - plot.gp << "set autoscale xy\n"; - //plot.addGrid(grid); - plot.splot.getView().setEnabled(false); - - // init ctrl and observation - MyControl ctrl; - ctrl.resetAfterTransition(); - MyObs obs; - - //History of all estimated particles. Used for smoothing - SMC::ForwardFilterHistory pfHistory; - - //filter init - SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); - //SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f))); - - pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); - //pf.setTransition(std::unique_ptr(new PFTransKLDSampling(grid, &ctrl))); - //pf.setTransition(std::unique_ptr(new PFTransSimple(grid))); - pf.setEvaluation(std::unique_ptr(new PFEval(WiFiModel, beaconModel, grid))); - - //resampling - if(Settings::useKLB){ - pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingDivergence())); - } else { - pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingSimple())); - //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingPercent(0.4))); - //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); - //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingKLD())); - } - - pf.setNEffThreshold(0.95); - - //estimation - pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); - //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); - //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5))); - //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationKernelDensity())); - - - /** Smoothing Init */ - SMC::FastKDESmoothing bf(Settings::numParticles, map, Settings::Grid::gridSize_cm, Settings::KDE::bandwidth); - if(Settings::Smoothing::activated){ - - //create the backward smoothing filter - bf.setSampler( std::unique_ptr>(new SMC::CumulativeSampler())); - - bool smoothing_resample = false; - //bf->setNEffThreshold(1.0); - if(smoothing_resample) - bf.setResampling( std::unique_ptr>(new SMC::ParticleFilterResamplingSimple()) ); - - //bf.setTransition(std::unique_ptr( new BFTrans) ); - bf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); - - //Smoothing estimation - bf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); - //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); - //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); - } - - - - Timestamp lastTimestamp = Timestamp::fromMS(0); - - StepDetection sd; - PoseDetection pd; - TurnDetection td(&pd); - MotionDetection md; - ActivityButterPressure act; - //ActivityDetector act; - - RelativePressure relBaro; - relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); - - Stats::Statistics errorStats; - Stats::Statistics errorStatsSmoothing; - - //file writing for error data - const long int t = static_cast(time(NULL)); - const std::string evalDir = errorDir + folder + std::to_string(t); - if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){ - Assert::doThrow("Eval folder couldn't be created!"); - } - - std::ofstream errorFile; - errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); - - std::ofstream errorFileSmoothing; - errorFileSmoothing.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_Smoothing.csv"); - - // parse each sensor-value within the offline data - for (const Offline::Entry& e : fr.getEntries()) { - - const Timestamp ts = Timestamp::fromMS(e.ts); - - if (e.type == Offline::Sensor::WIFI) { - obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; - - } else if (e.type == Offline::Sensor::BEACON){ - obs.beacons.entries.push_back(fr.getBeacons()[e.idx].data); - - // remove to old beacon measurements - obs.beacons.removeOld(ts); - - } else if (e.type == Offline::Sensor::ACC) { - if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { - ++ctrl.numStepsSinceLastTransition; - } - const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - pd.addAccelerometer(ts, _acc.data); - - } else if (e.type == Offline::Sensor::GYRO) { - const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; - const float delta_gyro = td.addGyroscope(ts, _gyr.data); - - ctrl.turnSinceLastTransition_rad += delta_gyro; - - } else if (e.type == Offline::Sensor::BARO) { - relBaro.add(ts, fr.getBarometer()[e.idx].data); - obs.relativePressure = relBaro.getPressureRealtiveToStart(); - obs.sigmaPressure = relBaro.getSigma(); - - //activity recognition - act.add(ts, fr.getBarometer()[e.idx].data); - obs.activity = act.get(); - //activity for transition - - } else if (e.type == Offline::Sensor::LIN_ACC) { - md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); - - } else if (e.type == Offline::Sensor::GRAVITY) { - md.addGravity(ts, fr.getGravity()[e.idx].data); - Eigen::Vector2f curVec = md.getCurrentMotionAxis(); - ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad(); - } - - if (ts.ms() - lastTimestamp.ms() > 500) { - - - /** filtering stuff */ - obs.currentTime = ts; - MyState est = pf.update(&ctrl, obs); - - Point3 estPos = est.position.inMeter(); - Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); - - /** plotting stuff */ - plot.pInterest.clear(); - - plot.setEst(estPos); - plot.setGT(gtPos); - //plot.addEstimationNode(estPos); - //plot.addParticles(pf.getParticles()); - - /** error calculation stuff */ - float err_m = gtPos.getDistance(estPos); - errorStats.add(err_m); - errorFile << err_m << "\n"; - - - /** smoothing stuff */ - if(Settings::Smoothing::activated){ - - // add everything from the forward step to the history - pfHistory.add(ts, pf.getNonResamplingParticles(), ctrl, obs); - - //backward filtering - //((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]); - MyState estBF = bf.update(pfHistory, Settings::Smoothing::lag); - - // get ground truth position at lag time - Point3 estPosSmoothing = estBF.position.inMeter(); - Point3 gtPosSmoothed; - if(pfHistory.size() <= Settings::Smoothing::lag){ - gtPosSmoothed = gtInterpolator.get(static_cast(pfHistory.getFirstTimestamp().ms())); - } else { - gtPosSmoothed = gtInterpolator.get(static_cast(pfHistory.getTimestamp(Settings::Smoothing::lag).ms())); - } - - - //plot - plot.addEstimationNodeSmoothed(estPosSmoothing); - plot.addParticles(bf.getbackwardParticles().back()); - - - if(Settings::Smoothing::lag >= pfHistory.size()){ - // error between GT and smoothing - float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing); - errorStatsSmoothing.add(errSmoothing_m); - errorFileSmoothing << errSmoothing_m << "\n"; - } - - } - - //plot misc - plot.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); - - 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.95, 0.98 'act:" << static_cast(obs.activity) << "'\n"; - - //draw gyro angle and motion angle - //turn angle plot - static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad; - plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); - - //motion angle plot - static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad; - plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); - - /** Draw everything */ - plot.show(); - usleep(10*10); - - lastTimestamp = ts; - - // reset control - ctrl.resetAfterTransition(); - } - - } - - errorFile.close(); - - std::cout << "Statistical Analysis Filtering: " << std::endl; - std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; - - std::cout << "Statistical Analysis Smoothing: " << std::endl; - std::cout << "Median: " << errorStatsSmoothing.getMedian() << " Average: " << errorStatsSmoothing.getAvg() << " Std: " << errorStatsSmoothing.getStdDev() << std::endl; - - //Write the current plotti buffer into file - std::ofstream plotFile; - plotFile.open(evalDir + "/plot_" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); - plot.saveToFile(plotFile); - plotFile.close(); - - for(int i = 0; i < map->floors.size(); ++i){ - plot.printSingleFloor(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); - plot.show(); - usleep(10*10); - } - - plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); - plot.show(); - - plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); - plot.show(); - - plot.printOverview(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); - plot.show(); - - - /** Draw KLB */ - K::Gnuplot gp; - K::GnuplotPlot plotkld; - K::GnuplotPlotElementLines lines; - 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"; - - 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]); - - lines.addSegment(p1, p2); - } - - plotkld.add(&lines); - gp.draw(plotkld); - gp.flush(); - plot.splot.getView().setEnabled(false); - } - - std::cout << "finished" << std::endl; - sleep(1); - - return errorStats; - -} +#include "navMesh/main.h" int main(int argc, char** argv) { - Stats::Statistics statsAVG; - Stats::Statistics statsMedian; - Stats::Statistics statsSTD; - Stats::Statistics statsQuantil; - Stats::Statistics tmp; - - for(int i = 0; i < 10; ++i){ - - tmp = run(data.SecondFloorOnly, 0, "KDE-Smoothing-Test", Settings::Path_DongleTest::path1); - statsMedian.add(tmp.getMedian()); - statsAVG.add(tmp.getAvg()); - statsSTD.add(tmp.getStdDev()); - statsQuantil.add(tmp.getQuantile(0.75)); - -// tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1); -// statsMedian.add(tmp.getMedian()); -// statsAVG.add(tmp.getAvg()); -// statsSTD.add(tmp.getStdDev()); -// statsQuantil.add(tmp.getQuantile(0.75)); - - std::cout << "Iteration " << i << " completed" << std::endl;; - } - - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data: " << std::endl; - std::cout << "Median: " << statsMedian.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - - //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS - std::ofstream finalStatisticFile; - finalStatisticFile.open (errorDir + "/tmp.csv"); - - finalStatisticFile << "Average of all statistical data: \n"; - finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n"; - finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n"; - finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n"; - finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n"; - - finalStatisticFile.close(); - //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + navMeshMain(); } diff --git a/mainToni.h b/mainToni.h new file mode 100644 index 0000000..6e4b08b --- /dev/null +++ b/mainToni.h @@ -0,0 +1,494 @@ +#ifndef MAINTONI_H +#define MAINTONI_H + +#include + +#include "filter/Structs.h" +#include "filter/KLB.h" +#include "Plotti.h" +#include "filter/Logic.h" +#include "Settings.h" + +#include +#include + +#include +#include +#include +#include + +#include + +#include "navMesh/main.h" + +#define D_TONI 1 +#define D_FRANK 2 +#define USE_DATA D_FRANK + +#if (USE_DATA == D_FRANK) + //const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/"; + //const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/"; + const std::string mapDir = "/apps/museum/maps/"; + const std::string dataDir = "/apps/museum/data/"; + const std::string errorDir = dataDir + "results/"; +#elif (USE_DATA == D_TONI) + const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/"; + //const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/"; + const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/"; + //const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/"; + const std::string errorDir = dataDir + "results/"; +#endif + +/** describes one dataset (map, training, parameter-estimation, ...) */ +struct DataSetup { + std::string map; + std::vector training; + std::string wifiParams; + int minWifiOccurences; + VAPGrouper::Mode vapMode; + std::string grid; +}; + +/** all configured datasets */ +struct Data { + + DataSetup SecondFloorOnly = { + + mapDir + "SHL_Stock_2_01.xml", + + { + dataDir + "Path1_1.csv", + dataDir + "Path2_1.csv", + dataDir + "Path3_1.csv", + }, + + mapDir + "wifi_fp_all.dat", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "grid_Stock_2_01.dat" + }; + + DataSetup FloorOneToThree = { + + mapDir + "SHL_Stock_1-3_03.xml", + + { + dataDir + "Path4_0.csv", + dataDir + "Path5_0.csv", + dataDir + "Path6_0.csv", + }, + + mapDir + "wifi_fp_all.dat", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "grid_Stock_1-3_03.dat" + }; + + DataSetup MotionAxisTest = { + + mapDir + "SHL40_noElevator.xml", + + { + dataDir + "Path0_0.csv" + }, + + mapDir + "wifi_fp_all.dat", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "grid_SHL40_noElevator.dat" + }; + +} data; + +Floorplan::IndoorMap* MyState::map; + +Stats::Statistics run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { + + std::vector kld_data; + + // load the floorplan + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); + MyState::map = map; + + WiFiModelLogDistCeiling WiFiModel(map); + WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + + //Wi-Fi model new +// WiFiModelFactory factory(map); +// WiFiModel* wifimodel= factory.loadXML("/home/toni/Documents/programme/localization/data/wifi/model/eachOptParPos_multimodel.xml"); +// Assert::isFalse(wifimodel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + + BeaconModelLogDistCeiling beaconModel(map); + beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); + //Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml"); + + + // build the grid + std::ifstream inp(setup.grid, std::ifstream::binary); + Grid grid(Settings::Grid::gridSize_cm); + + // grid.dat empty? -> build one and save it + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + std::ofstream onp; + onp.open(setup.grid); + GridFactory factory(grid); + factory.build(map); + + // add node-importance + Importance::addImportance(grid); + + grid.write(onp); + } else { + grid.read(inp); + } + + // stamp WiFi signal-strengths onto the grid + WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); + + // reading file + Offline::FileReader fr(setup.training[numFile]); + + //interpolator for ground truth + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); + + //gnuplot plot + Plotti plot; + plot.addFloors(map); + plot.addOutline(map); + plot.addStairs(map); + plot.gp << "set autoscale xy\n"; + //plot.addGrid(grid); + plot.splot.getView().setEnabled(false); + + // init ctrl and observation + MyControl ctrl; + ctrl.resetAfterTransition(); + MyObs obs; + + //History of all estimated particles. Used for smoothing + SMC::ForwardFilterHistory pfHistory; + + //filter init + SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); + //SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f))); + + pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + //pf.setTransition(std::unique_ptr(new PFTransKLDSampling(grid, &ctrl))); + //pf.setTransition(std::unique_ptr(new PFTransSimple(grid))); + pf.setEvaluation(std::unique_ptr(new PFEval(WiFiModel, beaconModel, grid))); + + //resampling + if(Settings::useKLB){ + pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingDivergence())); + } else { + pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingPercent(0.4))); + //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); + //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingKLD())); + } + + pf.setNEffThreshold(0.95); + + //estimation + pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5))); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationKernelDensity())); + + + /** Smoothing Init */ + SMC::FastKDESmoothing bf(Settings::numParticles, map, Settings::Grid::gridSize_cm, Settings::KDE::bandwidth); + if(Settings::Smoothing::activated){ + + //create the backward smoothing filter + bf.setSampler( std::unique_ptr>(new SMC::CumulativeSampler())); + + bool smoothing_resample = false; + //bf->setNEffThreshold(1.0); + if(smoothing_resample) + bf.setResampling( std::unique_ptr>(new SMC::ParticleFilterResamplingSimple()) ); + + //bf.setTransition(std::unique_ptr( new BFTrans) ); + bf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + + //Smoothing estimation + bf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + } + + + + Timestamp lastTimestamp = Timestamp::fromMS(0); + + StepDetection sd; + PoseDetection pd; + TurnDetection td(&pd); + MotionDetection md; + ActivityButterPressure act; + //ActivityDetector act; + + RelativePressure relBaro; + relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + + Stats::Statistics errorStats; + Stats::Statistics errorStatsSmoothing; + + //file writing for error data + const long int t = static_cast(time(NULL)); + const std::string evalDir = errorDir + folder + std::to_string(t); + if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){ + Assert::doThrow("Eval folder couldn't be created!"); + } + + std::ofstream errorFile; + errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + std::ofstream errorFileSmoothing; + errorFileSmoothing.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_Smoothing.csv"); + + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) { + + const Timestamp ts = Timestamp::fromMS(e.ts); + + if (e.type == Offline::Sensor::WIFI) { + obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; + + } else if (e.type == Offline::Sensor::BEACON){ + obs.beacons.entries.push_back(fr.getBeacons()[e.idx].data); + + // remove to old beacon measurements + obs.beacons.removeOld(ts); + + } else if (e.type == Offline::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { + ++ctrl.numStepsSinceLastTransition; + } + const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; + pd.addAccelerometer(ts, _acc.data); + + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; + const float delta_gyro = td.addGyroscope(ts, _gyr.data); + + ctrl.turnSinceLastTransition_rad += delta_gyro; + + } else if (e.type == Offline::Sensor::BARO) { + relBaro.add(ts, fr.getBarometer()[e.idx].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + + //activity recognition + act.add(ts, fr.getBarometer()[e.idx].data); + obs.activity = act.get(); + //activity for transition + + } else if (e.type == Offline::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == Offline::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + Eigen::Vector2f curVec = md.getCurrentMotionAxis(); + ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad(); + } + + if (ts.ms() - lastTimestamp.ms() > 500) { + + + /** filtering stuff */ + obs.currentTime = ts; + MyState est = pf.update(&ctrl, obs); + + Point3 estPos = est.position.inMeter(); + Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); + + /** plotting stuff */ + plot.pInterest.clear(); + + plot.setEst(estPos); + plot.setGT(gtPos); + //plot.addEstimationNode(estPos); + //plot.addParticles(pf.getParticles()); + + /** error calculation stuff */ + float err_m = gtPos.getDistance(estPos); + errorStats.add(err_m); + errorFile << err_m << "\n"; + + + /** smoothing stuff */ + if(Settings::Smoothing::activated){ + + // add everything from the forward step to the history + pfHistory.add(ts, pf.getNonResamplingParticles(), ctrl, obs); + + //backward filtering + //((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]); + MyState estBF = bf.update(pfHistory, Settings::Smoothing::lag); + + // get ground truth position at lag time + Point3 estPosSmoothing = estBF.position.inMeter(); + Point3 gtPosSmoothed; + if(pfHistory.size() <= Settings::Smoothing::lag){ + gtPosSmoothed = gtInterpolator.get(static_cast(pfHistory.getFirstTimestamp().ms())); + } else { + gtPosSmoothed = gtInterpolator.get(static_cast(pfHistory.getTimestamp(Settings::Smoothing::lag).ms())); + } + + + //plot + plot.addEstimationNodeSmoothed(estPosSmoothing); + plot.addParticles(bf.getbackwardParticles().back()); + + + if(Settings::Smoothing::lag >= pfHistory.size()){ + // error between GT and smoothing + float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing); + errorStatsSmoothing.add(errSmoothing_m); + errorFileSmoothing << errSmoothing_m << "\n"; + } + + } + + //plot misc + plot.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); + + 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.95, 0.98 'act:" << static_cast(obs.activity) << "'\n"; + + //draw gyro angle and motion angle + //turn angle plot + static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad; + plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); + + //motion angle plot + static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad; + plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); + + /** Draw everything */ + plot.show(); + usleep(10*10); + + lastTimestamp = ts; + + // reset control + ctrl.resetAfterTransition(); + } + + } + + errorFile.close(); + + std::cout << "Statistical Analysis Filtering: " << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; + + std::cout << "Statistical Analysis Smoothing: " << std::endl; + std::cout << "Median: " << errorStatsSmoothing.getMedian() << " Average: " << errorStatsSmoothing.getAvg() << " Std: " << errorStatsSmoothing.getStdDev() << std::endl; + + //Write the current plotti buffer into file + std::ofstream plotFile; + plotFile.open(evalDir + "/plot_" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); + + for(int i = 0; i < map->floors.size(); ++i){ + plot.printSingleFloor(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); + plot.show(); + usleep(10*10); + } + + plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); + plot.show(); + + plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); + plot.show(); + + plot.printOverview(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.show(); + + + /** Draw KLB */ + K::Gnuplot gp; + K::GnuplotPlot plotkld; + K::GnuplotPlotElementLines lines; + 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"; + + 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]); + + lines.addSegment(p1, p2); + } + + plotkld.add(&lines); + gp.draw(plotkld); + gp.flush(); + plot.splot.getView().setEnabled(false); + } + + std::cout << "finished" << std::endl; + sleep(1); + + return errorStats; + +} + +int main(int argc, char** argv) { + + Stats::Statistics statsAVG; + Stats::Statistics statsMedian; + Stats::Statistics statsSTD; + Stats::Statistics statsQuantil; + Stats::Statistics tmp; + + for(int i = 0; i < 10; ++i){ + + tmp = run(data.SecondFloorOnly, 0, "KDE-Smoothing-Test", Settings::Path_DongleTest::path1); + statsMedian.add(tmp.getMedian()); + statsAVG.add(tmp.getAvg()); + statsSTD.add(tmp.getStdDev()); + statsQuantil.add(tmp.getQuantile(0.75)); + +// tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1); +// statsMedian.add(tmp.getMedian()); +// statsAVG.add(tmp.getAvg()); +// statsSTD.add(tmp.getStdDev()); +// statsQuantil.add(tmp.getQuantile(0.75)); + + std::cout << "Iteration " << i << " completed" << std::endl;; + } + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data: " << std::endl; + std::cout << "Median: " << statsMedian.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + std::ofstream finalStatisticFile; + finalStatisticFile.open (errorDir + "/tmp.csv"); + + finalStatisticFile << "Average of all statistical data: \n"; + finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n"; + finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n"; + finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n"; + finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n"; + + finalStatisticFile.close(); + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + +} + + +#endif // MAINTONI_H diff --git a/navMesh/filter.h b/navMesh/filter.h new file mode 100644 index 0000000..f6bd84b --- /dev/null +++ b/navMesh/filter.h @@ -0,0 +1,184 @@ +#ifndef NAV_MESH_FILTER_H +#define NAV_MESH_FILTER_H + +#include "mesh.h" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +struct MyState { + + /** the state's position (within the mesh) */ + MyNavMeshLocation pos; + + /** the state's heading */ + Heading heading; + + MyState() : pos(), heading(0) {;} + + MyState& operator += (const MyState& o) { + pos.tria = nullptr; // impossible + pos.pos += o.pos.pos; + return *this; + } + + MyState& operator /= (const double val) { + pos.tria = nullptr; // impossible + pos.pos /= val; + return *this; + } + + MyState operator * (const double val) const { + MyState res; + res.pos.pos = pos.pos * val; + return res; + } + +}; + +struct MyControl { + + int numStepsSinceLastEval = 0; + float headingChangeSinceLastEval = 0; + + void afterEval() { + numStepsSinceLastEval = 0; + headingChangeSinceLastEval = 0; + } + + +}; + +struct MyObservation { + + + +}; + +class MyPFInitUniform : public K::ParticleFilterInitializer { + + const MyNavMesh* mesh; + +public: + + MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) { + ; + } + + virtual void initialize(std::vector>& particles) override { + + /** random position and heading within the mesh */ + Distribution::Uniform dHead(0, 2*M_PI); + MyNavMeshRandom rnd = mesh->getRandom(); + for (K::Particle& p : particles) { + p.state.pos = rnd.draw(); + p.state.heading = dHead.draw(); + p.weight = 1.0 / particles.size(); + } + } + +}; + +class MyPFInitFixed : public K::ParticleFilterInitializer { + + const MyNavMesh* mesh; + const Point3 pos; + +public: + + MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) { + ; + } + + virtual void initialize(std::vector>& particles) override { + + /** random position and heading within the mesh */ + Distribution::Uniform dHead(0, 2*M_PI); + for (K::Particle& p : particles) { + p.state.pos = mesh->getLocation(pos); + p.state.heading = dHead.draw(); + p.weight = 1.0 / particles.size(); + } + } + +}; + + +class MyPFTrans : public K::ParticleFilterTransition { + + using MyNavMeshWalk = NM::NavMeshWalkSimple; + MyNavMeshWalk walker; + +public: + + MyPFTrans(MyNavMesh& mesh) : walker(mesh) { + + // how to evaluate drawn points + //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); + //walker.addEvaluator(new NM::WalkEvalDistance(0.1)); + walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target + + } + + void transition(std::vector>& particles, const MyControl* control) override { + + Distribution::Normal dStepSizeFloor(0.70, 0.1); + Distribution::Normal dStepSizeStair(0.35, 0.1); + Distribution::Normal dHeading(0.0, 0.10); + + + for (K::Particle& p : particles) { + + // how to walk + MyNavMeshWalkParams params; + params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw(); + params.numSteps = control->numStepsSinceLastEval; + params.start = p.state.pos; + params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); + params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); + + // walk + MyNavMeshWalk::ResultEntry res = walker.getOne(params); + + // assign back to particle's state + p.weight *= res.probability; + p.state.pos = res.location; + p.state.heading = res.heading; + + } + + // reset the control (0 steps, 0 delta-heading) + //control->afterEval(); + + } + + +}; + +class MyPFEval : public K::ParticleFilterEvaluation { + +public: + + virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { + + return 1.0; + + } + +}; + + +using MyFilter = K::ParticleFilter; + + +#endif diff --git a/navMesh/main.h b/navMesh/main.h new file mode 100644 index 0000000..728b88b --- /dev/null +++ b/navMesh/main.h @@ -0,0 +1,94 @@ +#ifndef NAV_MESH_MAIN_H +#define NAV_MESH_MAIN_H + +#include "mesh.h" +#include "filter.h" +#include +#include +#include + +void navMeshMain() { + + std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml"; + + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile); + + NM::NavMeshSettings set; + MyNavMesh mesh; + MyNavMeshFactory fac(&mesh, set); + fac.build(map); + + const Point3 src(26, 43, 7.5); + + // add shortest-path to destination + //const Point3 dst(51, 45, 1.7); + const Point3 dst(25, 45, 0); + NM::NavMeshDijkstra::stamp(mesh, dst); + + // debug show + NM::NavMeshDebug dbg; + dbg.addMesh(mesh); + //dbg.addDijkstra(mesh); + dbg.draw(); + + // particle-filter + const int numParticles = 1000; + auto init = std::make_unique(&mesh, src); // known position + //auto init = std::make_unique(&mesh); // uniform distribution + auto eval = std::make_unique(); + auto trans = std::make_unique(mesh); + auto resample = std::make_unique>(); + auto estimate = std::make_unique>(); + + // setup + MyFilter pf(numParticles, std::move(init)); + pf.setEvaluation(std::move(eval)); + pf.setTransition(std::move(trans)); + pf.setResampling(std::move(resample)); + pf.setEstimation(std::move(estimate)); + pf.setNEffThreshold(1); + + + MyControl ctrl; + MyObservation obs; + + //Distribution::Uniform dHead(0, 2*M_PI); + Distribution::Normal dHead(0, 0.1); + + for (int i = 0; i < 10000; ++i) { + + ctrl.numStepsSinceLastEval = 1; + ctrl.headingChangeSinceLastEval = dHead.draw(); + + MyState est = pf.update(&ctrl, obs); + + ctrl.afterEval(); + + try { + MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos); + auto path = loc.tria->getPathToDestination(loc.pos); + dbg.addDijkstra(path); + } catch (...) {;} + + const int d = (i * 1) % 360; + dbg.plot.getView().setCamera(60, d); + dbg.showParticles(pf.getParticles()); + dbg.setCurPos(est.pos.pos); + + //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png"); + //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); + + std::cout << i << std::endl; + + dbg.draw(); + + + + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + } + + +} + +#endif diff --git a/navMesh/mesh.h b/navMesh/mesh.h new file mode 100644 index 0000000..be6447e --- /dev/null +++ b/navMesh/mesh.h @@ -0,0 +1,32 @@ +#ifndef NAV_MESH_MESH_H +#define NAV_MESH_MESH_H + + +#include +#include +#include +#include + +#include +#include + +/** the triangle to use with the nav-mesh */ +class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra { + + // add own parameters here + +public: + + MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) { + ; + } + +}; + +using MyNavMeshFactory = NM::NavMeshFactory; +using MyNavMesh = NM::NavMesh; +using MyNavMeshLocation = NM::NavMeshLocation; +using MyNavMeshRandom = NM::NavMeshRandom; +using MyNavMeshWalkParams = NM::NavMeshWalkParams; + +#endif