bbiwarg-original/bbiwarg/DepthImage.cs

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

namespace bbiwarg
{
    class DepthImage
    {
        private int width;
        private int height;
        private Image<Gray, Int16> image;
        private Image<Gray, Int16> edges;
        private bool[,] fingerPoints;
        private List<Finger> fingers;

        private Int16 minDepth;
        private Int16 maxDepth;

        public DepthImage(int width, int height, Image<Gray, Int16> image) {
            this.width = width;
            this.height = height;
            this.image = image;

            Stopwatch sw = new Stopwatch();
            sw.Start();

            this.image = this.image.SmoothMedian(3);
            //minDepth
            findMinDepth();

            //maxDepth
            maxDepth = (Int16)(minDepth + 200);
            thresholdDepth(minDepth, maxDepth);

            //edges
            calculateEdges();

            //findFingerPoints
            findFingerPoints();

            //findFingers
            findFingers();

            sw.Stop();
            Console.WriteLine(sw.ElapsedMilliseconds);

        }

        public int getWidth() {
            return width;
        }

        public int getHeight() {
            return height;
        }

        public Int16 getMinDepth() {
            return minDepth;
        }

        public Int16 getMaxDepth() {
            return maxDepth;
        }

        public Int16 getDepthAt(int x, int y) {
            return image.Data[y, x, 0];
        }

        public Int16 getEdgeAt(int x, int y)
        {
            return edges.Data[y, x, 0];
        }

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

        private void setDepthAt(int x, int y, Int16 value) {
            image.Data[y, x, 0] = value;
        }

        private void findMinDepth() {
            minDepth = Int16.MaxValue;
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    Int16 depth = getDepthAt(x,y);
                    if (depth < minDepth)
                        minDepth = depth;
                }
            }
        }

        private void thresholdDepth(int min, int max)
        {
            image = image.ThresholdToZero(new Gray(min));
            image = image.ThresholdTrunc(new Gray(max));

            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    if (getDepthAt(x, y) == 0)
                        setDepthAt(x, y, (short)max);
                }
            }
        }

        private void calculateEdges() {
            Image<Gray, Byte> tmp = image.Convert<Byte>(delegate(Int16 depth) { return (byte)(((depth - minDepth) * Byte.MaxValue) / (maxDepth - minDepth)); });
            Image<Gray, byte> tmp2 = tmp.Canny(100, 75, 3);
            edges = tmp2.Convert<Int16>(delegate(byte depth) { if(depth > 0) {return Int16.MaxValue;} else {return 0;}});//((depth * Int16.MaxValue) / Byte.MaxValue); });
        }

        private void findFingerPoints()
        {
            int maxFingerSize = 30;
            int minFingerSize = 10;
            fingerPoints = new bool[width, height];

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

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

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

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

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

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

                        }

                        //x = edgeRightX - 1; // better performance but looses some points (skips x-values where it has already found a horizontal-fingerPoint, but it will miss any vertical-fingerPoints in this area)
                    }
                }
            }
        }

        private void findFingers() {
            float maxDistanceTogether = 5.0f;
            float minFingerLength = 20.0f;
            List<Finger> possibleFingers = new List<Finger>();

            for (int y = 0; y < height; y++) {
                for (int x = 0; x < width; x++) {
                    if (fingerPoints[x, y]) {
                        Point fingerPoint = new Point(x,y);
                        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].add(fingerPoint);
                        }
                        else 
                        {
                            possibleFingers.Add(new Finger(fingerPoint));
                        }
                    }
                }
            }

            fingers = new List<Finger>();
            fingerPoints = new bool[width, height];
            foreach (Finger finger in possibleFingers) {
                float length = finger.getLength();
                if (length > minFingerLength)
                {
                    fingers.Add(finger);
                    
                    /*foreach (Point fingerPoint in finger.getFingerPoints()) {
                        fingerPoints[fingerPoint.X, fingerPoint.Y] = true;
                    }*/

                    List<Point> points = finger.getFingerPoints();

                    PointF[] pointArray = new PointF[points.Count];
                    int i = 0;
                    foreach (Point p in points) {
                        pointArray[i] = new PointF(p.X, p.Y);
                        ++i;
                    }

                    PointF direction, pointOnLine;
                    PointCollection.Line2DFitting(pointArray, Emgu.CV.CvEnum.DIST_TYPE.CV_DIST_L2, out direction, out pointOnLine);

                    PointF start = projectToLine(finger.getFarthest(this), direction, pointOnLine);
                    PointF end = projectToLine(finger.getNearest(this), direction, pointOnLine);
                    setFingerPoints(start, end);
                }
            }
        }

        private PointF projectToLine(PointF p, PointF direction, PointF pointOnLine)
        {
            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;

            float diff_d = (diffx * dx + diffy * dy);
            float d_d = (dx * dx + dy * dy);
            float q = diff_d / d_d;

            return new PointF(ox + q * dx, oy + q * dy);
        }

        private void setFingerPoints(PointF start, PointF end)
        {
            // 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(getWidth() - 1, Math.Max(0, x)), Math.Min(getHeight() - 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(getWidth() - 1, Math.Max(0, x)), Math.Min(getHeight() - 1, Math.Max(0, y))] = true;
            }
        }
    }
}