Mercurial Hosting > traffic-intelligence
diff python/prediction.py @ 630:69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 17 Feb 2015 02:21:31 +0100 |
parents | 0a5e89d6fc62 |
children | 3058e00887bc |
line wrap: on
line diff
--- a/python/prediction.py Tue Feb 17 00:56:47 2015 +0100 +++ b/python/prediction.py Tue Feb 17 02:21:31 2015 +0100 @@ -287,55 +287,6 @@ return currentInstant, collisionPoints, crossingZones -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): - '''Computes all crossing and collision points at each common instant for two road users. ''' - collisionPoints={} - crossingZones={} - if timeInterval: - commonTimeInterval = timeInterval - 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-1)) # do not look at the 1 last position/velocities, often with errors - commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors - for i 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 - if collisionPoints!={} or crossingZones!={}: - for i in commonTimeIntervalList1: - 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 - else: - 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,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: - from multiprocessing import Pool - pool = Pool(processes = nProcesses) - 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: - 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 class PredictionParameters: def __init__(self, name, maxSpeed): @@ -352,7 +303,56 @@ 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=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 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): + '''Computes all crossing and collision points at each common instant for two road users. ''' + collisionPoints={} + crossingZones={} + if timeInterval: + commonTimeInterval = timeInterval + 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-1)) # do not look at the 1 last position/velocities, often with errors + commonTimeIntervalList2= list(xrange(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 = 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: + from multiprocessing import Pool + pool = Pool(processes = nProcesses) + jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, 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: + 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 @@ -487,7 +487,7 @@ def __init__(self): PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) - def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): + def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): collisionPoints = [] crossingZones = [] @@ -498,24 +498,43 @@ else: v1 = obj1.getVelocityAtInstant(currentInstant) v2 = obj2.getVelocityAtInstant(currentInstant) - intersection = moving.intersection(p1, p2, v1, v2) + intersection = moving.intersection(p1, p1+v1, p2, p2+v2) if intersection != None: dp1 = intersection-p1 dp2 = intersection-p2 - if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection + 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() - s1 = v1.norm2() - s2 = v2.norm2() + s1 = math.copysign(v1.norm2(), dot1) + s2 = math.copysign(v2.norm2(), dot2) halfCollisionDistanceThreshold = collisionDistanceThreshold/2. timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1) timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) - if computeCZ and collisionTimeInterval.empty(): - crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] + + if collisionTimeInterval.empty(): + if computeCZ: + crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] else: collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] + # elif computeCZ and (dot1 > 0 or dot2 > 0): + # if dot1 > 0: + # firstUser = obj2 # first through crossingzone + # secondUser = obj1 # second through crossingzone + # elif dot2 > 0: + # firstUser = obj1 + # secondUser = obj2 + # p2 = secondUser.getPositionAtInstant(currentInstant) + # v2 = secondUser.getVelocityAtInstant(currentInstant) + # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2) + # if indices != None: + # pass + # else: # one has to predict !!! if debug and intersection!= None: from matplotlib.pyplot import plot, figure, axis, title @@ -528,7 +547,7 @@ title('instant {0}'.format(currentInstant)) axis('equal') - return collisionPoints, crossingZones + return currentInstant, collisionPoints, crossingZones class CVExactPredictionParameters(PredictionParameters): '''Prediction parameters of prediction at constant velocity @@ -546,7 +565,7 @@ p2 = obj2.getPositionAtInstant(currentInstant) v1 = obj1.getVelocityAtInstant(currentInstant) v2 = obj2.getVelocityAtInstant(currentInstant) - intersection = moving.intersection(p1, p2, v1, v2) + intersection = moving.intersection(p1, p1+v1, p2, p2+v2) if intersection != None: ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)