From de8783b9ca0bf4459c236d3a31df5b078369774c Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 11 Apr 2016 10:57:29 +0200 Subject: [PATCH] added astar to smoothing transition --- code/CMakeLists.txt | 2 +- code/CMakeLists.txt.user | 2 +- code/DijkstraMapper.h | 3 + code/MyGridNode.h | 1 + code/Settings.h | 2 +- code/eval/FixedLagEvalBase.h | 60 ++++++++--------- code/eval/SmoothingEval1.h | 2 +- code/eval/SmoothingEvalBase.h | 61 ++++++++++++++---- .../smoothing/MySmoothingTransition.h | 42 ++++++++++-- tex/bare_conf.dvi | Bin 2124 -> 3204 bytes tex/bare_conf.tex | 2 +- tex/chapters/experiments.tex | 14 ++++ tex/chapters/introduction.tex | 7 +- 13 files changed, 146 insertions(+), 52 deletions(-) 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 3b0924a93df8fb78e1ee6779eec93b77f8d230a7..2412b5f8f97e362034c9928687d7a070b6964a2d 100644 GIT binary patch delta 1167 zcmb7^Pe@cz6vm&qwWJm=qf;aQd%-no)?{M#XzH(!W zYLia$JTO@bR(n7-rs|Ip6pF&iCd`^xE#i z!Md)aZS8GGZW@MB6|@})&fEx9-~NHOYIX)oHJyWkyMQMw=?EOo3gKoLPb1?v=s7OK zVc<$g9N@MPGC#M+Feayrmyf#Vk5%n8Yv&s1cycoMi9vYB_~df^>HUk>jT%zY?{G4! zu5>sc3oxlS`nWp zr0w`j+S&)1RUmt@d{uhc-ju#0x-!uhtf{WEsbY9FN`Ys>tBn-Pfnpa|17(*vN4P~Z zHsuo#GZql(ET%lg1jn3}1F4bGyx21hLLgJ_w4MS|QZvio3Ao^NQjyHKnGwlM3;K5n zn`V#xwEjfjlQxmnK}+kYnDD_N-`bzWec-l7O`Jd1Kiec1^SAVA0Zz|>cVE_OC delta 141 zcmZpXJR>kch1=B1(8R>3n}LCWaiXEp#K1OY!OZIuZ^?QF6lLZYP4-Ki(5haRnLgoR zP-01De%@s7y!?s()6&axQd81XCuinO_ASjR$($UVnpd1()Hh#-fuVO2!=h?2&%BiU t{Gy2*X$7107-d-)7jACi6lY@QWMG^u$09y?GxsL$p9~D_(M