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

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace bbiwarg.Recognition.Tracking
{
    abstract class Tracker<T, TrackedT>
        where T : TrackableObject
        where TrackedT : TrackedObject<T>
    {
        private float minSimilarity;
        private List<Similarity<T, TrackedT>> similarities;
        protected TrackIDPool idPool;

        public List<TrackedT> TrackedObjects;


        public Tracker(float minSimilarity)
        {
            this.minSimilarity = minSimilarity;
            reset();
        }

        public void reset()
        {
            idPool = new TrackIDPool();
            TrackedObjects = new List<TrackedT>();
        }

        public void updateFrame(List<T> detectedObjects)
        {
            if (TrackedObjects.Count == 0)
            {
                addNewTrackedObjects(detectedObjects);
            }
            else
            {
                updateTrackedObjects(detectedObjects);
                removeDeletableTrackedObjects();
            }
        }

        public void updateTrackedObjects(List<T> detectedObjects)
        {
            List<TrackedT> unasignedTrackedObjects = new List<TrackedT>(TrackedObjects);
            List<T> unasignedDetectedObjects = new List<T>(detectedObjects);

            createSimilarities(detectedObjects);
            while (similarities.Count > 0) {
                Similarity<T, TrackedT> maxSimilarity = similarities[0];
                maxSimilarity.TrackedObject.updateFrame(maxSimilarity.DetectedObject);

                unasignedDetectedObjects.Remove(maxSimilarity.DetectedObject);
                unasignedTrackedObjects.Remove(maxSimilarity.TrackedObject);
                removeConcurringSimilarities(maxSimilarity);
            }

            addNewTrackedObjects(unasignedDetectedObjects);

            foreach (TrackedT trackedObject in unasignedTrackedObjects) {
                trackedObject.updateFrame(null);
            }
        }

        private void addNewTrackedObjects(List<T> detectedObjects)
        {
            foreach (T detectedObject in detectedObjects)
            {
                TrackedT trackedObject = createTrackedObject(detectedObject);
                TrackedObjects.Add(trackedObject);
            }
        }

        private void createSimilarities(List<T> detectedObjects) {
            similarities = new List<Similarity<T, TrackedT>>();

            foreach (TrackedT trackedObject in TrackedObjects) {
                foreach (T detectedObject in detectedObjects) {
                    float similarityValue = trackedObject.calculateSimilarity(detectedObject);
                    Similarity<T, TrackedT> similarity = new Similarity<T,TrackedT>(trackedObject, detectedObject, similarityValue);
                    if (similarity.Value > minSimilarity)
                        similarities.Add(similarity);
                }
            }

            // sort depending on similarity-value
            similarities.Sort((s1, s2) => s2.Value.CompareTo(s1.Value));
        }

        private void removeConcurringSimilarities(Similarity<T, TrackedT> similarity) {
            for (int i = similarities.Count - 1; i >= 0; i--) {
                Similarity<T, TrackedT> s = similarities[i];
                if (s.TrackedObject == similarity.TrackedObject || s.DetectedObject == similarity.DetectedObject)
                    similarities.RemoveAt(i);
            }
        }

        private void removeDeletableTrackedObjects() {
            for (int i = TrackedObjects.Count - 1; i >= 0; i--) {
                TrackedT trackedObject = TrackedObjects[i];
                if (trackedObject.CurrentState == TrackingState.Deleted) {
                    idPool.setIDUnused(trackedObject.ID);
                    TrackedObjects.RemoveAt(i);
                }
            }
        }

        protected List<T> getCurrentObjectsWithState(TrackingState state) {
            List<T> objects = new List<T>();
            foreach (TrackedT trackedObject in TrackedObjects) {
                if (trackedObject.CurrentState == state)
                    objects.Add(trackedObject.CurrentObject);
            }
            return objects;
        } 

        protected abstract TrackedT createTrackedObject(T detectedObject);
    }
}