Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 258:d90be3c02267

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
reasonably efficient computation of collision points and crossing zones
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 24 Jul 2012 12:37:47 -0400
parents 9281878ff19e
children 8ab76b95ee72
comparison
equal deleted inserted replaced
257:9281878ff19e 258:d90be3c02267
7 class ExtrapolatedTrajectory: 7 class ExtrapolatedTrajectory:
8 '''Class for extrapolated trajectories with lazy evaluation 8 '''Class for extrapolated trajectories with lazy evaluation
9 if the predicted position has not been already computed, compute it 9 if the predicted position has not been already computed, compute it
10 10
11 it should also have a probability''' 11 it should also have a probability'''
12
13 def __init__(self):
14 self.probability = 0.
15 self.predictedPositions = {}
16 self.predictedSpeedOrientations = {}
17 self.collisionPoints = {}
18 self.crossingZones = {}
12 19
13 def predictPosition(self, nTimeSteps): 20 def predictPosition(self, nTimeSteps):
14 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 21 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
15 self.predictPosition(nTimeSteps-1) 22 self.predictPosition(nTimeSteps-1)
16 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) 23 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
61 def __init__(self, name): 68 def __init__(self, name):
62 self.name = name 69 self.name = name
63 70
64 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): 71 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
65 '''extrapolationParameters specific to each method (in name field) ''' 72 '''extrapolationParameters specific to each method (in name field) '''
66 if extrapolationHypothesis.name == 'constant velocity': 73 if extrapolationParameters.name == 'constant velocity':
67 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] 74 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
68 else: 75 else:
69 print('Unknown extrapolation hypothesis') 76 print('Unknown extrapolation hypothesis')
70 return [] 77 return []
71 78
79 class CollisionPoint(moving.Point):
80 def __init__(self, p, probability = 1., TTC = -1):
81 self.x = p.x
82 self.y = p.y
83 self.probability = probability
84 self.TTC = TTC
85
86 class CrossingZone(moving.Point):
87 def __init__(self, p, probability = 1., pPET = -1):
88 self.x = p.x
89 self.y = p.y
90 self.probability = probability
91 self.pPET = pPET
92
72 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): 93 def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
73 '''returns the lists of collision points and crossing zones ''' 94 '''returns the lists of collision points and crossing zones '''
74 collisionPoints = [] 95 collisionPoints = []
75 TTCs = []
76 crossingZones = [] 96 crossingZones = []
77 pPETs = []
78 for et1 in extrapolatedTrajectories1: 97 for et1 in extrapolatedTrajectories1:
79 for et2 in extrapolatedTrajectories2: 98 for et2 in extrapolatedTrajectories2:
80 99
81 t = 1 100 t = 1
82 p1 = et1.predictPosition(t) 101 p1 = et1.predictPosition(t)
83 p2 = et2.predictPosition(t) 102 p2 = et2.predictPosition(t)
84 while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold: 103 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
85 p1 = et1.predictPosition(t) 104 p1 = et1.predictPosition(t)
86 p2 = et2.predictPosition(t) 105 p2 = et2.predictPosition(t)
87 t += 1 106 t += 1
88 107
89 if t <= timeHorizon: 108 if t <= timeHorizon:
90 TTCs.append(t) # todo store probability 109 collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
91 collisionPoints.append((p1+p2).multiply(0.5))
92 else: # check if there is a crossing zone 110 else: # check if there is a crossing zone
93 for t1 in xrange(timeHorizon-1): 111 # TODO? zone should be around the points at which the traj are the closest
94 for t2 in xrange(timeHorizon-1): 112 # look for CZ at different times, otherwise it would be a collision
95 cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) 113 # an approximation would be to look for close points at different times, ie the complementary of collision points
114 cz = None
115 t1 = 0
116 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
117 t2 = 0
118 while not cz and t2 < timeHorizon:
119 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
120 # 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))
96 if cz: 122 if cz:
97 crossingZones.append(cz) 123 crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2)))
98 pPETs.append(abs(t1-t2)) 124 t2 += 1
99 break 125 t1 += 1
100 return collisionPoints, crossingZones, TTCs, pPETs 126 return collisionPoints, crossingZones
101 127
102 128
103 129
104 # Default values: to remove because we cannot tweak that from a script where the value may be different 130 # Default values: to remove because we cannot tweak that from a script where the value may be different
105 FPS= 25 # No. of frame per second (FPS) 131 FPS= 25 # No. of frame per second (FPS)