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

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

namespace bbiwarg.Detectors.Fingers
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImage;
        private FingerImage fingerImage;
        private List<Finger> fingers;
        Image<Gray, byte> fingerPoints;

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

            findFingerPoints();
            findFingers();
        }

        public List<Finger> getFingers() {
            return fingers;
        }

        private void findFingerPoints() {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            fingerPoints = new Image<Gray, byte>(width, height);
            
            for (int y = 0; y < height; y++) {
                for (int x = 0; x < width; x++) {
                    if (edgeImage.isEdgeAt(x, y))
                    {
                        Vector2D startPoint = new Vector2D(x, y);
                        searchFingerPoint(startPoint, new Vector2D(1, 0), fingerPoints);
                        searchFingerPoint(startPoint, new Vector2D(0, 1), fingerPoints);
                        //searchFingerPoint(startPoint, new Vector2D(1, -1), fingerPoints);
                        //searchFingerPoint(startPoint, new Vector2D(1, 1), fingerPoints);
                    }
                }
            }
        }

        private void searchFingerPoint(Vector2D start, Vector2D direction, Image<Gray, byte> fingerPointsImage) {
            int width = fingerPointsImage.Width;
            int height = fingerPointsImage.Height;
            float minFingerSize = 10;
            float maxFingerSize = 30;
            float stepSize = direction.getLength();


            bool edgeFound = false;
            Vector2D edge = start + minFingerSize * direction;
            float index = minFingerSize;
            float maxXIndex = (direction.x == 0) ? float.MaxValue : ((direction.x > 0) ? ((width - start.x) / direction.x) : (-start.x / direction.x));
            float maxYIndex = (direction.y == 0) ? float.MaxValue : ((direction.y > 0) ? ((height - start.y) / direction.y) : (-start.y / direction.y));

            float maxIndex = Math.Min(maxFingerSize, Math.Min(maxXIndex, maxYIndex));
            
            while (!edgeFound && index < maxIndex) {
                if (edgeImage.isEdgeAt((int)edge.x, (int)edge.y))
                    edgeFound = true;
                else {
                    index += stepSize;
                    edge += direction;
                }
            }
            
            if (edgeFound && fingerDepthTest(start, edge))
            {
                Vector2D mid = 0.5f * (start + edge);
                fingerPointsImage.Data[(int)mid.y, (int)mid.x, 0] = byte.MaxValue;
                fingerImage.setFingerAt((int)mid.x, (int)mid.y, FingerImageState.possibleFinger);
            }
        }

        private bool fingerDepthTest(Vector2D p1, Vector2D p2) {
            Vector2D mid = 0.5f * (p1 + p2);
            Int16 depthP1 = depthImage.getDepthAt((int)p1.x, (int)p1.y);
            Int16 depthMid = depthImage.getDepthAt((int)mid.x, (int)mid.y);
            Int16 depthP2 = depthImage.getDepthAt((int)p2.x, (int)p2.y);
            return (depthP1 > depthMid && depthMid < depthP2);
        }

        private void findFingers()
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            fingerPoints = fingerPoints.Dilate(1);

            double rhoResolution = 1;
            double thetaResolution = Math.PI / 90.0;
            int threshold = 10;
            double minLineWidth = 20;
            double gapBetweenLines = 2;

            LineSegment2D[] lines = fingerPoints.HoughLinesBinary(rhoResolution, thetaResolution, threshold, minLineWidth, gapBetweenLines)[0];

            fingers = new List<Finger>();

            foreach (LineSegment2D line in lines) {
                Vector2D p1 = new Vector2D(line.P1.X, line.P1.Y);
                Vector2D p2 = new Vector2D(line.P2.X, line.P2.Y);
                Finger finger = new Finger(p1, p2);

                List<Finger> mergeableFingers = new List<Finger>();

                foreach (Finger f in fingers) {
                    if (finger.isMergeable(f))
                        mergeableFingers.Add(f);
                }

                if (mergeableFingers.Count == 0)
                    fingers.Add(finger);
                else if (mergeableFingers.Count == 1)
                    mergeableFingers[0].mergeFingers(finger);
                else {
                    foreach (Finger mf in mergeableFingers) {
                        finger.mergeFingers(mf);
                        fingers.Remove(mf);
                    }
                    fingers.Add(finger);
                }
            }

            //draw fingers in fingerImage
            foreach(Finger finger in fingers) {
                fingerImage.drawLine(finger.getTipPoint(), finger.getHandPoint(), FingerImageState.fingerDetected);
            }
        }
    }
}