work in progress.

.gitignore

@@ -19,9 +19,6 @@ code/CMakeLists.txt.user
 code-build/
 measurements/
 
-##cmake
-code/CMakeLists.txt.user
-
 ## Core latex/pdflatex auxiliary files:
 *.aux
 *.lof

code/eval/FixedLagEvalBase.h

@@ -8,6 +8,7 @@
 #include <KLib/math/filter/particles/ParticleFilter.h>
 #include <KLib/math/filter/particles/ParticleFilterHistory.h>
 #include <KLib/math/filter/smoothing/BackwardSimulation.h>
+#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
 #include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
 #include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
 #include <KLib/math/statistics/Statistics.h>
@@ -69,7 +70,7 @@ protected:
 	std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
 	std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
 	std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
-	std::vector<int> 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<int> 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 << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
-                    std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
+//                    std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
-                    std::cout << "EstimationF: " << est.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 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
@@ -410,6 +412,46 @@ public:
 
         // prevent gnuplot errors
         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 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_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();
     }
 
 };

code/eval/SmoothingEval1.h

@@ -52,6 +52,7 @@ public:
 
         //create the backward smoothing filter
         bf = new K::BackwardSimulation<MyState>(500);
+        //bf = new K::CondensationBackwardFilter<MyState>;
         bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
 
 
@@ -118,21 +119,29 @@ public:
     // Hier dauert sehr laaaaaaaannnnnnnggge @Frank
     void fixedIntervallDijkstraTransPath4(){
 
-        runName = "fixedIntervallSimpleTrans";
+        runName = "fixedIntervallDijkstra";
-        bool smoothing_resample = false;
-        smoothing_time_delay = 1;
 
         BarometerEvaluation::barometerSigma = 0.10;
-        sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
+        sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
-        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
-        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/Steps2.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<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
         pf->setTransition( std::unique_ptr<MyTransition>( 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<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
         if(smoothing_resample)
             bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );

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
+                // skip the first 24 scans due to uniform distribution start
-                if (++cnt > 10) {
+                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){
+        MyState estBF;
-            //Set time
+        int currentParticleSetToSmoothAtTimet = 0;
-            ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
+
+        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<MyState>& 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();
 
     }
 

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();}

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;
+//                double angle = 0.0;
-                if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
+//                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);
+//                    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);
+//                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);