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

Conflicts:
	bbiwarg/MainBBWIWARG.cs

bbiwarg/Detectors/Fingers/Finger.cs

@@ -7,48 +7,50 @@ using System.Threading.Tasks;
 using Emgu.CV;
 using Emgu.CV.Structure;
 using bbiwarg.Images;
+using bbiwarg.Utility;
 
 namespace bbiwarg.Detectors.Fingers
 {
     class Finger
     {
-        private List<FingerPoint> fingerPoints;
+        private List<Vector<int>> fingerPoints;
         private bool lineUpToDate = false;
-        private PointF direction;
-        private PointF pointOnLine;
-        private PointF lineEndPoint1;
-        private PointF lineEndPoint2;
-        private FingerPoint tipPoint;
-        private FingerPoint handPoint;
+        private Vector<float> direction;
+        private Vector<float> pointOnLine;
+        private Vector<float> lineEndPoint1;
+        private Vector<float> lineEndPoint2;
+        private Vector<int> tipPoint;
+        private Vector<int> handPoint;
         private float length;
         
-        public Finger(FingerPoint fingerPoint)
+        public Finger(Vector<int> fingerPoint)
         {
-            fingerPoints = new List<FingerPoint>();
+            fingerPoints = new List<Vector<int>>();
             addFingerPoint(fingerPoint);
         }
 
-        public PointF getLineEndPoint1()
+        public Vector<float> getLineEndPoint1()
         {
             if (!lineUpToDate) updateLine();
             return lineEndPoint1;
         }
 
-        public PointF getLineEndPoint2()
+        public Vector<float> getLineEndPoint2()
         {
             if (!lineUpToDate) updateLine();
             return lineEndPoint2;
         }
 
-        public FingerPoint getTipPoint()
+        public Vector<float> getDirection()
         {
             if (!lineUpToDate) updateLine();
-            return tipPoint;
+            return direction;
         }
 
-        public PointF getDirection() {
+        public Vector<int> getTipPoint()
+        {
             if (!lineUpToDate) updateLine();
-            return direction;
+            return tipPoint;
         }
 
         public float getLength()
@@ -57,18 +59,18 @@ namespace bbiwarg.Detectors.Fingers
             return length;
         }
 
-        public void addFingerPoint(FingerPoint fingerPoint)
+        public void addFingerPoint(Vector<int> fingerPoint)
         {
             fingerPoints.Add(fingerPoint);
             lineUpToDate = false;
         }
 
-        public float getMinDistance(FingerPoint fingerPoint)
+        public float getMinDistance(Vector<int> fingerPoint)
         {
             float minDinstance = float.MaxValue;
-            foreach (FingerPoint fp in fingerPoints)
+            foreach (Vector<int> fp in fingerPoints)
             {
-                float distance = fp.getDistanceTo(fingerPoint);
+                float distance = fingerPoint.subDistance(fp, 2);
                 if (distance < minDinstance)
                 {
                     minDinstance = distance;
@@ -81,23 +83,27 @@ namespace bbiwarg.Detectors.Fingers
         public float getSimilarity(Finger compareFinger) {
             //startDistance
             float maxStartDistance = 100;
-            float xDiffStart = lineEndPoint1.X - compareFinger.getLineEndPoint1().X;
-            float yDiffStart = lineEndPoint1.X - compareFinger.getLineEndPoint1().Y;
+            float xDiffStart = lineEndPoint1.x - compareFinger.getLineEndPoint1().x;
+            float yDiffStart = lineEndPoint1.y - compareFinger.getLineEndPoint1().y;
             float startDistance = (float)Math.Sqrt(xDiffStart * xDiffStart + yDiffStart * yDiffStart);
             float startSimilarity = Math.Max(1 - (startDistance / maxStartDistance), 0);
 
             //endDistance
-            float maxEndDistance = 50;
-            float xDiffEnd = lineEndPoint2.X - compareFinger.getLineEndPoint2().X;
-            float yDiffEnd = lineEndPoint2.X - compareFinger.getLineEndPoint2().Y;
+            float maxEndDistance = 100;
+            float xDiffEnd = lineEndPoint2.x - compareFinger.getLineEndPoint2().x;
+            float yDiffEnd = lineEndPoint2.y - compareFinger.getLineEndPoint2().y;
             float endDistance = (float)Math.Sqrt(xDiffEnd * xDiffEnd + yDiffEnd * yDiffEnd);
             float endSimilarity = Math.Max(1 - (endDistance / maxEndDistance), 0);
 
             //direction
+            float scalaProduct = direction * compareFinger.getDirection();
+            float lengthDirection = direction.norm();
+            float lengthCompareDirection = compareFinger.getDirection().norm();
+            float directionSimilarity = Math.Abs(scalaProduct / (lengthDirection * lengthCompareDirection));
 
+            //Console.WriteLine(Math.Round(directionSimilarity, 2) + "###" + Math.Round(startSimilarity, 2) + "###" + Math.Round(endSimilarity, 2));
 
-
-            return (0.2f*startSimilarity + 0.8f*endSimilarity);
+            return (startSimilarity + endSimilarity + directionSimilarity) / 3;
             
         }
 
@@ -105,19 +111,23 @@ namespace bbiwarg.Detectors.Fingers
             //update direction+pointonline
             PointF[] pointArray = new PointF[fingerPoints.Count];
             int i = 0;
-            foreach (FingerPoint fp in fingerPoints)
+            foreach (Vector<int> fp in fingerPoints)
             {
-                pointArray[i] = new PointF(fp.getX(), fp.getY());
+                pointArray[i] = new PointF(fp.x, fp.y);
                 ++i;
             }
-            PointCollection.Line2DFitting(pointArray, Emgu.CV.CvEnum.DIST_TYPE.CV_DIST_L2, out direction, out pointOnLine);
-
-            FingerPoint fp1 = fingerPoints[0];
-            FingerPoint fp2 = fingerPoints[0];
+            PointF tempDirection;
+            PointF tempPointOnLine;
+            PointCollection.Line2DFitting(pointArray, Emgu.CV.CvEnum.DIST_TYPE.CV_DIST_L2, out tempDirection, out tempPointOnLine);
+            direction = new Vector<float>(new float[2]{tempDirection.X, tempDirection.Y});
+            pointOnLine = new Vector<float>(new float[2] { tempPointOnLine.X, tempPointOnLine.Y });
+
+            Vector<int> fp1 = fingerPoints[0];
+            Vector<int> fp2 = fingerPoints[0];
             length = 0.0f;
-            foreach (FingerPoint fp in fingerPoints) {
-                float distanceToFP1 = fp.getDistanceTo(fp1);
-                float distanceToFP2 = fp.getDistanceTo(fp2);
+            foreach (Vector<int> fp in fingerPoints) {
+                float distanceToFP1 = fp.subDistance(fp1,2);
+                float distanceToFP2 = fp.subDistance(fp2,2);
                 if (length < distanceToFP1 && distanceToFP1 >= distanceToFP2) 
                 {
                     fp2 = fp;
@@ -130,7 +140,7 @@ namespace bbiwarg.Detectors.Fingers
                 }
             }
 
-            if (fp1.getY() < fp2.getY())
+            if (fp1.y < fp2.y)
             {
                 tipPoint = fp1;
                 handPoint = fp2;
@@ -142,15 +152,15 @@ namespace bbiwarg.Detectors.Fingers
             }
 
             //update start+end
-            lineEndPoint1 = projectToLine(new PointF(tipPoint.getX(), tipPoint.getY()));
-            lineEndPoint2 = projectToLine(new PointF(handPoint.getX(), handPoint.getY()));
+            lineEndPoint1 = projectToLine(tipPoint);
+            lineEndPoint2 = projectToLine(handPoint);
 
             lineUpToDate = true;
         }
 
-        private PointF projectToLine(PointF p)
+        private Vector<float> projectToLine(Vector<int> p)
         {
-            float px = p.X, py = p.Y, dx = direction.X, dy = direction.Y, ox = pointOnLine.X, oy = pointOnLine.Y;
+            float px = p.x, py = p.y, dx = direction.x, dy = direction.y, ox = pointOnLine.x, oy = pointOnLine.y;
             float diffx = px - ox;
             float diffy = py - oy;
 
@@ -158,7 +168,10 @@ namespace bbiwarg.Detectors.Fingers
             float d_d = (dx * dx + dy * dy);
             float q = diff_d / d_d;
 
-            return new PointF(ox + q * dx, oy + q * dy);
+            float newX = ox + q * dx;
+            float newY = oy + q * dy;
+
+            return new Vector<float>(new float[2]{newX, newY});
         }
     }
 }

bbiwarg/Detectors/Fingers/FingerDetector.cs

@@ -7,6 +7,7 @@ using System.Drawing;
 using Emgu.CV;
 using Emgu.CV.Structure;
 using bbiwarg.Images;
+using bbiwarg.Utility;
 
 namespace bbiwarg.Detectors.Fingers
 {
@@ -104,7 +105,7 @@ namespace bbiwarg.Detectors.Fingers
                     if (fingerImage.getStateAt(x,y) == FingerImageState.possibleFinger)
                     {
                         Int16 depth = depthImage.getDepthAt(x, y);
-                        FingerPoint fingerPoint = new FingerPoint(x,y,depth);
+                        Vector<int> fingerPoint = new Vector<int>(new int[3]{x,y,depth});
                         float minDistanceValue = float.MaxValue;
                         int minDistanceIndex = 0;
                         for (int i = 0; i < possibleFingers.Count; i++)
@@ -151,8 +152,8 @@ namespace bbiwarg.Detectors.Fingers
             int height = depthImage.getHeight();
 
             foreach (Finger finger in fingers) {
-                PointF lineEndPoint1 = finger.getLineEndPoint1();
-                PointF lineEndPoint2 = finger.getLineEndPoint2();
+                Vector<float> lineEndPoint1 = finger.getLineEndPoint1();
+                Vector<float> lineEndPoint2 = finger.getLineEndPoint2();
                 fingerImage.drawLine(lineEndPoint1, lineEndPoint2, FingerImageState.fingerDetected);
             }
         }

bbiwarg/Detectors/Fingers/FingerPoint.cs

@@ -1,41 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-
-namespace bbiwarg.Detectors.Fingers
-{
-    class FingerPoint
-    {
-        private int x;
-        private int y;
-        private Int16 depth;
-
-        public FingerPoint(int x, int y, Int16 depth) {
-            this.x = x;
-            this.y = y;
-            this.depth = depth;
-        }
-
-        public int getX() {
-            return x;
-        }
-
-        public int getY() {
-            return y;
-        }
-
-        public Int16 getDepth() {
-            return depth;
-        }
-
-        public float getDistanceTo(FingerPoint fingerPoint) {
-            int xDiff = x - fingerPoint.getX();
-            int yDiff = y - fingerPoint.getY();
-            float distance = (float)Math.Sqrt(xDiff * xDiff + yDiff * yDiff);
-            return distance;
-        }
-
-    }
-}

bbiwarg/Detectors/Fingers/FingerTracker.cs

@@ -44,7 +44,7 @@ namespace bbiwarg.Detectors.Fingers
 
         private void findTrackedFingers()
         {
-            float minSimilarity = 0.6f;
+            float minSimilarity = 0.75f;
             trackedFingers = new List<Finger>();
 
             foreach (Finger finger in detectedFingers[0])

bbiwarg/Detectors/Touch/TouchDetector.cs

@@ -8,6 +8,7 @@ using Emgu.CV;
 using Emgu.CV.Structure;
 using bbiwarg.Images;
 using bbiwarg.Detectors.Fingers;
+using bbiwarg.Utility;
 
 namespace bbiwarg.Detectors.Touch
 {
@@ -26,18 +27,20 @@ namespace bbiwarg.Detectors.Touch
             float floodValueThreshold = 0.5f;
 
             foreach (Finger finger in fingers) {
-                FingerPoint tipPoint = finger.getTipPoint();
+                 Vector<int> tipPoint = finger.getTipPoint();
 
-                float floodValue = getFloodValue(tipPoint.getX(), tipPoint.getY());
+                float floodValue = getFloodValue(tipPoint.x, tipPoint.y);
                 if (floodValue > floodValueThreshold)
                 {
-                    PointF direction = finger.getDirection();
+                    //correct touchEvent position
+                    Vector<float> direction = finger.getDirection();
                     float directionFactor = -10;
-                    PointF tep = new PointF(Math.Min(Math.Max(tipPoint.getX() + directionFactor*direction.X, 0), depthImage.getWidth()), Math.Min(Math.Max(tipPoint.getY() + directionFactor*direction.Y, 0), depthImage.getHeight()));
-                    
+                    float x = Math.Min(Math.Max(tipPoint.x + directionFactor * direction.x, 0), depthImage.getWidth());
+                    float y = Math.Min(Math.Max(tipPoint.y + directionFactor * direction.y, 0), depthImage.getHeight());
+                    Vector<float> tep = new Vector<float>(new float[2]{x,y});
 
-                    touchImage.setTouchAt((int)tep.X, (int)tep.Y, TouchImageState.touchDetected);
-                    TouchEvent touchEvent = new TouchEvent((int)tep.X, (int)tep.Y, floodValue, finger);
+                    touchImage.setTouchAt((int)tep.x, (int)tep.y, TouchImageState.touchDetected);
+                    TouchEvent touchEvent = new TouchEvent((int)tep.x, (int)tep.y, floodValue, finger);
                     touchEvents.Add(touchEvent);
                 }
             }

bbiwarg/Detectors/Touch/TouchTracker.cs

@@ -15,7 +15,7 @@ namespace bbiwarg.Detectors.Touch
         private int framesUntilTracked;
 
         public TouchTracker() {
-            framesUntilTracked = 3;
+            framesUntilTracked = 2;
             detectedTouchEvents = new List<TouchEvent>[framesUntilTracked];
 
             for (int i = 0; i < framesUntilTracked; i++) {
@@ -51,7 +51,7 @@ namespace bbiwarg.Detectors.Touch
                 {
                     touchImage.setTouchAt(te.getX(), te.getY(), TouchImageState.touchTracked);
                     trackedTouchEvents.Add(te);
-                    Console.WriteLine("touch tracked at x:" + te.getX() + " y:" + te.getY() + " [floodValue:" + te.getFloodValue() + "]");
+                    //Console.WriteLine("touch tracked at x:" + te.getX() + " y:" + te.getY() + " [floodValue:" + te.getFloodValue() + "]");
                 }
             }
         }

bbiwarg/Images/FingerImage.cs

@@ -6,6 +6,8 @@ using System.Text;
 using System.Threading.Tasks;
 using Emgu.CV;
 using Emgu.CV.Structure;
+using bbiwarg.Utility;
+
 namespace bbiwarg.Images
 {
     public enum FingerImageState {
@@ -31,13 +33,13 @@ namespace bbiwarg.Images
             return (FingerImageState)image.Data[y, x, 0];
         }
 
-        public void drawLine(PointF start, PointF end, FingerImageState state)
+        public void drawLine(Vector<float> start, Vector<float> end, FingerImageState state)
         {
             int width = image.Width;
             int height = image.Height;
 
             // bresenham from wikipedia
-            int xstart = (int)start.X, xend = (int)end.X, ystart = (int)start.Y, yend = (int)end.Y;
+            int xstart = (int)start.x, xend = (int)end.x, ystart = (int)start.y, yend = (int)end.y;
             int x, y, t, dx, dy, incx, incy, pdx, pdy, ddx, ddy, es, el, err;
 
             /* Entfernung in beiden Dimensionen berechnen */

bbiwarg/MainBBWIWARG.cs

@@ -12,7 +12,7 @@ namespace bbiwarg
     {
         static void Main(string[] args)
         {
-            IInputProvider inputProvider = new IisuInputProvider("..\\..\\videos\\touch\\1.skv");
+            IInputProvider inputProvider = new IisuInputProvider();//"..\\..\\videos\\touch\\4.skv");
             VideoHandle videoHandle = new VideoHandle(inputProvider);
             videoHandle.start();
 

bbiwarg/Utility/Vector.cs

@@ -104,18 +104,47 @@ namespace bbiwarg.Utility
             }
         }
         public T norm()
+        {
+            return subNorm(elements.Length);
+        }
+        public T subNorm(int subLength)
         {
             T result = (dynamic)0;
-            for (int i = 0; i < elements.Length; i++)
+            for (int i = 0; i < subLength; i++)
             {
                 result += (dynamic)elements[i] * elements[i];
             }
-            return Math.Sqrt((dynamic)result);
+            return (T)Math.Sqrt((dynamic)result);
         }
         public T distance(Vector<T> vector)
         {
-            return (this - vector).norm();
+            return (vector - this).norm();
+        }
+        public T sum()
+        {
+            T result = (dynamic)0;
+            for (int i = 0; i < this.length(); i++)
+            {
+                result += (dynamic)elements[i];
+            }
+            return result;
         }
+        public Vector<T> normalize() 
+        {
+            T norm = this.norm();
+            Vector<T> result = new Vector<T>(this);
+            for (int i = 0; i < result.length(); i++)
+            {
+                result[i] = (result[i] / (dynamic)norm);
+            }
+            return result;
+        }
+
+        public T subDistance(Vector<T> vector, int subLength)
+        {
+            return (vector - this).subNorm(subLength);
+        }
+
 
         private static void checkLength(Vector<T> vector1, Vector<T> vector2)
         {
@@ -149,6 +178,24 @@ namespace bbiwarg.Utility
             }
             return result;
         }
+        public static bool operator ==(Vector<T> vector1, Vector<T> vector2)
+        {
+            checkLength(vector1, vector2);
+            bool result = true;
+            for (int i = 0; i < vector1.length(); i++)
+            {
+                if (vector1[i] != (dynamic)vector2[i])
+                {
+                    result = false;
+                    break;
+                }
+            }
+            return result;
+        }
+        public static bool operator !=(Vector<T> vector1, Vector<T> vector2)
+        {
+            return !(vector1 == vector2);
+        }
         public static Vector<T> crossProduct(Vector<T> vector1, Vector<T> vector2)
         {
             if (vector1.length() != 3 || vector2.length() != 3)
@@ -161,5 +208,46 @@ namespace bbiwarg.Utility
 
             return result;
         }
+        public static Vector<T> pointwiseMultiply(Vector<T> vector1, Vector<T> vector2)
+        {
+            checkLength(vector1, vector2);
+            Vector<T> result = new Vector<T>(vector1);
+            for (int i = 0; i < vector1.length(); i++)
+            {
+                result[i] = (dynamic)result[i] * vector2[i];
+            }
+            return result;
+        }
+        public static float distancePointPlane(Vector<T> point, Vector<T> planeA, Vector<T> planeB, Vector<T> planeC)
+        {
+            //TODO - Diese funktion funktioniert nur mit T = float, 
+            //die normalisierte Normale einer Ebene macht nur mit floats sinn (werte zwischen 0 und 1)
+            if (point.length() != 3 || planeA.length() != 3 || planeB.length() != 3 || planeC.length() != 3)
+                throw new ArgumentException("The vectors' length should be 3");
+
+            Vector<T> ab = planeB - planeA;
+            Vector<T> ac = planeC - planeA;
+            Vector<T> normal = crossProduct(ab, ac).normalize();
+            
+
+            Vector<T> temp = point - planeA;
+            temp = pointwiseMultiply(temp, normal);
+            temp = pointwiseMultiply(temp, normal);
+            T sum = temp.sum();
+
+            temp = pointwiseMultiply(normal, normal);
+            T sumR = temp.sum();
+
+            if (sumR == (dynamic)0)
+                throw new ArgumentException("the points do not clamp an Plane");
+
+            float distance = (sum * (dynamic)(-1)) / sumR;
+
+            if (distance < 0)
+                distance *= (float)(-1);
+
+            return distance;
+
+        }
     }
 }

bbiwarg/VideoHandle.cs

@@ -126,6 +126,7 @@ namespace bbiwarg
 
             //detect+track touchEvents
             touchDetector = new TouchDetector(fingerTracker.getFingers(), depthImage, touchImage);
+            //touchDetector = new TouchDetector(fingerDetector.getFingers(), depthImage, touchImage);
             touchTracker.setDetectedTouchEventsThisFrame(touchDetector.getTouchEvents(), touchImage);
 
             palmImage = new PalmImage(edgeImage);

bbiwarg/bbiwarg.csproj

@@ -68,7 +68,6 @@
   <ItemGroup>
     <Compile Include="Detectors\Fingers\Finger.cs" />
     <Compile Include="Detectors\Fingers\FingerDetector.cs" />
-    <Compile Include="Detectors\Fingers\FingerPoint.cs" />
     <Compile Include="Detectors\Fingers\FingerTracker.cs" />
     <Compile Include="Detectors\Touch\TouchDetector.cs" />
     <Compile Include="Detectors\Touch\TouchEvent.cs" />