bbiwarg-original/bbiwarg/Detectors/Finger/FingerDetector.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Drawing;
using Emgu.CV;
using Emgu.CV.Structure;
using bbiwarg.Images;

namespace bbiwarg.Detectors.Finger
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImage;
        private bool[,] possibleFingerPoints;
        private bool[,] fingerPoints;
        private List<Finger> possibleFingers;
        private List<Finger> fingers;

        public FingerDetector(DepthImage depthImage, EdgeImage edgeImage) {
            this.depthImage = depthImage;
            this.edgeImage = edgeImage;

            findPossibleFingerPoints();
            findPossibleFingers();
            setFingers();
            setFingerPoints();
        }

        public bool isPossibleFingerPointAt(int x, int y) {
            return possibleFingerPoints[x, y];
        }

        public bool isFingerPointAt(int x, int y) {
            return fingerPoints[x, y];
        }

        private void findPossibleFingerPoints()
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();
            int maxFingerSize = 30;
            int minFingerSize = 10;
            possibleFingerPoints = new bool[width, height];

            for (int y = 0; y < height; y++) {
                for (int x = 0; x < width; x++) {
                    if (edgeImage.isEdgeAt(x,y)) {
                        //search horizontal
                        bool edgeRightFound = false;
                        int edgeRightX = x + minFingerSize;
                        while (!edgeRightFound && edgeRightX < width) {
                            if (edgeImage.isEdgeAt(edgeRightX,y))
                                edgeRightFound = true;
                            else
                                edgeRightX++;
                        }

                        if (edgeRightFound){
                            int midX = (edgeRightX + x) / 2;
                            Int16 depthLeft = depthImage.getDepthAt(x, y);
                            Int16 depthMid = depthImage.getDepthAt(midX, y);
                            Int16 depthRight = depthImage.getDepthAt(edgeRightX, y);

                            if ((edgeRightX - x) < maxFingerSize && depthLeft > depthMid && depthMid < depthRight) {
                                possibleFingerPoints[midX, y] = true;
                            } 
                        }

                        //search vertical
                        bool edgeBottomFound = false;
                        int edgeBottomY = y + minFingerSize;
                        while (!edgeBottomFound && edgeBottomY < height) {
                            if (edgeImage.isEdgeAt(x, edgeBottomY))
                                edgeBottomFound = true;
                            else
                                edgeBottomY++;
                        }

                        if (edgeBottomFound) {
                            int midY = (edgeBottomY + y) / 2;
                            Int16 depthTop = depthImage.getDepthAt(x, y);
                            Int16 depthMid = depthImage.getDepthAt(x, midY);
                            Int16 depthBottom = depthImage.getDepthAt(x, edgeBottomY);

                            if ((edgeBottomY - y) < maxFingerSize && depthTop > depthMid && depthMid < depthBottom) {
                                possibleFingerPoints[x, midY] = true;
                            }

                        }
                    }
                }
            }
        }

        private void findPossibleFingers()
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();
            float maxDistanceTogether = 5.0f;
            possibleFingers = new List<Finger>();

            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    if (possibleFingerPoints[x, y])
                    {
                        Int16 depth = depthImage.getDepthAt(x, y);
                        FingerPoint fingerPoint = new FingerPoint(x,y,depth);
                        float minDistanceValue = float.MaxValue;
                        int minDistanceIndex = 0;
                        for (int i = 0; i < possibleFingers.Count; i++)
                        {
                            float distance = possibleFingers[i].getMinDistance(fingerPoint);
                            if (distance < minDistanceValue)
                            {
                                minDistanceValue = distance;
                                minDistanceIndex = i;
                            }
                        }
                        if (minDistanceValue < maxDistanceTogether)
                        {
                            possibleFingers[minDistanceIndex].addFingerPoint(fingerPoint);
                        }
                        else
                        {
                            possibleFingers.Add(new Finger(fingerPoint));
                        }
                    }
                }
            }
        }

        private void setFingers()
        {   
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();
            float minFingerLength = 20.0f;
            fingers = new List<Finger>();

            foreach (Finger finger in possibleFingers)
            {
                float length = finger.getLength();
                if (length > minFingerLength)
                {
                    fingers.Add(finger);
                }
            }      
        }

        private void setFingerPoints() {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();
            fingerPoints = new bool[width, height];

            foreach (Finger finger in fingers) {
                PointF start = finger.getStart();
                PointF end = finger.getEnd();
                drawFingerPointLine(start, end);
            }
        }



        private void drawFingerPointLine(PointF start, PointF end)
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            // bresenham from wikipedia
            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 */
            dx = xend - xstart;
            dy = yend - ystart;

            /* Vorzeichen des Inkrements bestimmen */
            incx = dx < 0 ? -1 : 1;
            incy = dy < 0 ? -1 : 1;
            if (dx < 0) dx = -dx;
            if (dy < 0) dy = -dy;

            /* feststellen, welche Entfernung größer ist */
            if (dx > dy)
            {
                /* x ist schnelle Richtung */
                pdx = incx; pdy = 0;    /* pd. ist Parallelschritt */
                ddx = incx; ddy = incy; /* dd. ist Diagonalschritt */
                es = dy; el = dx;   /* Fehlerschritte schnell, langsam */
            }
            else
            {
                /* y ist schnelle Richtung */
                pdx = 0; pdy = incy; /* pd. ist Parallelschritt */
                ddx = incx; ddy = incy; /* dd. ist Diagonalschritt */
                es = dx; el = dy;   /* Fehlerschritte schnell, langsam */
            }

            /* Initialisierungen vor Schleifenbeginn */
            x = xstart;
            y = ystart;
            err = el / 2;
            fingerPoints[Math.Min(width - 1, Math.Max(0, x)), Math.Min(height - 1, Math.Max(0, y))] = true;

            /* Pixel berechnen */
            for (t = 0; t < el; ++t) /* t zaehlt die Pixel, el ist auch Anzahl */
            {
                /* Aktualisierung Fehlerterm */
                err -= es;
                if (err < 0)
                {
                    /* Fehlerterm wieder positiv (>=0) machen */
                    err += el;
                    /* Schritt in langsame Richtung, Diagonalschritt */
                    x += ddx;
                    y += ddy;
                }
                else
                {
                    /* Schritt in schnelle Richtung, Parallelschritt */
                    x += pdx;
                    y += pdy;
                }
                fingerPoints[Math.Min(width - 1, Math.Max(0, x)), Math.Min(height - 1, Math.Max(0, y))] = true;
            }
        }

    }
}