From f2a31fc151c947460c399def923cf32812f078ae Mon Sep 17 00:00:00 2001
From: kazu <kazu@suse.nyan.cat>
Date: Sat, 16 Apr 2016 18:18:10 +0200
Subject: [PATCH] added code to produce error plots fixed minor statistical
 issues

---
 code/eval/FixedLagEvalBase.h | 68 +++++++++++++++++++++++++++++-------
 1 file changed, 55 insertions(+), 13 deletions(-)

diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h
index 12c01bd..5eed57e 100644
--- a/code/eval/FixedLagEvalBase.h
+++ b/code/eval/FixedLagEvalBase.h
@@ -191,6 +191,14 @@ public:
         K::Statistics<double> statsSmoothing;
         int cnt = 0;
 
+		// error per update-step
+		std::vector<float> errorsNorm;
+		std::vector<float> errorsSmooth;
+
+		// number of updates to skip before starting the smoothing process
+		const int skip = 35;
+
+
         //stats file
         std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
 
@@ -286,18 +294,19 @@ public:
 
                 pathEst.push_back(curEst);
                 const float err = diff.length();
+				errorsNorm.push_back(err);
 
-                // skip the first 24 scans due to uniform distribution start
-                if (++cnt > 35) {
-                    statsFiltering.add(err);
-                    std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
-                }
 
-                if(cnt > 35){
+				// count number of transitions
+				++cnt;
+
+
+
+				if(cnt > skip){
                     //save the current estimation for later smoothing.
                     pfHistory.push_back(pf->getNonResamplingParticles());
                     tsHistory.push_back(se.ts);
-                }
+				}
 
 
                 //fixed-lag smoothing
@@ -324,8 +333,17 @@ public:
                     const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[(tsHistory.size() - 1) - MiscSettings::lag]);
                     const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
 
+
+					// skip the first 35 scans due to uniform distribution start
+				   // if (cnt > 35) {
+						statsFiltering.add(err);
+						std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
+					//}
+
                     const float errSmoothed = diffSmoothed.length();
                     statsSmoothing.add(errSmoothed);
+					errorsSmooth.push_back(errSmoothed);
+
                     std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
 
                     //plot
@@ -377,6 +395,19 @@ public:
         }
         statsout.close();
 
+
+		{	// detailled error-description (normally filtered)
+			std::ofstream oError("/tmp/err_norm_" + runName + ".dat");
+			for (float f : errorsNorm) {oError << f << "\n";}
+			oError.close();
+		}
+
+		{	// detailled error-description (smothed)
+			std::ofstream oError("/tmp/err_smooth_" + runName + ".dat");
+			for (float f : errorsSmooth) {oError << f << "\n";}
+			oError.close();
+		}
+
         // plot
         //vis.clearStates();
         vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
@@ -425,9 +456,10 @@ public:
         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 oPath("/tmp/path_" + runName + ".dat");		vis.groundTruth.addDataTo(oPath); oPath.close();		// ground truth
+		std::ofstream oEstN("/tmp/est_norm_" + runName + ".dat");	vis.estPath.addDataTo(oEstN); oEstN.close();			// estimation via filter itself
+		std::ofstream oEstS("/tmp/est_smooth_" + runName + ".dat");	vis.smoothPath.addDataTo(oEstS); oEstS.close();			// estimation via smoothing
+		std::ofstream oFloor("/tmp/floors.dat");					vis.floors.addDataTo(oFloor); oFloor.close();
 
         std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
 
@@ -447,11 +479,21 @@ public:
         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 << "'floors.dat'							skip 21 notitle with lines lc rgb '#777777', \\\n";
+		oPlot << "'path_" << runName << ".dat'			skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
+		oPlot << "'est_norm_" << runName << ".dat'		skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
+		oPlot << "'est_smooth_" << runName << ".dat'	skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";
 
         oPlot.close();
+
+		std::ofstream oErrPlot("/tmp/plot_err_" + runName + ".gp");
+		oErrPlot << "plot \\\n";
+		oErrPlot << "'err_norm_" << runName << ".dat' with lines, \\\n";
+		oErrPlot << "'err_smooth_" << runName << ".dat' using (($$0)+" << skip << "):1 with lines";
+		oErrPlot.close();
+
+
+
     }
 
 };