--- a/python/prediction.py	Wed Jul 19 18:02:38 2017 -0400
+++ b/python/prediction.py	Thu Jul 20 12:12:28 2017 -0400
@@ -159,22 +159,26 @@
         collision = (p1-p2).norm2() <= collisionDistanceThreshold
     return collision, t, p1, p2
 
-def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
+def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
     from matplotlib.pyplot import figure, axis, title, clf, savefig
-    figure()
-    #clf()
+    if printFigure:
+        clf()
+    else:
+        figure()
     for et in predictedTrajectories1:
-        et.predictPosition(int(np.round(timeHorizon)))
-        et.plot('rx')
-
+        for t in xrange(int(np.round(timeHorizon))):
+            et.predictPosition(t)
+            et.plot('rx')
     for et in predictedTrajectories2:
-        et.predictPosition(int(np.round(timeHorizon)))
-        et.plot('bx')
+        for t in xrange(int(np.round(timeHorizon))):
+            et.predictPosition(t)
+            et.plot('bx')
     obj1.plot('r', withOrigin = True)
     obj2.plot('b', withOrigin = True)
     title('instant {0}'.format(currentInstant))
     axis('equal')
-    savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
+    if printFigure:
+        savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
 
 def calculateProbability(nMatching,similarity,objects):
     sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
@@ -254,16 +258,10 @@
         prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
     return prototypeTrajectories
 
-def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):  
+def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
     '''returns the lists of collision points and crossing zones'''
-    # if usePrototypes:
-    #     prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-    #     prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-    #     predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
-    #     predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)     
-    # else:
     predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
-    predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)        
+    predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
 
     collisionPoints = []
     crossingZones = []
@@ -273,16 +271,12 @@
             if collision:
                 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
             elif computeCZ: # check if there is a crossing zone
-                # TODO? zone should be around the points at which the traj are the closest
-                # look for CZ at different times, otherwise it would be a collision
-                # an approximation would be to look for close points at different times, ie the complementary of collision points
+                # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
                 cz = None
                 t1 = 0
                 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
                     t2 = 0
                     while not cz and t2 < timeHorizon:
-                        #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
-                        #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
                         cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
                         if cz is not None:
                             deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
@@ -307,11 +301,7 @@
     def generatePredictedTrajectories(self, obj, instant):
         return []
 
-#    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
-#        return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-
-    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
-        #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
+    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
         '''Computes all crossing and collision points at each common instant for two road users. '''
         collisionPoints={}
         crossingZones={}
@@ -320,46 +310,23 @@
         else:
             commonTimeInterval = obj1.commonTimeInterval(obj2)
         if nProcesses == 1:
-            # if usePrototypes:
-            #     firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
-            #     commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
-            #     commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
-            #     for i in commonTimeIntervalList2: 
-            #         i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-            #         if len(cp) != 0:
-            #             collisionPoints[i] = cp
-            #         if len(cz) != 0:
-            #             crossingZones[i] = cz
-            #     if collisionPoints!={} or crossingZones!={}:
-            #         for i in commonTimeIntervalList1:
-            #             if i not in commonTimeIntervalList2:
-            #                 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
-            #                 if len(cp) != 0:
-            #                     collisionPoints[i] = cp
-            #                 if len(cz) != 0:
-            #                     crossingZones[i] = cz                        
-            # else:
             for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-                i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
                 if len(cp) != 0:
                     collisionPoints[i] = cp
                 if len(cz) != 0:
                     crossingZones[i] = cz
         else:
             pool = Pool(processes = nProcesses)
-            jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] #,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype
-            #results = [j.get() for j in jobs]
-            #results.sort()
+            jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
             for j in jobs:
                 i, cp, cz = j.get()
-                #if len(cp) != 0 or len(cz) != 0:
                 if len(cp) != 0:
                     collisionPoints[i] = cp
                 if len(cz) != 0:
                     crossingZones[i] = cz
             pool.close()
         return collisionPoints, crossingZones
-#return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
 
     def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
         '''Computes only collision probabilities
@@ -437,7 +404,6 @@
 class PointSetPredictionParameters(PredictionParameters):
     def __init__(self, maxSpeed):
         PredictionParameters.__init__(self, 'point set', maxSpeed)
-        #self.nPredictedTrajectories = nPredictedTrajectories
     
     def generatePredictedTrajectories(self, obj, instant):
         predictedTrajectories = []
@@ -519,8 +485,6 @@
                 dp2 = intersection-p2
                 dot1 = moving.Point.dot(dp1, v1)
                 dot2 = moving.Point.dot(dp2, v2)
-                #print dot1, dot2
-                # (computeCZ and (dot1 > 0 or dot2 > 0)) or (
                 if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET
                     dist1 = dp1.norm2()
                     dist2 = dp2.norm2()
@@ -579,12 +543,11 @@
         return currentInstant, collisionPoints, crossingZones
 
 class PrototypePredictionParameters(PredictionParameters):
-    def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters
+    def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
         PredictionParameters.__init__(self, 'prototypes', None)
         self.prototypes = prototypes
         self.nPredictedTrajectories = nPredictedTrajectories
         self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
-        #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y)
         self.minSimilarity = minSimilarity
         self.minFeatureTime = minFeatureTime
         self.constantSpeed = constantSpeed

--- a/scripts/safety-analysis.py	Wed Jul 19 18:02:38 2017 -0400
+++ b/scripts/safety-analysis.py	Thu Jul 20 12:12:28 2017 -0400
@@ -55,7 +55,7 @@
         prototypes = storage.loadPrototypesFromSqlite(args.prototypeDatabaseFilename)
     for p in prototypes:
         p.getMovingObject().computeCumulativeDistances()
-    predictionParameters = prediction.PrototypePredictionParameters(prototypes, params.nPredictedTrajectories, 2., 0.5, 'cityblock', 10, params.constantSpeedPrototypePrediction, params.useFeaturesForPrediction)
+    predictionParameters = prediction.PrototypePredictionParameters(prototypes, params.nPredictedTrajectories, 2., 0.4, 'cityblock', 10, params.constantSpeedPrototypePrediction, params.useFeaturesForPrediction)
 # else:
 # no else required, since parameters is required as argument