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

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

namespace bbiwarg.Detectors.Fingers
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImage;
        private OutputImage outputImage;
        public List<Finger> Fingers { get; private set; }

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

            findFingers();
        }

        private void findFingers()
        {
            int minNumSlices = 20;
            int width = depthImage.Width;
            int height = depthImage.Height;
            int maxX = width - 1;
            int maxY = height - 1;

            Fingers = new List<Finger>();

            for (int y = 1; y < maxY; y++)
            {
                for (int x = 1; x < maxX; x++)
                {
                    if (edgeImage.isEdgeAt(x, y))
                    {
                        Vector2D edgePoint = new Vector2D(x, y);
                        Vector2D edgeDirection = getEdgeDirection(edgePoint);
                        if (edgeDirection != null)
                        {
                            Vector2D dir = edgeDirection.getOrthogonal(true);
                            if (depthImage.getDepthAt(edgePoint - dir) < depthImage.getDepthAt(edgePoint + dir))
                                dir = dir.getInverse();

                            FingerSlice slice = findFingerSliceFromStartEdge(edgePoint, dir);
                            if (slice != null)
                            {
                                FingerSliceTrail trail = findFingerSliceTrail(slice, edgeDirection);
                                if (trail != null && trail.NumSlices > minNumSlices)
                                    createFingerFromTrail(trail);
                            }
                        }
                    }
                }
            }
        }

        private Vector2D getEdgeDirection(Vector2D edgePoint)
        {
            int x = edgePoint.IntX;
            int y = edgePoint.IntY;

            if (edgeImage.isEdgeAt(x, y - 1) && edgeImage.isEdgeAt(x, y + 1)) return new Vector2D(0, 1);
            else if (edgeImage.isEdgeAt(x - 1, y) && edgeImage.isEdgeAt(x + 1, y)) return new Vector2D(1, 0);
            else if (edgeImage.isEdgeAt(x - 1, y - 1) && edgeImage.isEdgeAt(x + 1, y + 1)) return new Vector2D(1, 1).normalize();
            else if (edgeImage.isEdgeAt(x + 1, y - 1) && edgeImage.isEdgeAt(x - 1, y + 1)) return new Vector2D(1, -1).normalize();
            else return null;
        }

        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            int minNumSlicesForCorrection = 15;
            int numRemoveForCorrection = 3;

            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

            Vector2D position = startSlice.Mid + startDirection;
            Vector2D direction = startDirection;

            if (position.isWithin(0, 0, maxX, maxY))
            {
                FingerSlice nextSlice = findFingerSliceFromMid(position, direction);
                if (nextSlice != null)
                {
                    trail.addSlice(nextSlice);
                    trail = expandTrail(trail);

                    if (trail.NumSlices > minNumSlicesForCorrection)
                    {
                        trail.Slices.Reverse();
                        trail.Slices.RemoveRange(0, numRemoveForCorrection);
                        trail = expandTrail(trail);
                        trail.Slices.Reverse();
                        return trail;
                    }
                }
            }

            return null;

        }

        private FingerSliceTrail expandTrail(FingerSliceTrail trail)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            int numDirectionsForAverage = 10;

            List<Vector2D> directions = new List<Vector2D>();
            int numDirections = Math.Min(trail.NumSlices - 1, numDirectionsForAverage);
            for (int i = 0; i < numDirections; i++)
            {
                directions.Add(trail.Slices[i + 1].Mid - trail.Slices[i].Mid);
            }

            Vector2D currentDirection = Vector2D.mean(directions).normalize();
            Vector2D currentPosition = trail.End.Mid + currentDirection;

            int gapCounter = 0;
            int numSlices = trail.NumSlices;

            FingerSlice lastSlice = trail.End;
            FingerSlice nextSlice;

            while (currentPosition.isWithin(0, 0, maxX, maxY) && gapCounter <= Math.Min(numSlices / 2, 10))
            {
                nextSlice = findFingerSliceFromMid(currentPosition, currentDirection);
                if (nextSlice != null && (nextSlice.Length < lastSlice.Length + 5 && nextSlice.Length > lastSlice.Length - 5))
                {
                    gapCounter = 0;
                    numSlices++;
                    trail.addSlice(nextSlice);
                    directions.Add((nextSlice.Mid - lastSlice.Mid));

                    if (directions.Count == numDirectionsForAverage)
                        directions.RemoveAt(0);

                    currentDirection = Vector2D.mean(directions).normalize();
                    currentPosition = nextSlice.Mid + currentDirection;

                    lastSlice = nextSlice;
                }
                else
                {
                    gapCounter++;
                    currentPosition += currentDirection;
                }
            }

            return trail;
        }

        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction)
        {
            if (edgeImage.isEdgeAt(position)) return null;

            Vector2D dirStart = direction.getOrthogonal(true);
            Vector2D dirEnd = direction.getOrthogonal(false);

            Vector2D start = findNextEdge(position, dirStart);
            if (start == null) return null;

            Vector2D end = findNextEdge(position, dirEnd);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }
        private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
        {

            Vector2D end = findNextEdge(start, direction);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }

        private Vector2D findNextEdge(Vector2D start, Vector2D direction)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            int maxFingerSize = 30;

            int maxStepsX;
            if (direction.X > 0)
                maxStepsX = (int)((maxX - start.X) / direction.X);
            else if (direction.X < 0)
                maxStepsX = (int)(start.X / Math.Abs(direction.X));
            else
                maxStepsX = int.MaxValue;

            int maxStepsY;
            if (direction.Y > 0)
                maxStepsY = (int)((maxY - start.Y) / direction.Y);
            else if (direction.Y < 0)
                maxStepsY = (int)(start.Y / Math.Abs(direction.Y));
            else
                maxStepsY = int.MaxValue;

            int maxStepsLength = (int)(maxFingerSize / direction.Length);

            int maxSteps = Math.Min(maxStepsLength, Math.Min(maxStepsX, maxStepsY));

            Vector2D end = new Vector2D(start);
            for (int i = 0; i < maxSteps; i++)
            {
                end += direction;

                if (edgeImage.isEdgeAt(end))
                {
                    return end;
                }
            }

            return null;
        }

        private FingerSlice getFingerSlice(Vector2D start, Vector2D end)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            int minFingerSize = 5;
            int maxFingerSize = 30;

            Vector2D direction = (end - start).normalize();
            Vector2D beforeStart = start - direction;
            Vector2D behindEnd = end + direction;

            start = beforeStart.isWithin(0, 0, maxX, maxY) ? beforeStart : start;
            end = behindEnd.isWithin(0, 0, maxX, maxY) ? behindEnd : end;

            FingerSlice slice = new FingerSlice(start, end);
            if (slice.Length >= minFingerSize && slice.Length <= maxFingerSize && fingerSliceDepthTest(slice))
                return slice;

            return null;
        }

        private bool fingerSliceDepthTest(FingerSlice fingerSlice)
        {
            Int16 depthStart = depthImage.getDepthAt(fingerSlice.Start);
            Int16 depthMid = depthImage.getDepthAt(fingerSlice.Mid);
            Int16 depthEnd = depthImage.getDepthAt(fingerSlice.End);
            return (depthStart > depthMid && depthMid < depthEnd);
        }

        private void createFingerFromTrail(FingerSliceTrail trail)
        {
            Finger finger = new Finger(trail);

            //add finger
            Fingers.Add(finger);

            //draw finger
            drawDetectedFinger(finger);
        
            //remove edges around detected finger to improve performance
            edgeImage.removeFingerEdges(finger);
        }

        private void drawDetectedFinger(Finger finger)
        {
            FingerSliceTrail trail = finger.SliceTrail;
            for (int i = 0; i < trail.NumSlices; i++)
            {
                outputImage.drawLineSegment(trail.Slices[i].LineSegment, 255, 0, 255);
            }
            outputImage.drawLineSegment(finger.LineSegment, 255, 0, 0);
        }
    }
}