Mercurial Hosting > traffic-intelligence

changeset 610:0dc36203973d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
remove dublicated code for collision/crossing computations
author MohamedGomaa
date Wed, 03 Dec 2014 22:57:08 -0500
parents 84690dfe5560
children 233867934190
files python/events.py python/prediction.py
diffstat 2 files changed, 57 insertions(+), 80 deletions(-) [+]
line wrap: on
line diff
--- a/python/events.py	Tue Nov 25 22:49:47 2014 -0500
+++ b/python/events.py	Wed Dec 03 22:57:08 2014 -0500
@@ -8,12 +8,17 @@
 import multiprocessing
 import itertools
 
-import sys
 import moving, prediction, indicators, utils
-sys.path.append("D:/behaviourAnalysis/libs")
 import trajLearning
 __metaclass__ = type
 
+def getRoute(obj,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination=True):
+    route=(obj.startRouteID,obj.endRouteID)
+    if useDestination:
+        if route not in prototypes.keys():
+            route= trajLearning.findRoute(prototypes,objects,route,obj.getNum(),noiseEntryNums,noiseExitNums)
+    return route
+
 class Interaction(moving.STObject):
     '''Class for an interaction between two road users 
     or a road user and an obstacle
@@ -126,17 +131,20 @@
                 minDistance[instant] = moving.MovingObject.minDistance(self.roadUser1, self.roadUser2, instant)
             self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[3], minDistance))
 
-    def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
+    def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,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):
         '''Computes all crossing and collision points at each common instant for two road users. '''
         self.collisionPoints={}
         self.crossingZones={}
         TTCs = {}
+        if usePrototypes:
+            route1= getRoute(self.roadUser1,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination)
+            route2= getRoute(self.roadUser2,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination)
 
         if timeInterval:
             commonTimeInterval = timeInterval
         else:
             commonTimeInterval = self.timeInterval
-        self.collisionPoints, self.crossingZones = prediction.computeCrossingsCollisions(predictionParameters, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses)
+        self.collisionPoints, self.crossingZones = prediction.computeCrossingsCollisions(predictionParameters, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
         for i, cp in self.collisionPoints.iteritems():
             TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp)
         # add probability of collision, and probability of successful evasive action
@@ -144,70 +152,8 @@
         
         if computeCZ:
             pPETs = {}
-            for i in list(commonTimeInterval)[:-1]:
-                if len(self.crossingZones[i]) > 0:
-                    pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i])
-            self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs))
-
-    def computeCrosssingCollisionsPrototypeAtInstant(self, instant,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None, computeCZ = False, debug = False,useDestination=True,useSpeedPrototype=True):
-        inter1=moving.Interval(self.roadUser1.timeInterval.first,instant)
-        inter2=moving.Interval(self.roadUser2.timeInterval.first,instant)
-        partialObjPositions1= self.roadUser1.getObjectInTimeInterval(inter1).positions
-        partialObjPositions2= self.roadUser2.getObjectInTimeInterval(inter2).positions
-        if useSpeedPrototype:
-            prototypeTrajectories1=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route1,partialObjPositions1,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
-            prototypeTrajectories2=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route2,partialObjPositions2,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
-        else:
-            prototypeTrajectories1=trajLearning.findPrototypes(prototypes,nMatching,objects,route1,partialObjPositions1,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
-            prototypeTrajectories2=trajLearning.findPrototypes(prototypes,nMatching,objects,route2,partialObjPositions2,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
-        if prototypeTrajectories1=={}:
-            print self.roadUser1.num, 'is abnormal at instant', str(instant)
-            return [],[]
-        elif prototypeTrajectories2=={}:
-            print self.roadUser2.num, 'is abnormal at instant', str(instant)
-            return [],[]
-        else:
-            currentInstant,collisionPoints, crossingZones = predictionParameters.computeCrossingsCollisionsAtInstant(instant, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)
-            return collisionPoints,crossingZones
-
-    def computeCrossingsCollisionsPrototype2stages(self, predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,step=10,minSimilarity=0.1,mostMatched=None, computeCZ = False, debug = False, timeInterval = None,acceptPartialLength=30,useDestination=True,useSpeedPrototype=True):
-        '''Computes all crossing and collision points at each common instant for two road users. '''
-        self.collisionPoints={}
-        self.crossingZones={}
-        TTCs = {}
-        route1=(self.roadUser1.startRouteID,self.roadUser1.endRouteID)
-        route2=(self.roadUser2.startRouteID,self.roadUser2.endRouteID)
-        if useDestination:
-            if route1 not in prototypes.keys():
-                route1= trajLearning.findRoute(prototypes,objects,route1,self.roadUser1.num,noiseEntryNums,noiseExitNums)
-            if route2 not in prototypes.keys():
-                route2= trajLearning.findRoute(prototypes,objects,route2,self.roadUser2.num,noiseEntryNums,noiseExitNums)
-				
-        if timeInterval:
-            commonTimeInterval = timeInterval
-        else:
-            commonTimeInterval = self.timeInterval
-        reCompute=False
-        for i in xrange(commonTimeInterval.first,commonTimeInterval.last,step): # incremental calculation of CP,CZ to save time
-            if  i-self.roadUser1.timeInterval.first >= acceptPartialLength and i-self.roadUser2.timeInterval.first >= acceptPartialLength:
-				self.collisionPoints[i], self.crossingZones[i] = self.computeCrosssingCollisionsPrototypeAtInstant(i,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched, computeCZ, debug,useDestination,useSpeedPrototype)
-				if len(self.collisionPoints[i]) >0 or len(self.crossingZones[i])>0:
-					reCompute=True
-					break
-        if reCompute:
-            for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-				if  i-self.roadUser1.timeInterval.first >= acceptPartialLength and i-self.roadUser2.timeInterval.first >= acceptPartialLength:
-					self.collisionPoints[i], self.crossingZones[i] = self.computeCrosssingCollisionsPrototypeAtInstant(i,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched, computeCZ, debug,useDestination,useSpeedPrototype)		
-					if len(self.collisionPoints[i]) > 0:
-						TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.collisionPoints[i])
-        # add probability of collision, and probability of successful evasive action
-        self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs))
-        
-        if computeCZ:
-            pPETs = {}
-            for i in list(commonTimeInterval)[:-1]:
-                if i in self.crossingZones.keys() and len(self.crossingZones[i]) > 0:
-                    pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i])
+            for i, cz in self.crossingZones.iteritems():
+                pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(cz)
             self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs))
 			
     def addVideoFilename(self,videoFilename):
--- a/python/prediction.py	Tue Nov 25 22:49:47 2014 -0500
+++ b/python/prediction.py	Wed Dec 03 22:57:08 2014 -0500
@@ -5,6 +5,7 @@
 import math
 import random
 import numpy as np
+import trajLearning
 
 class PredictedTrajectory:
     '''Class for predicted trajectories with lazy evaluation
@@ -166,14 +167,25 @@
     savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
     close()
 
-def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None):  
+def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
+    partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant)
+    partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions	
+    if useSpeedPrototype:
+        prototypeTrajectories=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
+    else:
+        prototypeTrajectories=trajLearning.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=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):  
     '''returns the lists of collision points and crossing zones'''
-    if prototypeTrajectories1==None: 
+    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)		
-    else:
-        predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
-        predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
 
     collisionPoints = []
     crossingZones = []
@@ -205,7 +217,7 @@
         savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
     return currentInstant,collisionPoints, crossingZones
 
-def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,prototypeTrajectories1=None,prototypeTrajectories2=None):
+def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,usePrototypes=True,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):
     '''Computes all crossing and collision points at each common instant for two road users. '''
     collisionPoints={}
     crossingZones={}
@@ -214,8 +226,27 @@
     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= list(xrange(firstInstant,commonTimeInterval.last))
+            commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last,step))
+            for i in commonTimeIntervalList2[:-1]: # do not look at the 1 last position/velocities, often with errors
+                i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, 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[:-1]:
+                    if i not in commonTimeIntervalList2:
+                        i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, 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						
+	
         for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-            i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)
+            i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, 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:
@@ -223,7 +254,7 @@
     else:
         from multiprocessing import Pool
         pool = Pool(processes = nProcesses)
-        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)) for i in list(commonTimeInterval)[:-1]]
+        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
         #results = [j.get() for j in jobs]
         #results.sort()
         for j in jobs:
@@ -247,11 +278,11 @@
     def generatePredictedTrajectories(self, obj, instant):
         return []
 
-    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None):
-        return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,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,prototypeTrajectories1=None,prototypeTrajectories2=None):
-        return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,prototypeTrajectories1,prototypeTrajectories2)
+    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,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):
+        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