diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index 027e07a..bb39a31 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,7 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O2 + -O0 -DWITH_TESTS -DWITH_ASSERTIONS diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user index 8c81593..20be597 100644 --- a/code/CMakeLists.txt.user +++ b/code/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/code/DijkstraMapper.h b/code/DijkstraMapper.h index 16d24ee..bbde6de 100644 --- a/code/DijkstraMapper.h +++ b/code/DijkstraMapper.h @@ -26,6 +26,9 @@ public: return d; } + const std::vector& getAllNodes() const {return grid.getNodes();} + decltype(grid.neighbors(MyGridNode())) getNeighbors(const MyGridNode& node) const {return grid.neighbors(node);} + float getHeuristic(const MyGridNode& n1, const MyGridNode& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);} }; /** diff --git a/code/MyGridNode.h b/code/MyGridNode.h index db61cc2..89c03aa 100644 --- a/code/MyGridNode.h +++ b/code/MyGridNode.h @@ -21,6 +21,7 @@ struct MyGridNode : public GridNode, public GridPoint { public: /** needed ctor */ + MyGridNode() : GridNode(), GridPoint() {;} MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;} }; diff --git a/code/Settings.h b/code/Settings.h index 08a3586..2ee4e69 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -17,7 +17,7 @@ namespace MiscSettings { const int timeSteps = 500; - const int numParticles = 7500; + const int numParticles = 500; const int lag = 5; diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index 155478c..12c01bd 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -414,44 +414,44 @@ public: usleep(1000*33); -// // append error for each run to a file -// std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app); -// oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n"; -// oTError.close(); + // append error for each run to a file + std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app); + oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n"; + oTError.close(); -// // append error for each run to a file -// std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app); -// oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n"; -// oSError.close(); + // append error for each run to a file + std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app); + oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n"; + oSError.close(); -// // plot-data -// std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); -// std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close(); -// std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close(); + // plot-data + std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); + std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close(); + std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close(); -// std::ofstream oPlot("/tmp/plot_" + runName + ".gp"); + std::ofstream oPlot("/tmp/plot_" + runName + ".gp"); -// oPlot << "set terminal eps size 3.4,2\n"; -// oPlot << "set output '" << runName << ".eps'\n"; -// oPlot << "set termoption dashlength 0.5\n"; + oPlot << "set terminal eps size 3.4,2\n"; + oPlot << "set output '" << runName << ".eps'\n"; + oPlot << "set termoption dashlength 0.5\n"; -// oPlot << "set ticslevel 0\n"; -// oPlot << "set view equal xy\n"; -// oPlot << "set zrange [0:2200]\n"; -// oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n"; -// oPlot << "set view 72,33\n"; + oPlot << "set ticslevel 0\n"; + oPlot << "set view equal xy\n"; + oPlot << "set zrange [0:2200]\n"; + oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n"; + oPlot << "set view 72,33\n"; -// oPlot << "unset border\n"; -// oPlot << "unset xtics\n"; -// oPlot << "unset ytics\n"; -// oPlot << "unset ztics\n"; + oPlot << "unset border\n"; + oPlot << "unset xtics\n"; + oPlot << "unset ytics\n"; + oPlot << "unset ztics\n"; -// oPlot << "splot \\\n"; -// oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n"; -// oPlot << "'path_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n"; -// oPlot << "'est_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' "; + oPlot << "splot \\\n"; + oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n"; + oPlot << "'path_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n"; + oPlot << "'est_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' "; -// oPlot.close(); + oPlot.close(); } }; diff --git a/code/eval/SmoothingEval1.h b/code/eval/SmoothingEval1.h index 4695faa..5b34286 100644 --- a/code/eval/SmoothingEval1.h +++ b/code/eval/SmoothingEval1.h @@ -51,7 +51,7 @@ public: //create the backward smoothing filter - bf = new K::BackwardSimulation(500); + bf = new K::BackwardSimulation(50); //bf = new K::CondensationBackwardFilter; bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); diff --git a/code/eval/SmoothingEvalBase.h b/code/eval/SmoothingEvalBase.h index 29fd74d..57e2db5 100644 --- a/code/eval/SmoothingEvalBase.h +++ b/code/eval/SmoothingEvalBase.h @@ -184,7 +184,8 @@ public: uint64_t lastTransitionTS = 0; int64_t start_time = -1; - K::Statistics stats; + K::Statistics statsFiltering; + K::Statistics statsSmoothing; int cnt = 0; //stats file @@ -271,14 +272,14 @@ public: // skip the first 24 scans due to uniform distribution start if (++cnt > 24) { - pathEst.push_back(curEst); - const float err = diff.length(); - stats.add(err); - std::cout << stats.asString() << std::endl; + pathEst.push_back(curEst); + const float err = diff.length(); + statsFiltering.add(err); + std::cout << statsFiltering.asString() << std::endl; - //save the current estimation for later smoothing. - pfHistory.push_back(pf->getNonResamplingParticles()); - tsHistory.push_back(se.ts); + //save the current estimation for later smoothing. + pfHistory.push_back(pf->getNonResamplingParticles()); + tsHistory.push_back(se.ts); } // plot @@ -321,7 +322,7 @@ public: //File //std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats"); - stats.reset(); + //stats.reset(); bf->reset(); MyState estBF; @@ -345,8 +346,8 @@ public: const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed; const float errSmoothed = diffSmoothed.length(); - stats.add(errSmoothed); - std::cout << stats.asString() << std::endl; + statsSmoothing.add(errSmoothed); + std::cout << statsSmoothing.asString() << std::endl; // plot @@ -382,6 +383,44 @@ public: //statsout2.close(); + // append error for each run to a file + std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app); + oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n"; + oTError.close(); + + // append error for each run to a file + std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app); + oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n"; + oSError.close(); + + // plot-data + std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); + std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close(); + std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close(); + + std::ofstream oPlot("/tmp/plot_" + runName + ".gp"); + + oPlot << "set terminal eps size 3.4,2\n"; + oPlot << "set output '" << runName << ".eps'\n"; + oPlot << "set termoption dashlength 0.5\n"; + + oPlot << "set ticslevel 0\n"; + oPlot << "set view equal xy\n"; + oPlot << "set zrange [0:2200]\n"; + oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n"; + oPlot << "set view 72,33\n"; + + oPlot << "unset border\n"; + oPlot << "unset xtics\n"; + oPlot << "unset ytics\n"; + oPlot << "unset ztics\n"; + + oPlot << "splot \\\n"; + oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n"; + oPlot << "'path_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n"; + oPlot << "'est_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' "; + + oPlot.close(); } }; diff --git a/code/particles/smoothing/MySmoothingTransition.h b/code/particles/smoothing/MySmoothingTransition.h index d67641d..546ace1 100644 --- a/code/particles/smoothing/MySmoothingTransition.h +++ b/code/particles/smoothing/MySmoothingTransition.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "../MyState.h" @@ -22,6 +23,8 @@ static double smoothing_walk_sigma = 0.5; static double smoothing_heading_sigma = 15.0; static double smoothing_baro_sigma = 0.2; +typedef std::pair my_key_type; + class MySmoothingTransition : public K::BackwardFilterTransition { private: @@ -75,6 +78,7 @@ public: // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1)) std::vector> predictionProbabilities; + std::map shortestPathMap; auto p1 = particles_old.begin(); auto p2 = particles_new.begin(); @@ -94,12 +98,40 @@ public: const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z)); const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z)); - Dijkstra dijkstra; - dijkstra.build(src, dst, DijkstraMapper(*grid)); +// Dijkstra dijkstra; +// dijkstra.build(src, dst, DijkstraMapper(*grid)); - double distDijkstra_m = dijkstra.getNode(*src)->cumWeight; +// double distDijkstra_m = dijkstra.getNode(*src)->cumWeight; - const double distProb = distWalk.getProbability(distDijkstra_m); + AStar aStar; + DijkstraMapper dm(*grid); + double distDijkstra_m = 0; + std::vector shortestPath; + + // check if this shortestPath was already calculated + std::map::iterator it; + it = shortestPathMap.find(my_key_type(dst, src)); + if(it != shortestPathMap.end()){ + distDijkstra_m = it->second; + } + else{ + + //Dijkstra/A* for shortest path + shortestPath = aStar.get(src, dst, dm); + + //get distance walked and getProb using the walking model + for(int i = 0; i < shortestPath.size() - 1; ++i){ + distDijkstra_m += dm.getWeightBetween(*shortestPath[i], *shortestPath[i+1]); + } + + if(distDijkstra_m != distDijkstra_m) {throw "detected NaN";} + + //save distance and nodes in lookup map + #pragma omp critical + shortestPathMap.insert(std::make_pair(my_key_type(dst, src), distDijkstra_m)); + } + + const double distProb = distWalk.getProbability(distDijkstra_m * 0.01); //getProb using the angle(heading) between src and dst // double angle = 0.0; @@ -127,7 +159,7 @@ public: //if(distance_m != distance_m) {throw "detected NaN";} //if(distProb != distProb) {throw "detected NaN";} - if(angle != angle) {throw "detected NaN";} + //if(angle != angle) {throw "detected NaN";} if(headingProb != headingProb) {throw "detected NaN";} if(floorProb != floorProb) {throw "detected NaN";} if(floorProb == 0) {throw "detected NaN";} diff --git a/tex/bare_conf.dvi b/tex/bare_conf.dvi index 3b0924a..2412b5f 100644 Binary files a/tex/bare_conf.dvi and b/tex/bare_conf.dvi differ diff --git a/tex/bare_conf.tex b/tex/bare_conf.tex index 2fed740..6d68d60 100644 --- a/tex/bare_conf.tex +++ b/tex/bare_conf.tex @@ -137,7 +137,7 @@ % not capitalized unless they are the first or last word of the title. % Linebreaks \\ can be used within to get better formatting as desired. % Do not put math or special symbols in the title. -\title{On Prior Navigation Knowledge in Multi Sensor Indoor Localisation} +\title{On Monte Carlo Smoothing in Multi Sensor Indoor Localisation} % author names and affiliations diff --git a/tex/chapters/experiments.tex b/tex/chapters/experiments.tex index 4e9da90..01ca55d 100644 --- a/tex/chapters/experiments.tex +++ b/tex/chapters/experiments.tex @@ -1,3 +1,17 @@ \section{Experiments} ddd \cite{Ville09} dddd + + +\begin{itemize} + \item Vorwärtsschritt die Ergebnisse und Probleme beschreiben. Zeitlicher Verzug etc. + \item Fixed-Interval Smoothing mit beiden Methoden. Was wird besser? Warum? + \subitem AUf die schlechte Performance eingehen für was könnte man es nutzen? + \item Fixed-lag Smoothing der beiden Methoden + \subitem erstmal ergebnisse mit 3 unterschiedlichen lags. 5, 30, 50 oder so + \subitem Fehlerplot wie beim fusion paper. wo ist der error besonders hoch, warum? warum wurde er besser? + \subitem welche anwendungen gibt es da jeweils? + \subitem plot mit kurven die den fehler bei den unterschiedlichen lag längen angibt. + \item Fixed-lag gap + \subitem einen offset (gap) im smoothing. was bringt es? sinnvoll? +\end{itemize} diff --git a/tex/chapters/introduction.tex b/tex/chapters/introduction.tex index 0e5b0dd..025ef10 100644 --- a/tex/chapters/introduction.tex +++ b/tex/chapters/introduction.tex @@ -1,5 +1,10 @@ \section{Introduction} -Smoothing Smoothing Smoothing +Es gibt viele unterschiedliche Indoor Lösungen die auf blabalbal und blabal aufbauen. particle filter ist etabliert und fast alle nutzen einen. gibt solche und solche methoden. aber alle haben mehr oder wenig ähnliche probleme und herrausforderungen. eine generelle lösunge solceh herrausforderungen zu meistern sind smoothiner methoden. + +probleme: multimodalitäten. zeitlicher verzug. falsche messungen. etc etc. all das kann mit wissen aus zukünftigen messungen behoben werden. + + +