Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 259:8ab76b95ee72

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added code to save collision points and crossing zones in txt files
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 24 Jul 2012 15:18:12 -0400
parents d90be3c02267
children 36cb40c51a5e
comparison
equal deleted inserted replaced
258:d90be3c02267 259:8ab76b95ee72
74 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] 74 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
75 else: 75 else:
76 print('Unknown extrapolation hypothesis') 76 print('Unknown extrapolation hypothesis')
77 return [] 77 return []
78 78
79 class CollisionPoint(moving.Point): 79 class SafetyPoint(moving.Point):
80 def __init__(self, p, probability = 1., TTC = -1): 80 '''Can represent a collision point or crossing zone
81 with respective safety indicator, TTC or pPET'''
82 def __init__(self, p, probability = 1., indicator = -1):
81 self.x = p.x 83 self.x = p.x
82 self.y = p.y 84 self.y = p.y
83 self.probability = probability 85 self.probability = probability
84 self.TTC = TTC 86 self.indicator = indicator
85 87
86 class CrossingZone(moving.Point): 88 def computeExpectedIndicator(points):
87 def __init__(self, p, probability = 1., pPET = -1): 89 from numpy import sum
88 self.x = p.x 90 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
89 self.y = p.y
90 self.probability = probability
91 self.pPET = pPET
92 91
93 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): 92 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
94 '''returns the lists of collision points and crossing zones ''' 93 '''returns the lists of collision points and crossing zones '''
95 collisionPoints = [] 94 collisionPoints = []
96 crossingZones = [] 95 crossingZones = []
104 p1 = et1.predictPosition(t) 103 p1 = et1.predictPosition(t)
105 p2 = et2.predictPosition(t) 104 p2 = et2.predictPosition(t)
106 t += 1 105 t += 1
107 106
108 if t <= timeHorizon: 107 if t <= timeHorizon:
109 collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) 108 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
110 else: # check if there is a crossing zone 109 else: # check if there is a crossing zone
111 # TODO? zone should be around the points at which the traj are the closest 110 # TODO? zone should be around the points at which the traj are the closest
112 # look for CZ at different times, otherwise it would be a collision 111 # look for CZ at different times, otherwise it would be a collision
113 # an approximation would be to look for close points at different times, ie the complementary of collision points 112 # an approximation would be to look for close points at different times, ie the complementary of collision points
114 cz = None 113 cz = None
118 while not cz and t2 < timeHorizon: 117 while not cz and t2 < timeHorizon:
119 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: 118 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
120 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) 119 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
121 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) 120 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
122 if cz: 121 if cz:
123 crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2))) 122 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
124 t2 += 1 123 t2 += 1
125 t1 += 1 124 t1 += 1
126 return collisionPoints, crossingZones 125 return collisionPoints, crossingZones
127 126
128 127