bbiwarg-original/bbiwarg/Recognition/Tracking/Tracker.cs

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

namespace bbiwarg.Recognition.Tracking
{
    public enum TrackingState
    {
        None,
        Detected,
        Tracked,
        Lost,
    }

    struct SimilarityContainer<T> where T : TrackableObject
    {
        public float similarity;
        public TrackableObjectHistory<T> history;
        public T detectedObject;
    }

    class Tracker<T> where T : TrackableObject
    {
        public List<TrackableObjectHistory<T>> Histories { get; private set; }
        public List<T> TrackedObjects { get; private set; }
        private List<SimilarityContainer<T>> similarities;
        private String className;
        private Logger.LogSubject logSubject;
        private int numFramesUntilTracked;
        private int numFramesUntilLost;
        private float minSimilarityForTracking;

        public Tracker(Logger.LogSubject logSubject, int numFramesUntilTracked, int numFramesUntilLost, float minSimilarityForTracking)
        {
            this.className = typeof(T).Name;
            this.logSubject = logSubject;
            this.numFramesUntilTracked = numFramesUntilTracked;
            this.numFramesUntilLost = numFramesUntilLost;
            this.minSimilarityForTracking = minSimilarityForTracking;

            reset();
        }

        public void reset()
        {
            Histories = new List<TrackableObjectHistory<T>>();
            TrackedObjects = new List<T>();
        }

        public void updateFrame(List<T> detectedObjects)
        {
            if (Histories.Count == 0)
            {
                foreach (T detectedObject in detectedObjects)
                {
                    TrackableObjectHistory<T> history = new TrackableObjectHistory<T>(numFramesUntilTracked);
                    history.addObjectToHistory(detectedObject);
                    addToHistories(history);
                }
            }
            else
            {
                updateHistories(detectedObjects);
                removeLongLostObjects();
            }
        }

        private void updateHistories(List<T> detectedObjects)
        {
            TrackedObjects = new List<T>();

            List<TrackableObjectHistory<T>> unasignedHistories = new List<TrackableObjectHistory<T>>();
            foreach (TrackableObjectHistory<T> history in Histories)
                unasignedHistories.Add(history);

            List<TrackableObject> unasignedDetectedObjects = new List<TrackableObject>();
            foreach (T detectedObject in detectedObjects)
                unasignedDetectedObjects.Add(detectedObject);


            //asign previously tracked objects to their best fits of detectedObjects
            createSimilarities(detectedObjects);
            while (similarities.Count > 0)
            {
                SimilarityContainer<T> maxSimilarity = similarities[0];
                TrackableObjectHistory<T> history = maxSimilarity.history;
                T detectedObject = maxSimilarity.detectedObject;
                history.addObjectToHistory(detectedObject);

                TrackedObjects.Add(detectedObject);
                unasignedHistories.Remove(history);
                unasignedDetectedObjects.Remove(detectedObject);
                removeConcurringSimilarities(maxSimilarity);
            }
            //add new history for each new unasigned detectedObject
            foreach (T unasignedDetectedObject in unasignedDetectedObjects)
            {
                TrackableObjectHistory<T> newHistory = new TrackableObjectHistory<T>(numFramesUntilTracked);
                newHistory.addObjectToHistory(unasignedDetectedObject);
                addToHistories(newHistory);
            }

            //add null-object to each unasigned history (didn't find a best fit during this frame -> lost track)
            foreach (TrackableObjectHistory<T> unasignedHistory in unasignedHistories)
            {
                if (unasignedHistory.CurrentState != TrackingState.Lost)
                    Logger.log(className + " #" + unasignedHistory.ID + " lost", logSubject);
                unasignedHistory.addObjectToHistory(null);
            }

        }

        private void createSimilarities(List<T> detectedObjects)
        {
            similarities = new List<SimilarityContainer<T>>();

            foreach (TrackableObjectHistory<T> history in Histories)
            {
                foreach (T detectedObject in detectedObjects)
                {
                    float similarity = detectedObject.getSimilarity(history.LastObject);
                    if (similarity > minSimilarityForTracking)
                    {
                        SimilarityContainer<T> similarityContainer = new SimilarityContainer<T>();
                        similarityContainer.similarity = similarity;
                        similarityContainer.history = history;
                        similarityContainer.detectedObject = detectedObject;
                        similarities.Add(similarityContainer);
                    }
                }
            }

            // sort depending on similarity-value
            similarities.Sort((fs1, fs2) => fs2.similarity.CompareTo(fs1.similarity));
        }

        private void removeConcurringSimilarities(SimilarityContainer<T> removeSimilarity)
        {
            for (int i = similarities.Count - 1; i >= 0; i--)
            {
                SimilarityContainer<T> similarity = similarities[i];
                if (similarity.history == removeSimilarity.history || similarity.detectedObject == removeSimilarity.detectedObject)
                    similarities.RemoveAt(i);
            }
        }

        private void removeLongLostObjects()
        {
            for (int i = Histories.Count - 1; i >= 0; i--)
            {
                TrackableObjectHistory<T> history = Histories[i];
                if (history.CurrentState == TrackingState.Lost && history.NumFramesInCurrentState >= numFramesUntilLost)
                {
                    Histories.RemoveAt(i);
                    Logger.log(className + " #" + history.ID + " deleted", logSubject);
                }
            }
        }

        private void addToHistories(TrackableObjectHistory<T> history)
        {
            Histories.Add(history);
            Logger.log(className + " #" + history.ID + " detected", logSubject);
        }
    }
}