# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1417933956 18000
# Node ID 9e39cd95e01727fb571011cc5ee9219904abfc3d
# Parent  e2a873e085685700967e989d751b92f4cf87c728
first implementation of CLEAR MOT (needs formal tests)

diff -r e2a873e08568 -r 9e39cd95e017 python/moving.py
--- a/python/moving.py	Sat Dec 06 22:15:56 2014 -0500
+++ b/python/moving.py	Sun Dec 07 01:32:36 2014 -0500
@@ -1306,90 +1306,109 @@
 
     def matches(self, obj, instant, matchingDistance):
         '''Indicates if the annotation matches obj (MovingObject)
-        with threshold matchingDistance'''
+        with threshold matchingDistance
+        Returns distance if below matchingDistance, matchingDistance+1 otherwise
+        (returns an actual value, otherwise munkres does not terminate)'''
         d = Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant))
-        return d<matchingDistance, d
+        if d < matchingDistance:
+            return d
+        else:
+            return matchingDistance + 1
 
-def matchingGroundTruthToTracker(objects, annotations, matchingDistance, firstInstant, lastInstant):
-    '''Returns a matching of tracker output (objects) to ground truth (annnotations)
+def computeClearMOT(objects, annotations, matchingDistance, firstInstant, lastInstant, debug = False):
+    '''Computes the CLEAR MOT metrics 
+
+    Reference:
+    Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008)
 
     objects and annotations are supposed to in the same space
     current implementation is BBAnnotations (bounding boxes)
     mathingDistance is threshold on matching between annotation and object
 
-    Output is list of 
-    [frame number, ground truth id, tracker object id, distance]
-    where tracker object id is None if no matching was found'''
+    TO: tracker output (objects)
+    GT: ground truth (annotations)
+
+    Should we use the distance as weights or just 1/0 if distance below matchingDistance?
+    (add argument useDistanceForWeights = False)'''
+    from munkres import Munkres
+    from numpy import array
+    from sys import maxsize
     
-    matchTable = []
+    munk = Munkres()
+    dist = 0. # total distance between GT and TO
+    ct = 0 # number of associations between GT and tracker output in each frame
+    gt = 0 # number of GT.frames
+    mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame)
+    fpt = 0 # number of false alarm.frames (tracker objects without match in each frame)
+    mme = 0 # number of mismatches
+    matches = {} # match[i] is the tracker track associated with GT i (using object references)
     for t in xrange(firstInstant, lastInstant+1):
-        for a in annotations:
-            if a.existsAtInstant(t):
-                minDist = float('inf')
-                matchingObject = None
-                for o in objects:
-                    if o.existsAtInstant(t):
-                        match, d = a.matches(o, t, matchingDistance)
-                        if match and d<minDist:
-                            minDist = d
-                            matchingObject = o
-                if matchingObject == None:
-                    matchTable.append([t, a.getNum(), None, minDist])
+        previousMatches = matches.copy()
+        # go through currently matched GT-TO and check if they are still matched withing matchingDistance
+        toDelete = []
+        for a in matches:
+            if a.existsAtInstant(t) and matches[a].existsAtInstant(t):
+                d = a.matches(matches[a], t, matchingDistance)
+                if d < matchingDistance:
+                    dist += d
                 else:
-                    matchTable.append([t, a.getNum(), matchingObject.getNum(), minDist])
-    return matchTable
-
-def computeClearMOT(matchTable, nTrackFrames):
-    '''Computes the MOTA/MOTP measures from the matching statistics 
-    between ground truth and tracker output 
-    computed by matchingGroundTruthToTracker
-
-    nTrackFrames is the sum of the number of frames of existence of all tracker output
+                    toDelete.append(a)
+            else:
+                toDelete.append(a)
+        for a in toDelete:
+            del matches[a]
 
-    Adapted from Dariush Ettehadieh's thesis work'''
-    #Calculate MOTP
-    dist = 0. # total distance between GT and tracker output
-    nAssociatedGTFrames = 0
-    mt = 0 # number of missed GT frames (sum of the number of GT not detected in each frame)
-    for mtab in matchTable:
-        if mtab[2] != None:
-            dist += float(mtab[3])#/T
-            nAssociatedGTFrames += 1
-        else:
-            mt += 1
-    if nAssociatedGTFrames != 0:
-        motp = dist/nAssociatedGTFrames
+        # match all unmatched GT-TO
+        matchedGTs = matches.keys()
+        matchedTOs = matches.values()
+        costs = []
+        unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs]
+        unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs]
+        nGTs = len(matchedGTs)+len(unmatchedGTs)
+        nTOs = len(matchedTOs)+len(unmatchedTOs)
+        if len(unmatchedTOs) > 0:
+            for a in unmatchedGTs:
+                aCosts = [a.matches(o, t, matchingDistance) for o in unmatchedTOs]
+                if min(aCosts) < matchingDistance:
+                    costs.append(aCosts)
+        # print costs
+        if len(costs) > 0:
+            newMatches = munk.compute(costs)
+            for k,v in newMatches:
+                if costs[k][v] < matchingDistance:
+                    matches[unmatchedGTs[k]]=unmatchedTOs[v]
+                    dist += costs[k][v]
+        if debug:
+            print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()]))
+        
+        # compute metrics elements
+        ct += len(matches)
+        mt += nGTs-len(matches)
+        fpt += nTOs-len(matches)
+        gt += nGTs
+        # compute mismatches
+        # for gt that do not appear in both frames, check if the corresponding to was matched to another gt in previous/next frame
+        mismatches = []
+        for a in matches:
+            if a in previousMatches:
+                if matches[a] != previousMatches[a]:
+                    mismatches.append(a)
+            elif matches[a] in previousMatches.values():
+                mismatches.append(matches[a])
+        for a in previousMatches:
+            if a not in matches and previousMatches[a] in matches.values():
+                mismatches.append(previousMatches[a])
+        if debug: 
+            for mm in set(mismatches):
+                print type(mm), mm.getNum()
+        # some object mismatches may appear twice
+        mme += len(set(mismatches))
+        
+    if ct > 0:
+        motp = dist/ct
     else:
-        return 0,0,0,0,0,0
-
-    #Calculate MOTA
-    gt = len(matchTable) # sum of the number of GT in each frame, or sum of the length of existence of each GT
-    #for sgt in sorted_gt_positions:
-    #    gt += (len(sgt)-1)
-
-    #total_traces = len(object_positions)
-    fpt = nTrackFrames - nAssociatedGTFrames
-
-    # gtobj = 0
-    mme = 0
-    # while gtobj <= n_gt_objects:
-    #     prev = [0,0,-1,0]
-    #     new_match = 0
-    #     for mtab in matchTable:
-    #         if mtab[1] == gtobj:
-    #             if new_match == 0:
-    #                 new_match = 1
-    #                 mme = mme - 1
-    #                 if mtab[2] != prev[2]:
-    #                     mme += 1
-    #                     prev = mtab
-    #                     gtobj += 1
-
-    mota = 1-(float(mt+fpt+mme)/gt)
-
-    print 'MOTP: ' + str(motp)
-    print 'MOTA: ' + str(mota)
-    return motp, mota, dist, mt, mme, fpt
+        motp = None
+    return motp, 1.-float(mt+fpt+mme)/gt, mt, mme, fpt, gt
 
 
 def plotRoadUsers(objects, colors):
diff -r e2a873e08568 -r 9e39cd95e017 scripts/compute-clearmot.py
--- a/scripts/compute-clearmot.py	Sat Dec 06 22:15:56 2014 -0500
+++ b/scripts/compute-clearmot.py	Sun Dec 07 01:32:36 2014 -0500
@@ -5,7 +5,6 @@
 import moving, storage
 
 # TODO: need to trim objects to same mask ?
-# pass frame interval where matching is done?
 
 parser = argparse.ArgumentParser(description='The program computes the CLEAR MOT metrics between ground truth and tracker output (in Polytrack format)', epilog='''CLEAR MOT metrics information:
 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008)
@@ -16,30 +15,25 @@
 parser.add_argument('-d', dest = 'trackerDatabaseFilename', help = 'name of the Sqlite database containing the tracker output', required = True)
 parser.add_argument('-g', dest = 'groundTruthDatabaseFilename', help = 'name of the Sqlite database containing the ground truth', required = True)
 parser.add_argument('-o', dest = 'homographyFilename', help = 'name of the filename for the homography (if tracking was done using the homography)')
-parser.add_argument('-m', dest = 'matchingDistance', help = 'matching distance between tracker and ground truth trajectories', required = True)
-parser.add_argument('-f', dest = 'firstInstant', help = 'first instant for measurement', required = True)
-parser.add_argument('-l', dest = 'lastInstant', help = 'last instant for measurement', required = True)
+parser.add_argument('-m', dest = 'matchingDistance', help = 'matching distance between tracker and ground truth trajectories', required = True, type = float)
+parser.add_argument('-f', dest = 'firstInstant', help = 'first instant for measurement', required = True, type = int)
+parser.add_argument('-l', dest = 'lastInstant', help = 'last instant for measurement', required = True, type = int)
 args = parser.parse_args()
 
-# args.homographyFilename is None if nothing as argument
 if args.homographyFilename != None:
     homography = loadtxt(args.homographyFilename)
 else:
     homography = None
 
-firstInstant = int(args.firstInstant)
-lastInstant = int(args.lastInstant)
-
 objects = storage.loadTrajectoriesFromSqlite(args.trackerDatabaseFilename, 'object')
 annotations = storage.loadGroundTruthFromSqlite(args.groundTruthDatabaseFilename)
 for a in annotations:
     a.computeCentroidTrajectory(homography)
 
-matchTable = moving.matchingGroundTruthToTracker(objects, annotations, args.matchingDistance, 
-                                                 firstInstant, lastInstant)
+motp, mota, mt, mme, fpt, gt = moving.computeClearMOT(objects, annotations, args.matchingDistance, args.firstInstant, args.lastInstant)
 
-# number of frames of existence of all objects within [firstInstant, lastInstant]
-nTrackFrames = sum([min(o.getLastInstant(),lastInstant)-max(o.getFirstInstant(),firstInstant)+1 for o in objects])
-
-print moving.computeClearMOT(matchTable, nTrackFrames)
-
+print 'MOTP: {}'.format(motp)
+print 'MOTA: {}'.format(mota)
+print 'Number of missed objects.frames: {}'.format(mt)
+print 'Number of mismatches: {}'.format(mme)
+print 'Number of false alarms.frames: {}'.format(fpt)