slow version for frank

code/Vis.h

@@ -28,6 +28,7 @@ public:
 	K::GnuplotSplotElementLines particleDir;
 
 	K::GnuplotSplotElementLines estPath;
+    K::GnuplotSplotElementLines smoothPath;
 	K::GnuplotSplotElementLines groundTruth;
 
 public:
@@ -52,6 +53,8 @@ public:
 		particleDir.setColorHex("#444444");
 
 		estPath.setLineWidth(2);
+        smoothPath.setLineWidth(2);
+        smoothPath.setColorHex("#59C1DA");
 
 		// attach all layers
 		splot.add(&floors);
@@ -61,6 +64,7 @@ public:
 		splot.add(&particles);
 		splot.add(&groundTruth);
 		splot.add(&estPath);
+        splot.add(&smoothPath);
 
 	}
 
@@ -124,6 +128,14 @@ public:
 		}
 	}
 
+    void addSmoothPath(std::vector<Point3>& smt) {
+        smoothPath.clear();;
+        for (const Point3& p : smt) {
+            K::GnuplotPoint3 gp(p.x, p.y, p.z);
+            smoothPath.add(gp);
+        }
+    }
+
 	void setTimestamp(uint64_t ts) {
 		static uint64_t firstTs = ts;
 		gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";