Improved TouchEventVisualizer. (touch events from different frames are draw separately and the events are filtered with a kalman filter)

bbiwarg/Graphics/TouchEventVisualizer.cs

@@ -11,40 +11,80 @@ namespace bbiwarg.Graphics
     class TouchEventVisualizer
     {
         public OutputImage OutputImage { get; private set; }
-        
-        private List<Vector2D> touchPositions;
+
+        private List<Kalman2DPositionFilter> kalmanFilters;
+        private List<List<Vector2D>> touchPositions;
+        private int lastUpdated = -1;
         private int width, height;
 
         public TouchEventVisualizer(int width, int height)
         {
-            touchPositions = new List<Vector2D>();
+            touchPositions = new List<List<Vector2D>>();
+            kalmanFilters = new List<Kalman2DPositionFilter>();
             this.width = width;
             this.height = height;
         }
 
-        public void addPalmTouchEvent(PalmTouchEvent e)
+        public void addPalmTouchEvent(PalmTouchEvent e, int currentFrame)
         {
-            Vector2D pos = new Vector2D((int) (e.RelativePalmPosition.X * width), (int) (e.RelativePalmPosition.Y * height));
-            touchPositions.Add(pos);
+            List<Vector2D> currentList;
+            Kalman2DPositionFilter currentFilter;
+
+            bool addList = false;
+            if (lastUpdated == -1 || (currentFrame - lastUpdated) > 5)
+            {
+                currentList = new List<Vector2D>();
+                currentFilter = new Kalman2DPositionFilter(e.RelativePalmPosition);
+                addList = true;
+            }
+            else
+            {
+                currentList = touchPositions.Last<List<Vector2D>>();
+                currentFilter = kalmanFilters.Last<Kalman2DPositionFilter>();
+            }
+
+            Vector2D pos;
+            if (currentFilter == null)
+            {
+                currentFilter = new Kalman2DPositionFilter(e.RelativePalmPosition);
+                pos = e.RelativePalmPosition;
+            }
+            else 
+            {
+                pos = currentFilter.getCorrectedPosition(e.RelativePalmPosition);
+            }
+
+            currentList.Add(new Vector2D((int)(pos.X * width), (int)(pos.Y * height)));
+            if (addList)
+            {
+                touchPositions.Add(currentList);
+                kalmanFilters.Add(currentFilter);
+            }
+
+            lastUpdated = currentFrame;
         }
 
         public void updateImage()
         {
             OutputImage = new OutputImage(width, height);
 
-            for (int i = 1; i < touchPositions.Count; ++i)
+            foreach (List<Vector2D> positions in touchPositions)
             {
-                OutputImage.drawLineSegment(new LineSegment2D(touchPositions[i - 1], touchPositions[i]), 255, 255, 255);
-            }
+                for (int i = 1; i < positions.Count; ++i)
+                {
+                    OutputImage.drawLineSegment(new LineSegment2D(positions[i - 1], positions[i]), 255, 255, 255);
+                }
 
-            if (touchPositions.Count != 0)
-                OutputImage.fillCircle(touchPositions.Last<Vector2D>().IntX, touchPositions.Last<Vector2D>().IntY, 3, 255, 0, 0);
+                if (touchPositions.Count != 0)
+                    OutputImage.fillCircle(positions.Last<Vector2D>().IntX, positions.Last<Vector2D>().IntY, 3, 255, 0, 0);
+            }
         }
 
         public void Reset()
         {
             touchPositions.Clear();
             OutputImage = new OutputImage(width, height);
+            lastUpdated = -1;
         }
     }
 }

bbiwarg/VideoHandle.cs

@@ -41,6 +41,8 @@ namespace bbiwarg
         private OutputImage depthPalmTouchOutputImage;
         private TouchEventVisualizer touchEventVisualizer;
 
+        private int videoFrame = 0;
+
         public VideoHandle(IInputProvider inputProvider)
         {
             this.inputProvider = inputProvider;
@@ -100,6 +102,7 @@ namespace bbiwarg
             {
                 inputProvider.stop();
             }
+            ++videoFrame;
         }
 
         public List<PalmTouchEvent> getPalmTouchEvents()
@@ -168,7 +171,12 @@ namespace bbiwarg
             if (getCurrentMovieFrame() == 0)
                 touchEventVisualizer.Reset();
             foreach (PalmTouchEvent e in palmTouchDetector.PalmTouchEvents)
-                touchEventVisualizer.addPalmTouchEvent(e);
+            {
+                if (sourceIsMovie())
+                    touchEventVisualizer.addPalmTouchEvent(e, getCurrentMovieFrame());
+                else
+                    touchEventVisualizer.addPalmTouchEvent(e, videoFrame);
+            }
             touchEventVisualizer.updateImage();
 
             // output images