bbiwarg-original/bbiwarg/Detectors/FingerDetection/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.FingerDetection
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImageOriginal;
        private EdgeImage edgeImageAdapted;
        private OutputImage outputImage;
        public List<Finger> Fingers { get; private set; }

        public FingerDetector(DepthImage depthImage, EdgeImage edgeImage, OutputImage outputImage)
        {
            Console.WriteLine("---");
            this.depthImage = depthImage;
            this.edgeImageOriginal = edgeImage;
            this.edgeImageAdapted = edgeImage.copy();
            this.outputImage = outputImage;

            detectFingers();
            drawFingers();
        }

        private void detectFingers()
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            Fingers = new List<Finger>();

            for (int y = 1; y < maxY; y += 5)
            {
                for (int x = 1; x < maxX; x++)
                {
                    if (edgeImageAdapted.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 > Constants.FingerMinNumSlices)
                                {
                                    createFingerFromTrail(trail);
                                }
                            }
                        }
                    }
                }
            }
        }

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

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

        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

            Vector2D direction = startDirection;
            Vector2D position = startSlice.Mid + Constants.FingerStepSize * direction;

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

                    if (trail.NumSlices > Constants.FingerMinNumSlices / 2)
                    {
                        trail.removeFirstSlices(Constants.FingerRemoveNumSlicesForCorrection);
                        trail.reverse();
                        trail = expandTrail(trail, true);
                        trail.reverse();
                        return trail;
                    }
                }
            }

            return null;

        }

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

            Vector2D currentDirection = trail.getEndDirection();
            Vector2D currentPosition = trail.EndSlice.Mid + Constants.FingerStepSize * currentDirection;

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

            FingerSlice lastSlice = trail.EndSlice;
            FingerSlice nextSlice;

            while (currentPosition.isWithin(0, 0, maxX, maxY) && gapCounter <= Math.Min(numSlices, Constants.FingerMaxGapCounter))
            {
                if (reversed)
                    nextSlice = findFingerSliceFromMid(currentPosition, currentDirection.getInverse());
                else
                    nextSlice = findFingerSliceFromMid(currentPosition, currentDirection);

                if (nextSlice != null && Math.Abs(nextSlice.Length - lastSlice.Length) <= Constants.FingerMaxSliceDifferencePerStep)
                {
                    gapCounter = 0;
                    numSlices++;
                    trail.addSlice(nextSlice);
                    currentDirection = trail.getEndDirection();
                    currentPosition = nextSlice.Mid + Constants.FingerStepSize * currentDirection;

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

            return trail;
        }

        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction)
        {
            if (edgeImageAdapted.isRoughEdgeAt(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 + Constants.FingerSliceOverlapFactor * direction, direction);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }

        private Vector2D findNextEdge(Vector2D start, Vector2D direction, bool adaptedEdgeImage = true, bool stopAtMaxFingerSize = true, bool returnBoundIfNoEdge = false)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            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 maxSteps = Math.Min(maxStepsX, maxStepsY);

            if (stopAtMaxFingerSize)
                maxSteps = Math.Min(maxSteps, (int)(Constants.FingerMaxSize / direction.Length));

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

                if ((adaptedEdgeImage && edgeImageAdapted.isRoughEdgeAt(end)) || (!adaptedEdgeImage && edgeImageOriginal.isRoughEdgeAt(end)))
                {
                    return end;
                }
            }

            if (returnBoundIfNoEdge)
                return end;
            else
                return null;
        }

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

            Vector2D direction = (end - start).normalize();
            Vector2D beforeStart = (start - Constants.FingerSliceOverlapFactor * direction).moveInBound(0, 0, maxX, maxY);
            Vector2D behindEnd = (end + Constants.FingerSliceOverlapFactor * direction).moveInBound(0, 0, maxY, maxY);

            FingerSlice slice = new FingerSlice(beforeStart, behindEnd);
            if (slice.Length >= Constants.FingerMinSize && slice.Length <= Constants.FingerMaxSize && 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)
        {
            //bring finger in correct direction Tip->Hand
            trail = orderTrailTipToHand(trail);

            //create finger
            Finger finger = new Finger(trail);

            //add finger
            if (!isCrippleFinger(finger))
                Fingers.Add(finger);
            else
                Console.WriteLine("cripple");

            //remove edges around detected finger to improve performance
            edgeImageAdapted.removeEdgesInsidePolygon(finger.Contour.ToArray());
        }

        private bool isCrippleFinger(Finger finger)
        {
            int maxX = depthImage.Width - 1;
            int maxY = depthImage.Height - 1;

            FingerSlice midSlice = finger.SliceTrail.MidSlice;
            Vector2D direction = midSlice.Direction;
            Vector2D out1 = (midSlice.Start - Constants.FingerCrippleOutFactor * direction).moveInBound(0, 0, maxX, maxY);
            Vector2D out2 = (midSlice.End + Constants.FingerCrippleOutFactor * direction).moveInBound(0, 0, maxX, maxY);

            Int16 depthAtFinger = depthImage.getDepthAt(midSlice.Mid);
            Int16 depthAtOut1 = depthImage.getDepthAt(out1);
            Int16 depthAtOut2 = depthImage.getDepthAt(out2);
            int minDepthDifference = Math.Min(Math.Abs(depthAtFinger - depthAtOut1), Math.Abs(depthAtFinger - depthAtOut2));

            return (minDepthDifference < Constants.FingerCrippleOutMinDifference);
        }

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

            Vector2D dirOrth1 = direction.getOrthogonal(true);
            Vector2D dirOrth2 = direction.getOrthogonal(false);

            Vector2D outPoint = (start + Constants.FingerOutSliceFactor * direction).moveInBound(0, 0, maxX, maxY);
            Vector2D p1 = findNextEdge(outPoint, dirOrth1, false, false, true);
            Vector2D p2 = findNextEdge(outPoint, dirOrth2, false, false, true);

            FingerSlice slice = new FingerSlice(p1, p2);

            return slice;
        }

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

            FingerSlice start = trail.StartSlice;
            FingerSlice end = trail.EndSlice;

            Vector2D direction = (end.Mid - start.Mid).normalize();

            FingerSlice startOutSlice = findOutSlice(start.Mid, direction.getInverse());
            FingerSlice endOutSlice = findOutSlice(end.Mid, direction);

            float startOutLength = float.MaxValue;
            float endOutLength = float.MaxValue;

            if (startOutSlice.Start.isWithin(1, 1, maxX, maxY) && startOutSlice.End.isWithin(1, 1, maxX, maxY))
                startOutLength = startOutSlice.Length;

            if (endOutSlice.Start.isWithin(1, 1, maxX, maxY) && endOutSlice.End.isWithin(1, 1, maxX, maxY))
                endOutLength = endOutSlice.Length;

            if (startOutLength <= endOutLength)
            {
                trail.reverse();
                outputImage.drawLineSegment(startOutSlice.LineSegment, Constants.FingerHandOutSliceColor);
                outputImage.drawLineSegment(endOutSlice.LineSegment, Constants.FingerTipOutSliceColor);
            }
            else
            {
                outputImage.drawLineSegment(startOutSlice.LineSegment, Constants.FingerTipOutSliceColor);
                outputImage.drawLineSegment(endOutSlice.LineSegment, Constants.FingerHandOutSliceColor);
            }

            return trail;
        }

        private void drawFingers()
        {
            foreach (Finger finger in Fingers)
            {
                drawFinger(finger);
            }
        }

        private void drawFinger(Finger finger)
        {
            FingerSliceTrail trail = finger.SliceTrail;
            for (int i = 0; i < trail.NumSlices; i++)
            {
                outputImage.drawLineSegment(trail[i].LineSegment, Constants.FingerSliceColor);
            }
            outputImage.drawLineSegment(finger.LineSegment, Constants.FingerDetectedColor);
            outputImage.drawContour(finger.Contour, Constants.FingerContourColor, 1);
        }
    }
}