Mercurial Hosting > traffic-intelligence
changeset 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 | 2d1d33ae1c69 |
files | python/events.py python/moving.py python/prediction.py |
diffstat | 3 files changed, 111 insertions(+), 61 deletions(-) [+] |
line wrap: on
line diff
--- a/python/events.py Tue Feb 17 00:56:47 2015 +0100 +++ b/python/events.py Tue Feb 17 02:21:31 2015 +0100 @@ -196,7 +196,7 @@ commonTimeInterval = timeInterval else: commonTimeInterval = self.timeInterval - 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) + self.collisionPoints, self.crossingZones = predictionParameters.computeCrossingsCollisions(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
--- a/python/moving.py Tue Feb 17 00:56:47 2015 +0100 +++ b/python/moving.py Tue Feb 17 02:21:31 2015 +0100 @@ -579,6 +579,15 @@ else: return None +def segmentLineIntersection(p1, p2, p3, p4): + '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' + inter = intersection(p1, p2, p3, p4) + if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y)): + return inter + else: + return None + + class Trajectory(object): '''Class for trajectories: temporal sequence of positions @@ -808,7 +817,7 @@ q1=self.__getitem__(i) q2=self.__getitem__(i+1) p = utils.segmentIntersection(q1, q2, p1, p2) - if p: + if p != None: if q1.x != q2.x: ratio = (p.x-q1.x)/(q2.x-q1.x) elif q1.y != q2.y: @@ -818,6 +827,28 @@ indices.append(i+ratio) return indices + def getLineIntersections(self, p1, p2): + '''Returns a list of the indices at which the trajectory + intersects with the segment of extremities p1 and p2 + the list is empty if there is no crossing''' + indices = [] + intersections = [] + + for i in xrange(self.length()-1): + q1=self.__getitem__(i) + q2=self.__getitem__(i+1) + p = utils.segmentLineIntersection(p1, p2, q1, q2) + if p != None: + if q1.x != q2.x: + ratio = (p.x-q1.x)/(q2.x-q1.x) + elif q1.y != q2.y: + ratio = (p.y-q1.y)/(q2.y-q1.y) + else: + ratio = 0 + indices.append(i+ratio) + intersections.append(p) + return indices, intersections + def getTrajectoryInInterval(self, inter): 'Returns all position between index inter.first and index.last (included)' if inter.first >=0 and inter.last<= self.length():
--- 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)