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

bbiwarg/DataSource/ForeFingerDetection.cs

@@ -20,15 +20,17 @@ namespace bbiwarg.DataSource
          */
 
         public const int TOLERANCE_Z = 5;
-        public const int TOLERANCE_2D = 5;
+        public const int TOLERANCE_2D = 7;
 
         private IVideoHandle source;
 
+        private List<Vector> points;
         private HashSet<Vector> seenPoints;
         private Vector palmPosition;
         private Vector foreFingerPosition;
         private float foreFingerDistance;
-
+        private float lowestY;
+        private float highestZ;
 
 
         public ForeFingerDetection(IVideoHandle source)
@@ -44,41 +46,73 @@ namespace bbiwarg.DataSource
             palmPosition[1] = palmPosition2D[1];
             palmPosition[2] = source.getDepth((int)palmPosition[0], (int)palmPosition[1]);
 
+            points = new List<Vector>();
             seenPoints = new HashSet<Vector>();
             foreFingerPosition = palmPosition;
             foreFingerDistance = 0;
-            checkPoint(palmPosition);
-            return foreFingerPosition;
-        }
+            lowestY = palmPosition[1];
+            highestZ = palmPosition[2];
 
-        private void checkNeighbours(Vector point)
-        {
-            for (int y = -(int)Math.Min(TOLERANCE_2D, point[1]); y <= TOLERANCE_2D && point[1] + y < source.getHeight(); y++)
+            points.Add(palmPosition);
+            seenPoints.Add(palmPosition);
+            for (int i = 0; i < points.Count(); i++)
             {
-                for (int x = -(int)Math.Min(TOLERANCE_2D, point[0]); x <= TOLERANCE_2D && point[0] + x < source.getWidth(); x++)
-                {
-                    Vector currentPoint = new DenseVector(3);
-                    currentPoint[0] = point[0] + x;
-                    currentPoint[1] = point[1] + y;
-                    currentPoint[2] = source.getDepth((int) currentPoint[0], (int) currentPoint[1]);
-                    if(Math.Abs(currentPoint[2]-point[2]) <= TOLERANCE_Z)
-                      checkPoint(currentPoint);
-                }
+                checkPoint(points[i]);
             }
+
+            return foreFingerPosition;
         }
+
+
         private void checkPoint(Vector point)
         {
-            if (!seenPoints.Contains<Vector>(point))
+            bool isLowestY = point[1] <= lowestY;
+            bool isHighestZ = point[2] >= highestZ;
+            if (isLowestY || isHighestZ)
             {
-                seenPoints.Add(point);
+                if (isLowestY)
+                {
+                    if (lowestY == foreFingerPosition[1])
+                        foreFingerDistance = 0;
+                    lowestY = point[1];
+                }
+                if (isHighestZ)
+                {
+                    if (highestZ == foreFingerPosition[2])
+                        foreFingerDistance = 0;
+                    highestZ = point[2];
+                }
+
                 Vector distanceVector = new DenseVector(3);
-                palmPosition.Subtract(point, distanceVector);
+                source.pixel2VertexPosition(palmPosition).Subtract(source.pixel2VertexPosition(point), distanceVector);
+                //palmPosition.Subtract(point, distanceVector);
+
                 float currentPointDistance = distanceVector.Norm(2);
                 if (currentPointDistance >= foreFingerDistance)
                 {
                     foreFingerPosition = point;
                     foreFingerDistance = currentPointDistance;
-                    checkNeighbours(point);
+                    addNeighbours(point);
+                }
+            }
+        }
+
+        private void addNeighbours(Vector point)
+        {
+            Vector currentPoint;
+            for (int y = -(int)Math.Min(TOLERANCE_2D, point[1]); y <= TOLERANCE_2D && point[1] + y < source.getHeight(); y++)
+            {
+                for (int x = -(int)Math.Min(TOLERANCE_2D, point[0]); x <= TOLERANCE_2D && point[0] + x < source.getWidth(); x++)
+                {
+                    currentPoint = new DenseVector(3);
+                    currentPoint[0] = point[0] + x;
+                    currentPoint[1] = point[1] + y;
+                    currentPoint[2] = source.getDepth((int)currentPoint[0], (int)currentPoint[1]);
+                    if (Math.Abs(currentPoint[2] - point[2]) <= TOLERANCE_Z && !seenPoints.Contains(currentPoint))
+                    {
+                        points.Add(currentPoint);
+                        seenPoints.Add(currentPoint);
+                    }
                 }
             }
         }