Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 259:8ab76b95ee72
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 |