bbiwarg-original/bbiwarg/Recognition/FingerRecognition/FingerTracker.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using bbiwarg.Images;
using bbiwarg.Graphics;

using bbiwarg.Utility;

namespace bbiwarg.Recognition.FingerRecognition
{
    struct FingerSimilarity
    {
        public float similarity;
        public FingerHistory fingerHistory;
        public Finger fingerDetected;
    }

    class FingerTracker
    {
        public List<FingerHistory> FingerHistories { get; private set; }
        public List<Finger> Fingers { get; private set; }
        private List<FingerSimilarity> similarities;

        public FingerTracker()
        {
            FingerHistories = new List<FingerHistory>();
            Fingers = new List<Finger>();
        }

        ~FingerTracker() {
            foreach (FingerHistory fh in FingerHistories) {
                FingerHistory.setIDUnused(fh.ID);
            }
        }

        public void updateTrackedFingers(List<Finger> detectedFingers, OutputImage outputImage)
        {
            if (FingerHistories.Count == 0)
            {
                foreach (Finger finger in detectedFingers)
                {
                    FingerHistory fh = new FingerHistory(finger);
                    FingerHistories.Add(fh);
                    Logger.log("Finger #" + fh.ID + " detected", Logger.LogSubject.FingerTracker);
                }
            }
            else
            {
                updateHistories(detectedFingers);
                removeLongLostFingers();
            }

            drawFingers(outputImage);
            drawFingerIDs(outputImage);
        }

        private void updateHistories(List<Finger> detectedFingers)
        {
            Fingers = new List<Finger>();

            List<FingerHistory> unasignedFingerHistories = new List<FingerHistory>();
            foreach (FingerHistory fh in FingerHistories)
                unasignedFingerHistories.Add(fh);

            List<Finger> unasignedDetectedFingers = new List<Finger>();
            foreach (Finger f in detectedFingers)
                unasignedDetectedFingers.Add(f);

            createSimilarities(detectedFingers);

            while (similarities.Count > 0)
            {
                FingerSimilarity maxSimilarity = similarities[0];
                FingerHistory fingerHistory = maxSimilarity.fingerHistory;
                Finger fingerDetected = maxSimilarity.fingerDetected;
                fingerHistory.addFinger(fingerDetected);

                Fingers.Add(fingerDetected);
                unasignedFingerHistories.Remove(fingerHistory);
                unasignedDetectedFingers.Remove(fingerDetected);
                removeConcurringFingerSimilarities(maxSimilarity);
            }

            //add new fingerHistory for each new unmatched finger
            foreach (Finger finger in unasignedDetectedFingers)
            {
                FingerHistory fh = new FingerHistory(finger);
                FingerHistories.Add(fh);
                Logger.log("Finger #" + fh.ID + " detected", Logger.LogSubject.FingerTracker);
            }

            //add null finger to TrackedFingers that haven't found a match in this frame (status tracked->lost)
            foreach (FingerHistory fh in unasignedFingerHistories)
            {
                if (fh.CurrentState != TrackingState.Lost) 
                    Logger.log("Finger #" + fh.ID + " lost", Logger.LogSubject.FingerTracker);
                fh.addFinger(null);
            }

        }

        private void createSimilarities(List<Finger> detectedFingers)
        {
            similarities = new List<FingerSimilarity>();

            foreach (FingerHistory fh in FingerHistories)
            {
                foreach (Finger fingerDetected in detectedFingers)
                {
                    float similarity = fingerDetected.getSimilarity(fh.LastFinger);
                    if (similarity > Constants.FingerMinSimilarityForTracking)
                    {
                        FingerSimilarity fs = new FingerSimilarity();
                        fs.similarity = similarity;
                        fs.fingerHistory = fh;
                        fs.fingerDetected = fingerDetected;
                        similarities.Add(fs);
                    }
                }
            }

            similarities.Sort((fs1, fs2) => fs2.similarity.CompareTo(fs1.similarity));
        }

        private void removeConcurringFingerSimilarities(FingerSimilarity removeSimilarity)
        {
            for (int i = similarities.Count - 1; i >= 0; i--)
            {
                FingerSimilarity currentSimilarity = similarities[i];
                if (currentSimilarity.fingerHistory == removeSimilarity.fingerHistory || currentSimilarity.fingerDetected == removeSimilarity.fingerDetected)
                    similarities.RemoveAt(i);
            }
        }

        private void removeFingerDetectedFromSimilarities(Finger removeFinger)
        {
            for (int i = similarities.Count - 1; i >= 0; i--)
            {
                if (similarities[i].fingerDetected == removeFinger)
                    similarities.RemoveAt(i);
            }
        }

        private void removeLongLostFingers()
        {
            for (int i = FingerHistories.Count - 1; i >= 0; i--)
            {
                FingerHistory fh = FingerHistories[i];
                if (fh.CurrentState == TrackingState.Lost && fh.getNumFramesInCurrentState() >= Constants.FingerNumFramesUntilLost)
                {
                    FingerHistories.RemoveAt(i);
                    Logger.log("Finger #" + fh.ID + " deleted", Logger.LogSubject.FingerTracker);
                }
            }
        }

        private void drawFingers(OutputImage outputImage)
        {
            foreach (Finger finger in Fingers)
            {
                finger.draw(outputImage, true);
            }
        }

        private void drawFingerIDs(OutputImage outputImage) {
            foreach (FingerHistory fh in FingerHistories)
            {
                if (fh.CurrentFinger != null)
                {
                    Finger f = fh.CurrentFinger;
                    outputImage.drawText(f.TipPoint.IntX, f.TipPoint.IntY, fh.ID.ToString(), Constants.FingerIDColor);
                }
            }
        }
    }
}