removed Heading::rnd()

changed particle init
fixed uninitialized values
new ground-truth helper methods
added new stats to the eval (distance from ground-truth)

code/eval/FixedLagEvalBase.h

@@ -196,6 +196,8 @@ public:
 
         K::Statistics<double> statsFiltering;
         K::Statistics<double> statsSmoothing;
+		K::Statistics<float> statsDistFiltering;
+		K::Statistics<float> statsDistSmoothing;
         int cnt = 0;
 
 		// error per update-step
@@ -245,12 +247,12 @@ public:
                     break;
                 }
 
-                case s_accel: {
-                    float acc[3];
-                    SensorReaderAccel sre; sre.read(se, acc);
-                    actDet.addAccel(acc);
-                    break;
-                }
+				case s_accel: {
+					float acc[3];
+					SensorReaderAccel sre; sre.read(se, acc);
+					actDet.addAccel(acc);
+					break;
+				}
 
 //					case s_linearAcceleration:{
 //						baroSensorReader.readVerticalAcceleration(se);
@@ -297,7 +299,6 @@ public:
                 // timed updates
                 ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
 
-
                 // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
                 const MyState est = pf->update(&ctrl, obs);
                 const Point3 curEst = est.pCur;
@@ -315,6 +316,7 @@ public:
 
                 pathEst.push_back(curEst);
                 const float err = diff.length();
+				const float errDist = gtw.getMinDist(curEst);	// minimum distance between estimation and ground-truth
 
 
 
@@ -323,8 +325,10 @@ public:
 
                  if (cnt > 35) {
                      statsFiltering.add(err);
+					 statsDistFiltering.add(errDist);
                      errorsNorm.push_back(err);
-                     std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
+					 std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
+					 std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
                  }
 
 				if(cnt > skip){
@@ -363,10 +367,14 @@ public:
 
 
                     const float errSmoothed = diffSmoothed.length();
+					const float errDistSmoothed = gtw.getMinDist(curSmoothedEst);	// minimum distance between smoothed-estimation and ground-truth
+
                     statsSmoothing.add(errSmoothed);
+					statsDistSmoothing.add(errDistSmoothed);
 					errorsSmooth.push_back(errSmoothed);
 
-                    std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
+					std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
+					std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl;
 
                     //plot
                     vis.clearStates();

code/eval/GroundTruthWay.h

@@ -18,6 +18,29 @@ public:
 	/** get the ground truth way */
 	const std::vector<InterpolatorEntry>& getWay() const {return entries;}
 
+	/** get the time where the ground-truth is nearest to p */
+	uint64_t getNearestTime(const Point3 p) const {
+
+		// TODO only an approximation (100ms) -> minor errors possible
+
+		float minD = INFINITY;
+		uint64_t minTS = 0;
+		for (uint64_t ts = getMinKey(); ts <= getMaxKey(); ts += 100) {
+			const Point3 cur = getPosAtTime(ts);
+			const float curD = cur.getDistance(p);
+			if (curD < minD) {minD = curD; minTS = ts;}
+		}
+		return minTS;
+
+	}
+
+	/** get the minimum distance between the given point and the ground-truth (at any time) */
+	float getMinDist(const Point3 p1) const {
+		const uint64_t minTS = getNearestTime(p1);
+		const Point3 p2 = getPosAtTime(minTS);
+		return p1.getDistance(p2);
+	}
+
 };
 
 

code/eval/PaperVisImportance.h

@@ -1,6 +1,7 @@
 #ifndef PAPERVISIMPORTANCE_H
 #define PAPERVISIMPORTANCE_H
 
+/*
 #include <Indoor/grid/Grid.h>
 #include <Indoor/grid/factory/GridFactory.h>
 #include <Indoor/grid/factory/GridImportance.h>
@@ -275,5 +276,6 @@ public:
 
 
 };
+*/
 
 #endif // PAPERVISIMPORTANCE_H