--- a/python/cvutils.py	Fri Aug 07 01:05:01 2015 -0400
+++ b/python/cvutils.py	Fri Aug 07 13:07:53 2015 -0400
@@ -25,9 +25,22 @@
 cvRed = (0,0,255)
 cvGreen = (0,255,0)
 cvBlue = (255,0,0)
+cvCyan = (255, 255, 0)
+cvYellow = (0, 255, 255)
+cvMagenta = (255, 0, 255)
+cvWhite = (255, 255, 255)
+cvBlack = (0,0,0)
+cvColors3 = utils.PlottingPropertyValues([cvRed,
+                                          cvGreen,
+                                          cvBlue])
 cvColors = utils.PlottingPropertyValues([cvRed,
                                          cvGreen,
-                                         cvBlue])
+                                         cvBlue,
+                                         cvCyan,
+                                         cvYellow,
+                                         cvMagenta,
+                                         cvWhite,
+                                         cvBlack])
 
 def quitKey(key):
     return chr(key&255)== 'q' or chr(key&255) == 'Q'
@@ -58,6 +71,9 @@
     #out = utils.openCheck(resultFilename)
     img.save(resultFilename)
 
+def rgb2gray(rgb):
+    return dot(rgb[...,:3], [0.299, 0.587, 0.144])
+
 def matlab2PointCorrespondences(filename):
     '''Loads and converts the point correspondences saved 
     by the matlab camera calibration tool'''
@@ -98,10 +114,11 @@
         return cvmat
 
     def cvPlot(img, positions, color, lastCoordinate = None, **kwargs):
-        last = lastCoordinate+1
-        if lastCoordinate is not None and lastCoordinate >=0:
+        if lastCoordinate is None:
+            last = positions.length()-1
+        elif lastCoordinate >=0:
             last = min(positions.length()-1, lastCoordinate)
-        for i in range(0, last-1):
+        for i in range(0, last):
             cv2.line(img, positions[i].asint().astuple(), positions[i+1].asint().astuple(), color, **kwargs)
 
     def cvImshow(windowName, img, rescale = 1.0):
@@ -264,7 +281,7 @@
         return croppedImg, yCropMin, yCropMax, xCropMin, xCropMax
 
 
-    def displayTrajectories(videoFilename, objects, boundingBoxes = {}, homography = None, firstFrameNum = 0, lastFrameNumArg = None, printFrames = True, rescale = 1., nFramesStep = 1, saveAllImages = False, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
+    def displayTrajectories(videoFilename, objects, boundingBoxes = {}, homography = None, firstFrameNum = 0, lastFrameNumArg = None, printFrames = True, rescale = 1., nFramesStep = 1, saveAllImages = False, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., annotations = [], gtMatches = {}, toMatches = {}):
         '''Displays the objects overlaid frame by frame over the video '''
         from moving import userTypeNames
         from math import ceil, log10
@@ -301,6 +318,7 @@
                     if len(objectToDeleteIds) > 0:
                         objects = [o for o in objects if o.getNum() not in objectToDeleteIds]
                         objectToDeleteIds = []
+                    # plot objects
                     for obj in objects:
                         if obj.existsAtInstant(frameNum):
                             if obj.getLastInstant() == frameNum:
@@ -310,18 +328,32 @@
                                     obj.projectedPositions = obj.positions.project(homography)
                                 else:
                                     obj.projectedPositions = obj.positions
-                            cvPlot(img, obj.projectedPositions, cvRed, frameNum-obj.getFirstInstant())
+                            cvPlot(img, obj.projectedPositions, cvColors[obj.getNum()], frameNum-obj.getFirstInstant())
                             if frameNum not in boundingBoxes.keys() and obj.hasFeatures():
                                 imgcrop, yCropMin, yCropMax, xCropMin, xCropMax = imageBox(img, obj, frameNum, homography, width, height)
                                 cv2.rectangle(img, (xCropMin, yCropMin), (xCropMax, yCropMax), cvBlue, 1)
                             objDescription = '{} '.format(obj.num)
                             if userTypeNames[obj.userType] != 'unknown':
                                 objDescription += userTypeNames[obj.userType][0].upper()
-                            cv2.putText(img, objDescription, obj.projectedPositions[frameNum-obj.getFirstInstant()].asint().astuple(), cv2.cv.CV_FONT_HERSHEY_PLAIN, 1, cvRed)
+                            if len(annotations) > 0: # if we loaded annotations, but there is no match
+                                if frameNum not in toMatches[obj.getNum()]:
+                                    objDescription += " FA"
+                            cv2.putText(img, objDescription, obj.projectedPositions[frameNum-obj.getFirstInstant()].asint().astuple(), cv2.cv.CV_FONT_HERSHEY_PLAIN, 1, cvColors[obj.getNum()])
                     # plot object bounding boxes
                     if frameNum in boundingBoxes.keys():
                         for rect in boundingBoxes[frameNum]:
-                            cv2.rectangle(img, rect[0].asint().astuple(), rect[1].asint().astuple(), cvRed)
+                            cv2.rectangle(img, rect[0].asint().astuple(), rect[1].asint().astuple(), cvColors[obj.getNum()])
+                    # plot ground truth
+                    if len(annotations) > 0:
+                        for gt in annotations:
+                            if gt.existsAtInstant(frameNum):
+                                if frameNum in gtMatches[gt.getNum()]:
+                                    color = cvColors[gtMatches[gt.getNum()][frameNum]] # same color as object
+                                else:
+                                    color = cvRed
+                                    cv2.putText(img, 'Miss', gt.topLeftPositions[frameNum-gt.getFirstInstant()].asint().astuple(), cv2.cv.CV_FONT_HERSHEY_PLAIN, 1, cvRed)
+                                cv2.rectangle(img, gt.topLeftPositions[frameNum-gt.getFirstInstant()].asint().astuple(), gt.bottomRightPositions[frameNum-gt.getFirstInstant()].asint().astuple(), color)
+                    # saving images and going to next
                     if not saveAllImages:
                         cvImshow(windowName, img, rescale)
                         key = cv2.waitKey()