Merge branch 'master' of https://git.tk.informatik.tu-darmstadt.de/etri-smartspaces

bbiwarg/Constants.cs

@@ -37,7 +37,8 @@ namespace bbiwarg
 
         // palm detection
         public static readonly float PalmMinDefectMidFingerLineDistance = 20; // defects with mid point ((start + end) / 2) closer than this to a finger line are removed 
-    
+        public static readonly float PalmMaxThumbDefectAngle = 100; // degree
+
         //palm Grid
         public static readonly int PalmGridRows = 4;
         public static readonly int PalmGridColumns = 3;

bbiwarg/Detectors/Palm/PalmDetector.cs

@@ -18,9 +18,8 @@ namespace bbiwarg.Detectors.Palm
     class PalmDetector
     {
         private int width, height;
-        private DepthImage depthImage;
-        private EdgeImage edgeImage;
         private OutputImage outputImage;
+        private EdgeImage edgeImage;
 
         private Image<Gray, Byte> handImage;
         private Image<Gray, Byte> pointingHandMask;
@@ -33,7 +32,7 @@ namespace bbiwarg.Detectors.Palm
         private Vector2D thumbDefectEnd;
         private Vector2D thumbDefectDepth;
         
-        private static Kalman2DPositionFilter thumbDefectDepthFilter, thumbDefectStartFilter, thumbDefectEndFilter;
+        private Kalman2DPositionFilter thumbDefectDepthFilter, thumbDefectStartFilter, thumbDefectEndFilter;
 
         private bool valid = false;
         private Vector2D topLeft;
@@ -41,25 +40,31 @@ namespace bbiwarg.Detectors.Palm
         private Vector2D bottomLeft;
         private Vector2D bottomRight;
 
-        public Quadrangle PalmQuad;
+        public Quadrangle PalmQuad { get; private set; }
 
-        public PalmDetector(DepthImage depthImage, EdgeImage edgeImage, List<Finger> detectedFingers, OutputImage outputImage)
+        public PalmDetector()
         {
             // TODO: determine which fingers are index or thumb-fingers and detect palm in thumb-hand
+            thumbDefectDepthFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
+            thumbDefectStartFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
+            thumbDefectEndFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
+        }
+
+        public void findPalmQuad(DepthImage depthImage, EdgeImage edgeImage, OutputImage outputImage, List<Finger> trackedFingers)
+        {
+            this.width = depthImage.Width;
+            this.height = depthImage.Height;
 
-            width = depthImage.Width;
-            height = depthImage.Height;
-            this.depthImage = depthImage;
             this.edgeImage = edgeImage;
             this.outputImage = outputImage;
 
             // dst = (src > (MaxDepth - MinDepth)) ? 0 : 1
             handImage = depthImage.Image.ThresholdBinaryInv(new Gray(depthImage.MaxDepth - depthImage.MinDepth - 1), new Gray(1)).Convert<Gray, Byte>();
 
-            fingers = getFingersWithoutThumb(detectedFingers);
+            fingers = getFingersWithoutThumb(trackedFingers);
             buildPointingHandMask();
             handImage = handImage.And(pointingHandMask);
-            
+
             findLongestPalmContour();
             if (palmContour != null)
             {
@@ -75,11 +80,11 @@ namespace bbiwarg.Detectors.Palm
             }
         }
 
-        public static void resetFilter()
+        public void resetFilters()
         {
-            thumbDefectDepthFilter = null;
-            thumbDefectStartFilter = null;
-            thumbDefectEndFilter = null;
+            thumbDefectDepthFilter.reset();
+            thumbDefectStartFilter.reset();
+            thumbDefectEndFilter.reset();
         }
 
         private List<Finger> getFingersWithoutThumb(List<Finger> detectedFingers)
@@ -142,7 +147,7 @@ namespace bbiwarg.Detectors.Palm
             return pos;
         }
 
-        //public OutputImage i1, i2, i3, i4, i5, i6, i7, i8, i9;
+        public OutputImage i1, i2, i3, i4, i5, i6, i7, i8, i9;
         private void buildPointingHandMask()
         {
             /*i1 = new OutputImage(width, height);
@@ -193,6 +198,12 @@ namespace bbiwarg.Detectors.Palm
 
         private void findLongestPalmContour()
         {
+            i1 = new OutputImage(width, height);
+            i1.Image[0] = i1.Image[1] = i1.Image[2] = handImage * 255;
+
+            i2 = new OutputImage(width, height);
+            i2.Image[0] = i2.Image[1] = i2.Image[2] = pointingHandMask * 255;
+
             Contour<Point> contour = handImage.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_EXTERNAL);
 
             palmContour = contour;
@@ -242,20 +253,46 @@ namespace bbiwarg.Detectors.Palm
             convexityDefects = newDefects;
         }
 
-        private void findHandPoints()
+        private MCvConvexityDefect? findThumbDefect()
         {
-            if (convexityDefects.Count > 0)
+            foreach (MCvConvexityDefect d in convexityDefects)
             {
-                MCvConvexityDefect thumbDefect = convexityDefects[0];
+                Vector2D depth = new Vector2D(d.DepthPoint);
+                Vector2D start = new Vector2D(d.StartPoint);
+                Vector2D end = new Vector2D(d.EndPoint);
+
+                float angle = (float) ((depth - start).getAngleBetween(depth - end) * 180 / Math.PI);
 
-                thumbDefectDepth = new Vector2D(thumbDefect.DepthPoint);
-                thumbDefectStart = new Vector2D(thumbDefect.StartPoint);
-                thumbDefectEnd = new Vector2D(thumbDefect.EndPoint);
-                if (thumbDefectDepthFilter == null)
+                if (angle <= Constants.PalmMaxThumbDefectAngle)
+                    return d;
+            }
+            return null;
+        }
+
+        private void findHandPoints()
+        {
+            MCvConvexityDefect? thumbDefect = findThumbDefect();
+
+            if (convexityDefects.Count > 0 && (thumbDefect != null || thumbDefectDepthFilter.Initialized))
+            {
+                if (thumbDefect != null)
                 {
-                    thumbDefectDepthFilter = new Kalman2DPositionFilter(thumbDefectDepth, 1.0e-2f, 1.0e-2f);
-                    thumbDefectStartFilter = new Kalman2DPositionFilter(thumbDefectStart, 1.0e-2f, 1.0e-2f);
-                    thumbDefectEndFilter = new Kalman2DPositionFilter(thumbDefectEnd, 1.0e-2f, 1.0e-2f);
+                    thumbDefectDepth = new Vector2D(thumbDefect.Value.DepthPoint);
+                    thumbDefectStart = new Vector2D(thumbDefect.Value.StartPoint);
+                    thumbDefectEnd = new Vector2D(thumbDefect.Value.EndPoint);
+                }
+                
+                if (!thumbDefectDepthFilter.Initialized)
+                {
+                    thumbDefectDepthFilter.setInitialPosition(thumbDefectDepth);
+                    thumbDefectStartFilter.setInitialPosition(thumbDefectStart);
+                    thumbDefectEndFilter.setInitialPosition(thumbDefectEnd);
+                }
+                else if (thumbDefect == null)
+                {
+                    thumbDefectDepth = thumbDefectDepthFilter.getPrediction();
+                    thumbDefectStart = thumbDefectStartFilter.getPrediction();
+                    thumbDefectEnd = thumbDefectEndFilter.getPrediction();
                 }
                 else
                 {
@@ -304,7 +341,11 @@ namespace bbiwarg.Detectors.Palm
 
             if (PalmQuad != null)
             {
-                outputImage.drawLineSegment(new Utility.LineSegment2D(thumbDefectDepth, (thumbDefectStart + thumbDefectEnd) / 2.0f), Constants.PalmThumbDefectColor, 1);
+                outputImage.fillCircle(thumbDefectDepth.IntX, thumbDefectDepth.IntY, 3, Color.Red);
+                outputImage.fillCircle(thumbDefectStart.IntX, thumbDefectStart.IntY, 3, Color.Green);
+                outputImage.fillCircle(thumbDefectEnd.IntX, thumbDefectEnd.IntY, 3, Color.Blue);
+
+                //outputImage.drawLineSegment(new Utility.LineSegment2D(thumbDefectDepth, (thumbDefectStart + thumbDefectEnd) / 2.0f), Constants.PalmThumbDefectColor, 1);
                 
                 Vector2D[] vertices = PalmQuad.Vertices;
                 for (int i = 0; i < 4; ++i)

bbiwarg/Graphics/OutputWindow.cs

@@ -165,7 +165,7 @@ namespace bbiwarg.Graphics
 
             Timer.stop("onRenderFrame");
             
-            Timer.outputAll();
+            //Timer.outputAll();
         }
 
     }

bbiwarg/Graphics/TouchEventVisualizer.cs

@@ -13,57 +13,40 @@ namespace bbiwarg.Graphics
     {
         public OutputImage OutputImage { get; private set; }
 
-        private List<Kalman2DPositionFilter> kalmanFilters;
-        private List<List<Vector2D>> touchPositions;
         private int lastUpdated = -1;
         private int width, height;
 
+        private List<Vector2D> lastTouchPositions;
+        private List<Vector2D> currentTouchPositions;
+        private Kalman2DPositionFilter kalmanFilter;
+
         public TouchEventVisualizer(int width, int height)
         {
             this.width = width;
             this.height = height;
 
             OutputImage = new Graphics.OutputImage(width, height);
-            touchPositions = new List<List<Vector2D>>();
-            kalmanFilters = new List<Kalman2DPositionFilter>();
         }
 
         public void addPalmTouchEvent(PalmTouchEvent e, int currentFrame)
         {
-            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)
+            if (lastUpdated == -1 || (currentFrame - lastUpdated) > 5 || currentTouchPositions == null || kalmanFilter == null)
             {
-                currentFilter = new Kalman2DPositionFilter(e.RelativePalmPosition);
+                lastTouchPositions = currentTouchPositions;
+                currentTouchPositions = new List<Vector2D>();
+                kalmanFilter = new Kalman2DPositionFilter();
+                kalmanFilter.setInitialPosition(e.RelativePalmPosition);
                 pos = e.RelativePalmPosition;
             }
             else
             {
-                pos = currentFilter.getCorrectedPosition(e.RelativePalmPosition);
+                pos = kalmanFilter.getCorrectedPosition(e.RelativePalmPosition);
             }
 
+
             int size = Math.Min(width, height);
-            currentList.Add(new Vector2D((int)(pos.X * size), (int)(size - pos.Y * size)));
-            if (addList)
-            {
-                touchPositions.Add(currentList);
-                kalmanFilters.Add(currentFilter);
-            }
+            currentTouchPositions.Add(new Vector2D((int)(pos.X * size), (int)(size - pos.Y * size)));
 
             lastUpdated = currentFrame;
         }
@@ -72,9 +55,12 @@ namespace bbiwarg.Graphics
         {
             OutputImage = new OutputImage(width, height);
 
+            //-- Border --
             int size = Math.Min(width, height);
             OutputImage.drawRectangle(0, 0, size - 1, size - 1, Color.White);
 
+
+            //-- Grid --
             int numRows = Constants.PalmGridRows;
             int numColumns = Constants.PalmGridColumns;
 
@@ -86,25 +72,34 @@ namespace bbiwarg.Graphics
             for (int i = 1; i < numColumns; i++)
             {
                 int tmp = size / numColumns * i;
-                OutputImage.drawLineSegment(new LineSegment2D(new Vector2D(tmp, 0), new Vector2D(tmp, size-2)), Constants.PalmGridColor);
+                OutputImage.drawLineSegment(new LineSegment2D(new Vector2D(tmp, 0), new Vector2D(tmp, size - 2)), Constants.PalmGridColor);
             }
 
-
-            foreach (List<Vector2D> positions in touchPositions)
+            //-- Current Touch Event --
+            if (currentTouchPositions != null)
             {
-                for (int i = 1; i < positions.Count; ++i)
+                for (int i = 1; i < currentTouchPositions.Count; ++i)
                 {
-                    OutputImage.drawLineSegment(new LineSegment2D(positions[i - 1], positions[i]), Constants.TouchEventVisualizerLineColor);
+                    OutputImage.drawLineSegment(new LineSegment2D(currentTouchPositions[i - 1], currentTouchPositions[i]), Constants.TouchEventVisualizerLineColor);
                 }
-
-                if (touchPositions.Count != 0)
-                    OutputImage.fillCircle(positions.Last<Vector2D>().IntX, positions.Last<Vector2D>().IntY, 3, Constants.TouchEventVisualizerPointColor);
+                OutputImage.fillCircle(currentTouchPositions.Last<Vector2D>().IntX, currentTouchPositions.Last<Vector2D>().IntY, 3, Constants.TouchEventVisualizerPointColor);
+            }
+            //-- Last Touch Event --
+            if (lastTouchPositions != null)
+            {
+                for (int i = 1; i < lastTouchPositions.Count; ++i)
+                {
+                    OutputImage.drawLineSegment(new LineSegment2D(lastTouchPositions[i - 1], lastTouchPositions[i]), Constants.TouchEventVisualizerLineColor);
+                }
+                OutputImage.fillCircle(lastTouchPositions.Last<Vector2D>().IntX, lastTouchPositions.Last<Vector2D>().IntY, 3, Constants.TouchEventVisualizerPointColor);
             }
+            
         }
 
         public void Reset()
         {
-            touchPositions.Clear();
+            lastTouchPositions = null;
+            currentTouchPositions = null;
             OutputImage = new OutputImage(width, height);
             lastUpdated = -1;
         }

bbiwarg/Utility/Kalman2DPositionFilter.cs

@@ -12,21 +12,27 @@ namespace bbiwarg.Utility
     class Kalman2DPositionFilter
     {
         private Kalman kalman;
+        private float measurementNoiseFactor, processNoiseFactor;
+
+        public bool Initialized { get; private set; }
 
         // state: x, y, v_x, v_y
         //   x: current x position
         //   y: current y position
         // v_x: velocity in x direction
         // v_y: velocity in y direction
-        public Kalman2DPositionFilter(Vector2D initialPosition, float measurementNoiseFactor = 1.0e-1f, float processNoiseFactor = 1.0e-4f)
+        public Kalman2DPositionFilter(float measurementNoiseFactor = 1.0e-1f, float processNoiseFactor = 1.0e-4f)
+        {
+            this.measurementNoiseFactor = measurementNoiseFactor;
+            this.processNoiseFactor = processNoiseFactor;
+            reset();
+        }
+
+        public void reset()
         {
             // 4 state variables and 2 measurements (0 controls)
             kalman = new Kalman(4, 2, 0);
 
-            // initial state (x, y, v_x, v_y)
-            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f });
-            kalman.CorrectedState = initialState;
-
             // transition matrix 
             Matrix<float> transitionMatrix = new Matrix<float>(new float[,] 
             { {1.0f, 0.0f, 1.0f, 0.0f},   // x = x + v_x
@@ -56,6 +62,23 @@ namespace bbiwarg.Utility
             Matrix<float> errorCovariancePostMatrix = new Matrix<float>(4, 4);
             errorCovariancePostMatrix.SetIdentity();
             kalman.ErrorCovariancePost = errorCovariancePostMatrix;
+
+            Initialized = false;
+        }
+
+        public void setInitialPosition(Vector2D initialPosition)
+        {
+            // initial state (x, y, v_x, v_y)
+            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f });
+            kalman.CorrectedState = initialState;
+
+            Initialized = true;
+        }
+
+        public Vector2D getPrediction()
+        {
+            Matrix<float> predicton = kalman.Predict();
+            return new Vector2D(predicton[0, 0], predicton[1, 0]);
         }
 
         // updates the filter and returns the corrected position

bbiwarg/VideoHandle.cs

@@ -64,6 +64,8 @@ namespace bbiwarg
 
         public void start()
         {
+            palmDetector = new PalmDetector();
+
             inputProvider.init();
             inputProvider.start();
             inputProvider.updateFrame();
@@ -169,9 +171,10 @@ namespace bbiwarg
 
             //detect palm
             Timer.start("palmDetection");
-            palmDetector = new PalmDetector(depthImage, edgeImage, fingerDetector.Fingers, OutputImages[2]);
-            if (sourceIsMovie() && CurrentFrame == 0)
-                PalmDetector.resetFilter();
+            if (fingerTracker.TrackedFingers.Count >= 2)
+                palmDetector.findPalmQuad(depthImage, edgeImage, OutputImages[2], fingerTracker.TrackedFingers);
+            if (CurrentFrame == 0)
+                palmDetector.resetFilters();
             Timer.stop("palmDetection");
 
             //detect touchEvents