bbiwarg-original/bbiwarg/Recognition/FingerRecognition/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.Input.InputHandling;
using bbiwarg.Utility;
using Emgu.CV.Structure;
using Emgu.CV;

namespace bbiwarg.Recognition.FingerRecognition
{
    /// <summary>
    /// Detects fingers in the given depth and edge images. The finger detection searches for edges in the edge image and tries to find an initial finger slice. For each found finger slice, the finger detector tries to move along the finger direction to extend the finger slice trail. If the trail reaches its end, the finger detector removes the first few slices and starts the trail expansion in opposite direction. If the complete slice trail is long enough, the finger slices are sorted into correct order and the edges around the finger are removed to increase performance for the next finger detection.
    /// </summary>
    class FingerDetector
    {
        /// <summary>
        /// the coordinateConverter (used to calculate finger width (in milimeters))
        /// </summary>
        private CoordinateConverter coordinateConverter;

        /// <summary>
        /// the current depthImage
        /// </summary>
        private DepthImage depthImage;

        /// <summary>
        /// the current edge image in its original form
        /// </summary>
        private EdgeImage edgeImageOriginal;

        /// <summary>
        /// the current edge image in its adapted form (edges around detected fingers will be removed)
        /// </summary>
        private EdgeImage edgeImageAdapted;

        /// <summary>
        /// the detected fingers
        /// </summary>
        private List<Finger> fingers;

        /// <summary>
        /// Initializes a new instance of the FingerDetector class.
        /// </summary>
        /// <param name="coordinateConverter">The coordinate converter.</param>
        public FingerDetector(CoordinateConverter coordinateConverter) {
            this.coordinateConverter = coordinateConverter;
        }
        
        /// <summary>
        /// Detects fingers in the current frame using the depth and edge image. Afterwards the detected fingers are stored in the given frame data (detectedFingers).
        /// </summary>
        /// <param name="frameData">the current frame</param>
        public void detectFingers(FrameData frameData)
        {
            depthImage = frameData.DepthImage;
            edgeImageOriginal = frameData.EdgeImage;
            edgeImageAdapted = frameData.EdgeImage.copy();
            fingers = new List<Finger>();

            Vector2D maxPixel = depthImage.Size.MaxPixel;
            int maxX = maxPixel.IntX;
            int maxY = maxPixel.IntY;

            for (int y = 1; y < maxY; y += 4)
            {
                for (int x = 1; x < maxX; x += 2)
                {
                    if (edgeImageAdapted.isEdgeAt(x, y))
                    {
                        Vector2D edgePoint = new Vector2D(x, y);
                        Vector2D edgeDirection = getEdgeDirection(edgePoint);
                        if (edgeDirection != null)
                        {
                            Vector2D dir = edgeDirection.getOrthogonal();
                            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 > Parameters.FingerMinNumSlices)
                                {
                                    createFingerFromTrail(trail);
                                }
                            }
                        }
                    }
                }
            }

            frameData.DetectedFingers = fingers;
        }

        /// <summary>
        /// Gets the edge direction of the given edge point. The edge direction is either horizontal, vertical, diagonal (both) or null.
        /// </summary>
        /// <param name="edgePoint">the edge point</param>
        /// <returns>the edge directon at the given point</returns>
        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;
        }

        /// <summary>
        /// Creates a new FingerSliceTrail and expands it along its direction.
        /// </summary>
        /// <param name="startSlice">the initial finger slice</param>
        /// <param name="startDirection">the initial finger direction</param>
        /// <returns>the found FingerSliceTrail or null</returns>
        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

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

            if (position.isInBound(depthImage.Size))
            {
                FingerSlice nextSlice = findFingerSliceFromMid(position, direction);
                if (nextSlice != null)
                {
                    trail.addSlice(nextSlice);
                    trail = expandTrail(trail);

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

        /// <summary>
        ///  Tries to expand a trail along its direction.
        /// </summary>
        /// <param name="trail">the given finger slice trail</param>
        /// <param name="reversed">indicates in which direction the trail should be expanded</param>
        /// <returns>the expanded finger slice trail</returns>
        private FingerSliceTrail expandTrail(FingerSliceTrail trail, bool reversed = false)
        {
            if (reversed)
                trail.reverse();

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

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

            FingerSlice lastSlice = trail.EndSlice;
            FingerSlice nextSlice;

            while (currentPosition.isInBound(depthImage.Size) && gapCounter < Math.Min(numSlices, Parameters.FingerMaxGapCounter))
            {
                nextSlice = findFingerSliceFromMid(currentPosition, currentDirection, reversed);

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

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

            if (reversed)
                trail.reverse();

            return trail;
        }

        /// <summary>
        /// Tries to find a finger slice from a given position by searching in both orthogonal directions for an edge.
        /// </summary>
        /// <param name="position">the position somewhere in the middle of the possible finger</param>
        /// <param name="direction">the finger direction</param>
        /// <param name="reversed">indicates wether start and end should be swapped</param>
        /// <returns>the found finger slice or null</returns>
        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction, bool reversed = false)
        {
            if (edgeImageAdapted.isRoughEdgeAt(position)) return null;

            Int16 depth = depthImage.getDepthAt(position);
            int maxWidth2D = (int)coordinateConverter.convertLength3Dto2D(Parameters.FingerMaxWidth3D, depth);

            Vector2D dirStart = direction.getOrthogonal(reversed);
            Vector2D dirEnd = direction.getOrthogonal(!reversed);

            Vector2D startPositionStart = position + 0.5f*Parameters.FingerMinWidth2D * dirStart;
            Vector2D start = edgeImageAdapted.findNextRoughEdge(startPositionStart, dirStart, maxWidth2D);
            if (start == null) return null;

            Vector2D endPositionStart = position + 0.5f*Parameters.FingerMinWidth2D * dirEnd;
            Vector2D end = edgeImageAdapted.findNextRoughEdge(position, dirEnd, maxWidth2D);
            if (end == null) return null;

            FingerSlice slice = new FingerSlice(start, end);
            if (!fingerSliceDepthTest(slice) || slice.Length > maxWidth2D)
                return null;

            return slice;
        }

        /// <summary>
        /// Tries to find a finger slice from one point towards the given direction (searches for an edge).
        /// </summary>
        /// <param name="start">the start position</param>
        /// <param name="direction">the slice direction</param>
        /// <returns>the found finger slice or null</returns>
        private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
        {
            Vector2D searchStart = start + Parameters.FingerMinWidth2D * direction;

            Int16 depth = depthImage.getDepthAt(start);
            int maxWidth2D = (int)coordinateConverter.convertLength3Dto2D(Parameters.FingerMaxWidth3D, depth);

            Vector2D end = edgeImageAdapted.findNextRoughEdge(searchStart, direction, maxWidth2D);
            if (end == null)
                return null;

            FingerSlice slice = new FingerSlice(start, end);
            if (!fingerSliceDepthTest(slice))
                return null;

            return slice;
        }

        /// <summary>
        /// Checks if a possible finger slice is located on a finger. To pass this test, the depth value at the mid has to be lower than on the outside (start and end).
        /// </summary>
        /// <param name="slice">the possible finger slcie</param>
        /// <returns>wether the slice is located on a finger</returns>
        private bool fingerSliceDepthTest(FingerSlice slice)
        {
            Int16 depthStart = depthImage.getDepthAt(slice.Start.moveWithinBound(depthImage.Size, slice.Direction.getInverse(), Parameters.FingerContourMargin));
            Int16 depthMid = depthImage.getDepthAt(slice.Mid);
            Int16 depthEnd = depthImage.getDepthAt(slice.End.moveWithinBound(depthImage.Size, slice.Direction, Parameters.FingerContourMargin));
            return (depthStart > depthMid && depthMid < depthEnd);
        }

        /// <summary>
        /// Sorts the finger slices in correct order an checks if the finger is a valid finger (<see cref="isCrippleFinger(Finger)"/>). If it is valid the finger is added to the list of detected fingers. Afterwards the edges around the finger are removed to surpress a new finger search for the same finger.
        /// </summary>
        /// <param name="trail">the slice trail of the possible finger</param>
        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);

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

        /// <summary>
        /// Checks wether the finger has enough space around itself (fingers from a closed hand shouldn't bee detected).
        /// </summary>
        /// <param name="finger">the finger</param>
        /// <returns>wether the finger has enough space around itself</returns>
        private bool isCrippleFinger(Finger finger)
        {
            FingerSlice midSlice = finger.SliceTrail.MidSlice;
            Vector2D out1 = midSlice.Start.moveWithinBound(depthImage.Size, midSlice.Direction.getInverse(), Parameters.FingerOutMargin);
            Vector2D out2 = midSlice.End.moveWithinBound(depthImage.Size, midSlice.Direction, Parameters.FingerOutMargin);

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

            return (minDepthDifference < Parameters.FingerMaxCrippleDifference);
        }

        /// <summary>
        /// Sorts a slice trail in the corrent order, so that the start is at the finger tip and the end is at the hand. To guess the correct order the width of the first and last slice are compared.
        /// </summary>
        /// <param name="trail">the slice trail of the finger</param>
        /// <returns>the slice trail of the finger in correct order</returns>
        private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
        {
            float startLength = trail.StartSlice.Length;
            float endLength = trail.EndSlice.Length;

            if (startLength > endLength)
                trail.reverse();

            return trail;
        }
    }
}