diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index 9914982..f0051e8 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,7 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O0 + -O2 -DWITH_TESTS -DWITH_ASSERTIONS @@ -72,6 +72,14 @@ ADD_DEFINITIONS( endif() + +# allow OMP +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + # build a binary file ADD_EXECUTABLE( ${PROJECT_NAME} diff --git a/code/DijkstraMapper.h b/code/DijkstraMapper.h index 6679cd9..16d24ee 100644 --- a/code/DijkstraMapper.h +++ b/code/DijkstraMapper.h @@ -21,7 +21,8 @@ public: float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const { float d = ((Point3)n1 - (Point3)n2).length(2) ; //if (d > 20) {d*= 1.30;} - d /= std::pow(n2.imp, 3); + //d /= std::pow(n2.imp, 3); + d /= n2.imp; return d; } diff --git a/code/Helper.h b/code/Helper.h index dcbffd1..2726377 100644 --- a/code/Helper.h +++ b/code/Helper.h @@ -47,8 +47,9 @@ public: } /** align the given value onto the grid */ - static int align(const int val) { - return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm; + template static int align(Grid& grid, const int val) { + volatile const float gridSize_cm = grid.getGridSize_cm(); + return std::round(val / gridSize_cm) * gridSize_cm; } /** all floors within the building */ @@ -58,23 +59,20 @@ public: Stairs s01, s12, s23; - const LengthF h0 = LengthF::cm(align(getHeight(0))); - const LengthF h1 = LengthF::cm(align(getHeight(1))); - const LengthF h2 = LengthF::cm(align(getHeight(2))); - const LengthF h3 = LengthF::cm(align(getHeight(3))); + LengthF h0, h1, h2, h3; // all ground-truth points std::unordered_map gtwp; - FHWSFloors() {;} + FHWSFloors(LengthF h0, LengthF h1, LengthF h2, LengthF h3) : h0(h0), h1(h1), h2(h2), h3(h3) {;} }; /** load the entire floorplan */ - static FHWSFloors getFloors() { + template static FHWSFloors getFloors(Grid& grid) { FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222); - FHWSFloors f; + FHWSFloors f(LengthF::cm(align(grid, getHeight(0))), LengthF::cm(align(grid, getHeight(1))), LengthF::cm(align(grid, getHeight(2))), LengthF::cm(align(grid, getHeight(3)))); f.f0 = fpFac.getFloor("floor_0"); f.f1 = fpFac.getFloor("floor_1"); diff --git a/code/eval/DebugShortestPath.h b/code/eval/DebugShortestPath.h index 121ca58..527dcb3 100644 --- a/code/eval/DebugShortestPath.h +++ b/code/eval/DebugShortestPath.h @@ -41,6 +41,9 @@ public: vis.estPath.clear(); vis.particles.clear(); vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z)); + const int advance = this->stdDevDist / grid.getGridSize_cm(); + const T& d = *this->path->getFromStart(advance).element; + vis.particles.add(K::GnuplotPoint3(d.x_cm, d.y_cm, d.z_cm)); for (int i = 0; i < (int)this->path->size()-1; ++i) { const DijkstraNode& dn1 = (*this->path)[i+0]; const DijkstraNode& dn2 = (*this->path)[i+1]; diff --git a/code/eval/Eval1.h b/code/eval/Eval1.h index fe45713..a8302aa 100644 --- a/code/eval/Eval1.h +++ b/code/eval/Eval1.h @@ -243,6 +243,10 @@ public: } + + + + void bergwerk_path1_nexus_simple() { runName = "bergwerk_path1_nexus_simple"; @@ -253,11 +257,385 @@ public: srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt"); gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + for (auto& n : grid) {n.imp = 1;} + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + + void bergwerk_path1_nexus_imp() { + + runName = "bergwerk_path1_nexus_importance"; + + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); } + void bergwerk_path1_nexus_multi() { + + runName = "bergwerk_path1_nexus_multi"; + + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path1_nexus_shortest() { + + runName = "bergwerk_path1_nexus_shortest"; + + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path2_nexus() { + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path2/nexus/vor/1454779863041.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path2/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path2/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl); + } + + void bergwerk_path2_nexus_simple() { + + runName = "bergwerk_path2_nexus_simple"; + bergwerk_path2_nexus(); + for (auto& n : grid) {n.imp = 1;} // remove importance + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + + void bergwerk_path2_nexus_imp() { + + runName = "bergwerk_path2_nexus_importance"; + bergwerk_path2_nexus(); + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path2_nexus_multi() { + + runName = "bergwerk_path2_nexus_multi"; + bergwerk_path2_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path2_nexus_shortest() { + + runName = "bergwerk_path2_nexus_shortest"; + bergwerk_path2_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + walk->times = 4; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path3_nexus() { + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path3/nexus/vor/1454782562231.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path3/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path3/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl); + } + + void bergwerk_path3_nexus_simple() { + + runName = "bergwerk_path3_nexus_simple"; + bergwerk_path3_nexus(); + for (auto& n : grid) {n.imp = 1;} // remove importance + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + + void bergwerk_path3_nexus_imp() { + + runName = "bergwerk_path3_nexus_importance"; + bergwerk_path3_nexus(); + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path3_nexus_multi() { + + runName = "bergwerk_path3_nexus_multi"; + bergwerk_path3_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path3_nexus_shortest() { + + runName = "bergwerk_path3_nexus_shortest"; + bergwerk_path3_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path4_nexus() { + BarometerEvaluation::barometerSigma = 0.10; + sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl); + //for (const K::Particle& _p : pf->getParticles()) { + // K::Particle& p = (K::Particle&) _p; + // p.state.pCur = floors.gtwp[path4dbl.front()]; + // p.state.pOld = p.state.pCur; + // p.state.walkState.node = grid.getNodePtrFor( conv(p.state.pCur) ); + // p.state.walkState.heading = Angle::degToRad(90); + //} + } + + void bergwerk_path4_nexus_simple() { + + runName = "bergwerk_path4_nexus_simple"; + bergwerk_path4_nexus(); + for (auto& n : grid) {n.imp = 1;} // remove importance + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + + void bergwerk_path4_nexus_imp() { + + runName = "bergwerk_path4_nexus_importance"; + bergwerk_path4_nexus(); + + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path4_nexus_multi() { + + runName = "bergwerk_path4_nexus_multi"; + bergwerk_path4_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + void bergwerk_path4_nexus_shortest() { + + runName = "bergwerk_path4_nexus_shortest"; + bergwerk_path4_nexus(); + + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path1_galaxy() { + BarometerEvaluation::barometerSigma = 0.20; + stepSize = 0.9; + sr = new SensorReader("./measurements/bergwerk/path1/galaxy/vor/1454776168794.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path1/galaxy/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path1/galaxy/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + } + + void bergwerk_path1_galaxy_simple() { + runName = "bergwerk_path1_galaxy_simple"; + bergwerk_path1_galaxy(); + for (auto& n : grid) {n.imp = 1;} // remove importance + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path1_galaxy_multi() { + runName = "bergwerk_path1_galaxy_multi"; + bergwerk_path1_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path1_galaxy_shortest() { + runName = "bergwerk_path1_galaxy_shortest"; + bergwerk_path1_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path2_galaxy() { + BarometerEvaluation::barometerSigma = 0.20; + stepSize = 0.9; + sr = new SensorReader("./measurements/bergwerk/path2/galaxy/vor/1454780113404.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path2/galaxy/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path2/galaxy/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl); + } + + void bergwerk_path2_galaxy_simple() { + runName = "bergwerk_path2_galaxy_simple"; + bergwerk_path2_galaxy(); + for (auto& n : grid) {n.imp = 1;} // remove importance + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path2_galaxy_multi() { + runName = "bergwerk_path2_galaxy_multi"; + bergwerk_path2_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path2_galaxy_shortest() { + runName = "bergwerk_path2_galaxy_shortest"; + bergwerk_path2_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + void bergwerk_path3_galaxy() { + BarometerEvaluation::barometerSigma = 0.20; + stepSize = 0.9; + sr = new SensorReader("./measurements/bergwerk/path3/galaxy/vor/1454782896548.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path3/galaxy/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path3/galaxy/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl); + } + + void bergwerk_path3_galaxy_simple() { + runName = "bergwerk_path3_galaxy_simple"; + bergwerk_path3_galaxy(); + for (auto& n : grid) {n.imp = 1;} // remove importance + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path3_galaxy_multi() { + runName = "bergwerk_path3_galaxy_multi"; + bergwerk_path3_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path3_galaxy_shortest() { + runName = "bergwerk_path3_galaxy_shortest"; + bergwerk_path3_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + + /* ------------------------------------------------------------------------------------------------------------------------------- */ + + + void bergwerk_path4_galaxy() { + BarometerEvaluation::barometerSigma = 0.20; + stepSize = 0.9; + sr = new SensorReader("./measurements/bergwerk/path4/galaxy/vor/1454779020844.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path4/galaxy/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path4/galaxy/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl); + } + + void bergwerk_path4_galaxy_simple() { + runName = "bergwerk_path4_galaxy_simple"; + bergwerk_path4_galaxy(); + for (auto& n : grid) {n.imp = 1;} // remove importance + GridWalkSimpleControl* walk = new GridWalkSimpleControl(); + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path4_galaxy_multi() { + runName = "bergwerk_path4_galaxy_multi"; + bergwerk_path4_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); + GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + } + + void bergwerk_path4_galaxy_shortest() { + + runName = "bergwerk_path4_galaxy_shortest"; + bergwerk_path4_galaxy(); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); + DebugShortestPath* walk = new DebugShortestPath(grid, DijkstraMapper(grid), end, this->floors); + //walk->times = 2; + walk->pOther = 0.15; + pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + } + + }; diff --git a/code/eval/EvalBase.h b/code/eval/EvalBase.h index 9d96b52..c3f7c52 100644 --- a/code/eval/EvalBase.h +++ b/code/eval/EvalBase.h @@ -29,6 +29,9 @@ #include "../frank/BeaconSensorReader.h" #include "../frank/OrientationSensorReader.h" + +#include + class EvalBase { protected: @@ -64,9 +67,11 @@ protected: std::vector path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; std::vector path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint! + float stepSize = 0.71; + public: - EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) { + EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) { // build the grid Helper::buildTheGrid(grid, floors); @@ -173,9 +178,11 @@ public: uint64_t lastTransitionTS = 0; int64_t start_time = -1; + K::Statistics statsTime; K::Statistics stats; int cnt = 0; + std::vector errors; // process each single sensor reading while(sr->hasNext()) { @@ -225,7 +232,7 @@ public: while (!step_observations.empty() && current_time > step_observations.front().ts) { const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so; obs.step->steps++; - ctrl.walked_m = obs.step->steps * 0.71; + ctrl.walked_m = obs.step->steps * stepSize; } // process all occurred steps @@ -241,6 +248,8 @@ public: // time for a transition? if (se.ts - lastTransitionTS > MiscSettings::timeSteps) { + auto tick1 = Time::tick(); + lastTransitionTS = se.ts; // timed updates @@ -252,16 +261,25 @@ public: const Point3 curEst = est.pCur; // error calculation. compare ground-truth to estimation - const int offset = 750; + const int offset = 0; const Point3 curGT = gtw.getPosAtTime(se.ts - offset); const Point3 diff = curEst - curGT; + auto tick2 = Time::tick(); + float diffTime = Time::diffMS(tick1, tick2) * 1.25f; + statsTime.add(diffTime); + std::cout << "#" << statsTime.getAvg() << "\t" << diffTime << std::endl; + // skip the first 10 scans due to uniform distribution start - if (++cnt > 10) { + pathEst.push_back(curEst); const float err = diff.length(); - stats.add(err); - std::cout << stats.asString() << std::endl; + errors.push_back(err); + + // skip the first 24 scans due to uniform distribution start (12 seconds) + if (++cnt > 24) { + stats.add(err); + std::cout << stats.asString() << std::endl; } // plot @@ -287,17 +305,55 @@ public: //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05; vis.gp << "set arrow 999 from screen " << p1.x<<","< 0) { + vis.show(); + usleep(1000*33); // prevent gnuplot errors + dspCnt = 0; + } } - } - sleep(1000); + // append error for each run to a file + std::ofstream oTError("/tmp/errors.txt", std::ios_base::app); + oTError << runName << "\n\t"; stats.appendTo(oTError); oTError << "\n\n"; + oTError.close(); + + // detailled error-description + std::ofstream oError("/tmp/err_" + runName + ".dat"); + for (float f : errors) {oError << f << "\n";} + oError.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_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(); } diff --git a/code/eval/PaperPlot.h b/code/eval/PaperPlot.h index 73e3740..711b7fb 100644 --- a/code/eval/PaperPlot.h +++ b/code/eval/PaperPlot.h @@ -16,35 +16,68 @@ class PaperPlot { public: + struct Size { + float w; + float h; + Size(const float w, const float h) : w(w), h(h) {;} + }; + + +public: + + std::string file; K::Gnuplot gp; K::GnuplotSplot plot; K::GnuplotSplotElementLines floors; K::GnuplotSplotElementColorPoints nodes; - K::GnuplotSplotElementLines edges; + K::GnuplotSplotElementLines edgesSame; + K::GnuplotSplotElementLines edgesStair; public: + PaperPlot(const std::string& file, Size s) : file(file) { + toFile(file, s); + setup(); + } + PaperPlot() { + setup(); + } + + void toFile(const std::string& file, const Size s) { + gp << "set output '" << file << "'\n"; + gp << "set terminal eps size " << s.w << "," << s.h << "\n"; + } + + void setup() { floors.setLineWidth(2); - plot.add(&edges); + plot.add(&edgesSame); + plot.add(&edgesStair); + plot.add(&nodes); plot.add(&floors); - nodes.setPointSize(0.7); - edges.setColorHex("#555555"); + nodes.setPointSize(0.2); + + edgesSame.setColorHex("#555555"); + edgesStair.setColorHex("#AAAA55"); + gp << "set ticslevel 0\n"; - - //gp << "set zrange [0:0]\n"; - } void show() { gp.draw(plot); + if (file.length() != 0) { + std::string dataFile = file + ".dat"; + std::ofstream os(dataFile.c_str()); + os << gp.getBuffer(); + os.close(); + } gp.flush();; } @@ -95,7 +128,7 @@ public: } /** show all nodes (and edges?) within the given region */ - template void debugGrid(Grid& grid, const BBox3& bbox, const bool addNodes, const bool addEdges) { + template void debugGrid(Grid& grid, const BBox3& bbox, const bool addNodes, const bool addEdges, const bool addAllEdges = false) { std::set used; @@ -107,10 +140,11 @@ public: } if (addEdges) { for (const T& n2 : grid.neighbors(n1)) { - if (n1.z_cm == n2.z_cm) {continue;} // speedup + if (n1.z_cm == n2.z_cm && !addAllEdges) {continue;} // speedup if (used.find(n2.getIdx()) == used.end()) { const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); - edges.addSegment(p1, p2); + if (p1.z != p2.z) {edgesStair.addSegment(p1, p2);} + else {edgesSame.addSegment(p1, p2);} } } used.insert(n1.getIdx()); diff --git a/code/eval/PaperVisGrid.h b/code/eval/PaperVisGrid.h index f119ee1..81c5062 100644 --- a/code/eval/PaperVisGrid.h +++ b/code/eval/PaperVisGrid.h @@ -27,13 +27,47 @@ class PaperVisGrid { public: + static void show() { + + // the grid + Grid grid(40); + + // floors + Helper::FHWSFloors floors = Helper::getFloors(grid); + Helper::buildTheGrid(grid, floors); + + PaperPlot plot("/tmp/grid.eps", PaperPlot::Size(4,3)); + + // stairwell low left + {BBox3 bbox; + bbox.add(Point3(1300, 650,160)); + bbox.add(Point3(2000,1300,floors.h2.cm())); + plot.debugGrid(grid, bbox, true, true, true);} + plot.addFloor(floors.f1, floors.h1); + plot.addFloor(floors.f2, floors.h2); + + plot.gp << "set view 64, 300\n"; + plot.gp << "unset cbrange\n"; + plot.gp << "unset colorbox\n"; + plot.gp << "unset xtics\n"; + plot.gp << "unset ytics\n"; + plot.gp << "unset ztics\n"; + plot.gp << "unset border\n"; + plot.gp << "set xrange[1250:2050]\n"; + plot.gp << "set yrange[600:1350]\n"; + plot.gp << "set hidden3d front\n"; + + plot.show(); + + } + static void showStairs() { // the grid Grid grid(20); // floors - Helper::FHWSFloors floors = Helper::getFloors(); + Helper::FHWSFloors floors = Helper::getFloors(grid); Helper::buildTheGrid(grid, floors); // // load the floorplan diff --git a/code/eval/PaperVisImportance.h b/code/eval/PaperVisImportance.h index 478d80e..3218a45 100644 --- a/code/eval/PaperVisImportance.h +++ b/code/eval/PaperVisImportance.h @@ -21,7 +21,8 @@ #include "../Settings.h" #include "../DijkstraMapper.h" -PaperPlot2D::Size s1 = PaperPlot2D::Size(2,4); +PaperPlot2D::Size s1 = PaperPlot2D::Size(4,1.5); +PaperPlot2D::Size s2 = PaperPlot2D::Size(4,1.7); class PaperVisImportance { @@ -79,9 +80,11 @@ public: { PaperPlot2D plot("floorplan_importance.eps", s1); - plot.setRanges(0,2100, 0,5100); - plot.addFloor(f0); + plot.setRanges(0,5100, 0,2100); + //plot.addFloor(f0); plot.addGrid(grid, ColorizerImp()); + plot.gp << "set colorbox\n"; + plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0.6\n"; plot.show(); } @@ -110,8 +113,10 @@ public: // start and end - const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0)); - const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0)); + //const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0)); + //const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0)); + const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0)); + const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0)); // build all shortest path to reach th target Dijkstra dijkstra; @@ -131,23 +136,30 @@ public: DijkstraPath pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart)); // build plot - K::GnuplotPlotElementLines gpPath1; gpPath1.setLineWidth(2); gpPath1.setColorHex("#444444"); - K::GnuplotPlotElementLines gpPath2; gpPath2.setLineWidth(2); gpPath2.setColorHex("#000000"); + K::GnuplotPlotElementLines gpPath1a; gpPath1a.setLineWidth(6); gpPath1a.setColorHex("#000000"); + K::GnuplotPlotElementLines gpPath1b; gpPath1b.setLineWidth(2); gpPath1b.setColorHex("#ffffff"); + K::GnuplotPlotElementLines gpPath2a; gpPath2a.setLineWidth(7); gpPath2a.setColorHex("#000000"); + K::GnuplotPlotElementLines gpPath2b; gpPath2b.setLineWidth(2); gpPath2b.setColorHex("#ffffff"); for (DijkstraNode* dn : pathNormal) { - gpPath1.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); + gpPath1a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); + gpPath1b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); } for (DijkstraNode* dn : pathImp) { - gpPath2.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); + gpPath2a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); + gpPath2b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm)); } // plot the 2 paths { PaperPlot2D plot("floorplan_paths.eps", s1); - plot.setRanges(0,2100, 0,5100); + plot.floors.setColorHex("#777777"); + plot.setRanges(0,5100, 0,2100); plot.addFloor(f0); - plot.plot.add(&gpPath1); gpPath1.setCustomAttr("dashtype 3"); - plot.plot.add(&gpPath2); + plot.plot.add(&gpPath1a); //gpPath1a.setCustomAttr("dashtype 2"); + plot.plot.add(&gpPath1b); gpPath1b.setCustomAttr("dashtype 2"); + plot.plot.add(&gpPath2a); + plot.plot.add(&gpPath2b); plot.show(); } @@ -166,21 +178,98 @@ public: if (i % 250 == 0) {std::cout << i << std::endl;} const MyGridNode& nStart = gnEnd; GridWalkState sStart(&nStart, Heading::rnd()); - //GridWalkState sEnd = walk.getDestination(grid, sStart, 135, 0); + GridWalkState sEnd = walk.getDestination(grid, sStart, 135, 0); + (void) sEnd; } // plot the heat-map { - PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1); - plot.setRanges(0,2100, 0,5100); - plot.gp << "set palette gray negative\n"; + PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s2); + plot.setRanges(0,5220, 0,2100); + plot.floors.setColorHex("#000000"); + //plot.gp << "set palette gray negative\n"; + plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n"; + plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n"; plot.addFloor(f0); plot.addGrid(grid, ColorizeHeat(7000, 50)); + plot.plot.add(&gpPath1a); //gpPath1a.setCustomAttr("dashtype 2"); + plot.plot.add(&gpPath1b); gpPath1b.setCustomAttr("dashtype 2"); + plot.plot.add(&gpPath2a); + plot.plot.add(&gpPath2b); + plot.gp << "set object circle at 300,300 size 40,40 front\n"; + plot.gp << "set object circle at 4700,1300 size 40,40 front\n"; plot.show(); } + + + + } + + + static void showDijkstraDistance() { + + // load the floorplan + FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222); + Floor f0 = fpFac.getFloor("test1"); + const LengthF h0 = LengthF::cm(0); + + // add the floorplan to the grid + Grid grid(20); + GridFactory gridFac(grid); + + gridFac.addFloor(f0, h0.cm()); + + // remove all isolated nodes not attached to 300,300,floor0 + gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) ); + + + + // start and end + const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0)); + const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0)); + + // build all shortest path to reach th target + Dijkstra dijkstra; + + // stamp importance information onto the grid-nodes + GridImportance gridImp; + gridImp.addImportance(grid, h0.cm()); + + // path WITH importance + DijkstraMapper accImp(grid); + dijkstra.build(gnStart, gnStart, accImp); + DijkstraPath pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart)); + + // stamp distance information onto the grid + // attach a corresponding weight-information to each user-grid-node + for (MyGridNode& node : grid) { + const DijkstraNode* dn = dijkstra.getNode(node); + node.distToTarget = dn->cumWeight; + } + + // plot the heat-map + { + PaperPlot2D plot; + plot.setRanges(0,5220, 0,2100); + plot.floors.setColorHex("#000000"); + //plot.gp << "set palette gray negative\n"; + plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n"; + plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n"; + plot.addFloor(f0); + plot.addGrid(grid, ColorizerDist()); + plot.gp << "set object circle at 300,300 size 40,40 front\n"; + plot.gp << "set object circle at 4700,1300 size 40,40 front\n"; + plot.show(); + sleep(1000); + } + + + + + } diff --git a/code/main.cpp b/code/main.cpp index 4485943..8624034 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -27,7 +27,7 @@ void testModelWalk() { Grid grid(MiscSettings::gridSize_cm); - Helper::FHWSFloors floors = Helper::getFloors(); + Helper::FHWSFloors floors = Helper::getFloors(grid); Helper::buildTheGrid(grid, floors); MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm())); @@ -88,23 +88,68 @@ int main(void) { // testModelWalk(); Eval1 eval; - //eval.path2_forward_simple(); - //eval.path2_forward_path(); - //eval.path3_forward_simple(); - //eval.path3_forward_path(); +// //eval.path2_forward_simple(); +// //eval.path2_forward_path(); +// //eval.path3_forward_simple(); +// //eval.path3_forward_path(); - //eval.path4_nexus_simple(); - //eval.path4_nexus_imp(); - //eval.path4_nexus_path(); - //eval.path4_nexus_path_b(); +// //eval.path4_nexus_simple(); +// //eval.path4_nexus_imp(); +// //eval.path4_nexus_path(); +// //eval.path4_nexus_path_b(); - eval.bergwerk_path1_nexus_simple(); +// {Eval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();} +// //{Eval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();} +// {Eval1 eval; eval.bergwerk_path1_nexus_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path1_nexus_shortest(); eval.run();} - eval.run(); +// {Eval1 eval; eval.bergwerk_path2_nexus_simple(); eval.run();} +// //{Eval1 eval; eval.bergwerk_path2_nexus_imp(); eval.run();} +// {Eval1 eval; eval.bergwerk_path2_nexus_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path2_nexus_shortest(); eval.run();} +// {Eval1 eval; eval.bergwerk_path3_nexus_simple(); eval.run();} +// //{Eval1 eval; eval.bergwerk_path3_nexus_imp(); eval.run();} +// {Eval1 eval; eval.bergwerk_path3_nexus_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path3_nexus_shortest(); eval.run();} + + {Eval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();} + //{Eval1 eval; eval.bergwerk_path4_nexus_imp(); eval.run();} + {Eval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();} + {Eval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();} + + + +// {Eval1 eval; eval.bergwerk_path1_galaxy_simple(); eval.run();} +// {Eval1 eval; eval.bergwerk_path1_galaxy_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path1_galaxy_shortest(); eval.run();} + +// {Eval1 eval; eval.bergwerk_path2_galaxy_simple(); eval.run();} +// {Eval1 eval; eval.bergwerk_path2_galaxy_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path2_galaxy_shortest(); eval.run();} + +// {Eval1 eval; eval.bergwerk_path3_galaxy_simple(); eval.run();} +// {Eval1 eval; eval.bergwerk_path3_galaxy_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path3_galaxy_shortest(); eval.run();} + +// {Eval1 eval; eval.bergwerk_path4_galaxy_simple(); eval.run();} +// {Eval1 eval; eval.bergwerk_path4_galaxy_multi(); eval.run();} +// {Eval1 eval; eval.bergwerk_path4_galaxy_shortest(); eval.run();} + + + + + + + + //eval.run(); + //sleep(100); + +// PaperVisGrid::show(); // PaperVisGrid::showStairs(); // PaperVisImportance::createImportance(); // PaperVisImportance::createPath(); +// PaperVisImportance::showDijkstraDistance(); return 0; diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h index 9131b2d..3db5ca3 100755 --- a/code/particles/MyEvaluation.h +++ b/code/particles/MyEvaluation.h @@ -11,6 +11,8 @@ #include "../lukas/StepEvaluation.h" #include "../lukas/TurnEvaluation.h" +#include + class MyEvaluation : public K::ParticleFilterEvaluation { private: @@ -47,6 +49,11 @@ public: double sum = 0; for (K::Particle& p : particles) { +// std::atomic sum(0); +// #pragma omp parallel for +// for (int i = 0; i < (int) particles.size(); ++i) { +// K::Particle& p = particles[i]; + double weight = 1.0; if (useWifi) { diff --git a/code/plan_plots.svg b/code/plan_plots.svg index 4c5e234..e966485 100644 --- a/code/plan_plots.svg +++ b/code/plan_plots.svg @@ -9,9 +9,9 @@ xmlns="http://www.w3.org/2000/svg" xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" - width="724" - height="1800" - viewBox="0 0 724.00001 1800" + width="1820" + height="740" + viewBox="0 0 1820 740" id="svg5100" version="1.1" inkscape:version="0.91 r13725" @@ -25,9 +25,9 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="0.98994949" - inkscape:cx="451.23575" - inkscape:cy="136.10259" + inkscape:zoom="0.49497475" + inkscape:cx="776.28701" + inkscape:cy="236.53547" inkscape:document-units="px" inkscape:current-layer="layer1" showgrid="true" @@ -65,291 +65,318 @@ inkscape:label="test1" inkscape:groupmode="layer" id="layer1" - transform="translate(814.21766,568.50997)"> + transform="translate(814.21766,-491.49003)"> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + + + +