From aeaa4cb59df714b975038fc29c4bc5dea17a5452 Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 5 Apr 2016 18:06:23 +0200 Subject: [PATCH] work in progress. --- .gitignore | 3 - code/eval/FixedLagEvalBase.h | 64 +++++++++++++++--- code/eval/SmoothingEval1.h | 23 +++++-- code/eval/SmoothingEvalBase.h | 33 ++++++--- code/main.cpp | 5 +- .../smoothing/MySmoothingTransition.h | 17 +++-- tex/bare_conf.dvi | Bin 2124 -> 2124 bytes 7 files changed, 107 insertions(+), 38 deletions(-) diff --git a/.gitignore b/.gitignore index 4eb4c78..a74d7c6 100644 --- a/.gitignore +++ b/.gitignore @@ -19,9 +19,6 @@ code/CMakeLists.txt.user code-build/ measurements/ -##cmake -code/CMakeLists.txt.user - ## Core latex/pdflatex auxiliary files: *.aux *.lof diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index f6d7bcf..155478c 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ protected: std::vector path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31}; std::vector path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31}; 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! + 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; @@ -287,12 +288,12 @@ public: const float err = diff.length(); // skip the first 24 scans due to uniform distribution start - if (++cnt > 24) { + if (++cnt > 35) { statsFiltering.add(err); - std::cout << "Filtering: " << statsFiltering.asString() << std::endl; + std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl; } - if(cnt > 19){ + if(cnt > 35){ //save the current estimation for later smoothing. pfHistory.push_back(pf->getNonResamplingParticles()); tsHistory.push_back(se.ts); @@ -301,12 +302,12 @@ public: //fixed-lag smoothing MyState estBF; - if(pfHistory.size() >= MiscSettings::lag){ + if(pfHistory.size() > MiscSettings::lag){ bf->reset(); //use every n-th (std = 1) particle set of the history within a given lag (std = 5) - for(int i = 0; i < MiscSettings::lag; i += MiscSettings::fixedLagGap){ + for(int i = 0; i <= MiscSettings::lag; i += MiscSettings::fixedLagGap){ ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]); estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]); @@ -319,12 +320,13 @@ public: smoothedEst.push_back(curSmoothedEst); // error calculation. compare ground-truth to estimation - const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps)); + //const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps)); + const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[(tsHistory.size() - 1) - MiscSettings::lag]); const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed; const float errSmoothed = diffSmoothed.length(); statsSmoothing.add(errSmoothed); - std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl; + std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; //plot vis.clearStates(); @@ -353,9 +355,9 @@ public: //std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360) << std::endl; - std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl; - std::cout << "EstimationS: " << estBF.avgAngle << std::endl; - std::cout << "EstimationF: " << est.avgAngle << std::endl; +// std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl; +// std::cout << "EstimationS: " << estBF.avgAngle << std::endl; +// std::cout << "EstimationF: " << est.avgAngle << std::endl; //vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' at screen 0.1,0.15\n"; //vis.gp << "set label 111 '" <(500); + //bf = new K::CondensationBackwardFilter; bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); @@ -118,21 +119,29 @@ public: // Hier dauert sehr laaaaaaaannnnnnnggge @Frank void fixedIntervallDijkstraTransPath4(){ - runName = "fixedIntervallSimpleTrans"; - bool smoothing_resample = false; - smoothing_time_delay = 1; + runName = "fixedIntervallDijkstra"; BarometerEvaluation::barometerSigma = 0.10; - sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward - srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt"); - srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/Steps2.txt"); + 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); 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)) ); - //Smoothing using Simple Trans + + //Smoothing Variables + smoothing_walk_mu = 0.7; + smoothing_walk_sigma = 0.5; + smoothing_heading_sigma = 5.0; + smoothing_baro_sigma = 0.05; + + bool smoothing_resample = false; + smoothing_time_delay = 1; + + //Smoothing using Dijkstra bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); if(smoothing_resample) bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); diff --git a/code/eval/SmoothingEvalBase.h b/code/eval/SmoothingEvalBase.h index 99c0815..29fd74d 100644 --- a/code/eval/SmoothingEvalBase.h +++ b/code/eval/SmoothingEvalBase.h @@ -34,6 +34,7 @@ #include "../frank/OrientationSensorReader.h" static int smoothing_time_delay = 0; +float stepSize = 0.71; class SmoothingEvalBase { @@ -237,7 +238,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 @@ -268,8 +269,8 @@ public: const Point3 curGT = gtw.getPosAtTime(se.ts - offset); const Point3 diff = curEst - curGT; - // skip the first 10 scans due to uniform distribution start - if (++cnt > 10) { + // 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); @@ -319,20 +320,28 @@ public: // ========================= // //File - std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats"); + //std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats"); stats.reset(); + bf->reset(); - for(int i = pfHistory.size() - 1; i > 0; i -= smoothing_time_delay){ - //Set time - ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]); + MyState estBF; + int currentParticleSetToSmoothAtTimet = 0; + + while(currentParticleSetToSmoothAtTimet != pfHistory.size() - 1){ + + //iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT) + for(int i = pfHistory.size() - 1; i >= currentParticleSetToSmoothAtTimet; i -= smoothing_time_delay){ + //Set time + ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]); + estBF = bf->update(pfHistory[i]); + } - const MyState estBF = bf->update(pfHistory[i]); const Point3 curSmoothedEst = estBF.pCur; smoothedEst.push_back(curSmoothedEst); // error calculation. compare ground-truth to estimation - const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[i]); + const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[currentParticleSetToSmoothAtTimet]); const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed; const float errSmoothed = diffSmoothed.length(); @@ -346,7 +355,7 @@ public: const K::Particle& p = bf->getbackwardParticles().back()[j]; vis.addState(p.state.walkState); } - vis.setTimestamp(tsHistory[i]); + vis.setTimestamp(tsHistory[currentParticleSetToSmoothAtTimet]); vis.addGroundTruth(gtw); vis.addEstPath(smoothedEst); vis.setEstAndShould(curSmoothedEst, curGTSmoothed); @@ -367,9 +376,11 @@ public: // prevent gnuplot errors usleep(1000*33); + + ++currentParticleSetToSmoothAtTimet; } - statsout2.close(); + //statsout2.close(); } diff --git a/code/main.cpp b/code/main.cpp index 1567257..36bab6f 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -89,9 +89,12 @@ int main(void) { // testModelWalk(); SmoothingEval1 eval; - eval.fixedIntervallSimpleTransPath4(); + eval.fixedIntervallDijkstraTransPath4(); eval.run(); + //Eval1 eval; + //eval.bergwerk_path4_nexus_multi(); + //eval.run(); // {SmoothingEval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();} // //{SmoothingEval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();} diff --git a/code/particles/smoothing/MySmoothingTransition.h b/code/particles/smoothing/MySmoothingTransition.h index ebc7725..d67641d 100644 --- a/code/particles/smoothing/MySmoothingTransition.h +++ b/code/particles/smoothing/MySmoothingTransition.h @@ -102,11 +102,18 @@ public: const double distProb = distWalk.getProbability(distDijkstra_m); //getProb using the angle(heading) between src and dst - double angle = 0.0; - if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ - angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); - } - const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); +// double angle = 0.0; +// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ +// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); +// } +// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); + + // is the heading change similiar to the measurement? + double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359; + double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359; + + double diffDeg = p2AngleDeg - p1AngleDeg; + const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg); //assert(headingProb != 0.0); //assert(distProb != 0.0); diff --git a/tex/bare_conf.dvi b/tex/bare_conf.dvi index 18b272bfdbd801dd44aa2d3c869e95aabd9b1744..3b0924a93df8fb78e1ee6779eec93b77f8d230a7 100644 GIT binary patch delta 28 jcmX>ja7JK)BAja7JK)BA>CIk)@TPsexfP0|Nu&L_?*Gfxp=SaJL7!