Mercurial Hosting > traffic-intelligence
annotate python/prediction.py @ 680:da1352b89d02 dev
classification is working
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 05 Jun 2015 02:25:30 +0200 |
parents | 15e244d2a1b5 |
children | 9a258687af4c |
rev | line source |
---|---|
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
1 #! /usr/bin/env python |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
2 '''Library for motion prediction methods''' |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
3 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
4 import moving |
613 | 5 from utils import LCSS |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
6 |
665
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
7 import math, random |
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
8 import numpy as np |
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
9 |
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
10 |
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
11 class PredictedTrajectory(object): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
12 '''Class for predicted trajectories with lazy evaluation |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
13 if the predicted position has not been already computed, compute it |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
14 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
15 it should also have a probability''' |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
16 |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
17 def __init__(self): |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
18 self.probability = 0. |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
19 self.predictedPositions = {} |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
20 self.predictedSpeedOrientations = {} |
300
f65b828e5521
working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
296
diff
changeset
|
21 #self.collisionPoints = {} |
f65b828e5521
working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
296
diff
changeset
|
22 #self.crossingZones = {} |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
23 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
24 def predictPosition(self, nTimeSteps): |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
25 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
26 self.predictPosition(nTimeSteps-1) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
27 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
28 return self.predictedPositions[nTimeSteps] |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
29 |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
30 def getPredictedTrajectory(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
31 return moving.Trajectory.fromPointList(self.predictedPositions.values()) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
32 |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
33 def getPredictedSpeeds(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
34 return [so.norm for so in self.predictedSpeedOrientations.values()] |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
35 |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
489
diff
changeset
|
36 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs): |
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
489
diff
changeset
|
37 self.getPredictedTrajectory().plot(options, withOrigin, timeStep, **kwargs) |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
38 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
39 class PredictedTrajectoryConstant(PredictedTrajectory): |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
40 '''Predicted trajectory at constant speed or acceleration |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
41 TODO generalize by passing a series of velocities/accelerations''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
42 |
336
124f85c6cfae
modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
300
diff
changeset
|
43 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1., maxSpeed = None): |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
44 self.control = control |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
45 self.maxSpeed = maxSpeed |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
46 self.probability = probability |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
47 self.predictedPositions = {0: initialPosition} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
48 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
49 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
50 def getControl(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
51 return self.control |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
52 |
607 | 53 def findNearestParams(initialPosition,prototypeTrajectory): |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
54 ''' nearest parameters are the index of minDistance and the orientation ''' |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
55 distances=[] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
56 for position in prototypeTrajectory.positions: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
57 distances.append(moving.Point.distanceNorm2(initialPosition, position)) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
58 minDistanceIndex= np.argmin(distances) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
59 return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
60 |
466
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
61 class PredictedTrajectoryPrototype(PredictedTrajectory): |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
62 '''Predicted trajectory that follows a prototype trajectory |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
63 The prototype is in the format of a moving.Trajectory: it could be |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
64 1. an observed trajectory (extracted from video) |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
65 2. a generic polyline (eg the road centerline) that a vehicle is supposed to follow |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
66 |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
67 Prediction can be done |
467
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
68 1. at constant speed (the instantaneous user speed) |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
69 2. following the trajectory path, at the speed of the user |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
70 (applying a constant ratio equal |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
71 to the ratio of the user instantaneous speed and the trajectory closest speed)''' |
466
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
72 |
467
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
73 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.): |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
74 self.prototypeTrajectory = prototypeTrajectory |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
75 self.constantSpeed = constantSpeed |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
76 self.probability = probability |
08b67c9baca2
finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
466
diff
changeset
|
77 self.predictedPositions = {0: initialPosition} |
607 | 78 self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} |
79 | |
470
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
80 def predictPosition(self, nTimeSteps): |
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
81 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
607 | 82 if self.constantSpeed: |
470
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
83 # calculate cumulative distance |
607 | 84 speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm |
85 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] | |
86 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype) | |
87 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) | |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
88 |
470
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
89 else: # see c++ code, calculate ratio |
607 | 90 speedNorm= self.predictedSpeedOrientations[0].norm |
91 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] | |
92 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] | |
93 ratio=float(speedNorm)/prototypeSpeeds[0] | |
94 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] | |
95 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] | |
96 if nTimeSteps<len(resampledSpeeds): | |
97 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) | |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
98 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) |
607 | 99 else: |
100 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) | |
101 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) | |
102 | |
470
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
103 return self.predictedPositions[nTimeSteps] |
a84b9ba9631f
small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
467
diff
changeset
|
104 |
466
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
105 class PredictedTrajectoryRandomControl(PredictedTrajectory): |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
106 '''Random vehicle control: suitable for normal adaptation''' |
336
124f85c6cfae
modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
300
diff
changeset
|
107 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None): |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
108 '''Constructor |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
109 accelerationDistribution and steeringDistribution are distributions |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
110 that return random numbers drawn from them''' |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
111 self.accelerationDistribution = accelerationDistribution |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
112 self.steeringDistribution = steeringDistribution |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
113 self.maxSpeed = maxSpeed |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
114 self.probability = probability |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
115 self.predictedPositions = {0: initialPosition} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
116 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
117 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
118 def getControl(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
119 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
120 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
121 class SafetyPoint(moving.Point): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
122 '''Can represent a collision point or crossing zone |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
123 with respective safety indicator, TTC or pPET''' |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
124 def __init__(self, p, probability = 1., indicator = -1): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
125 self.x = p.x |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
126 self.y = p.y |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
127 self.probability = probability |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
128 self.indicator = indicator |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
129 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
130 def __str__(self): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
131 return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
132 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
133 @staticmethod |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
134 def save(out, points, predictionInstant, objNum1, objNum2): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
135 for p in points: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
136 out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p)) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
137 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
138 @staticmethod |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
139 def computeExpectedIndicator(points): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
140 from numpy import sum |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
141 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
142 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
143 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon): |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
144 '''Computes the first instant |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
145 at which two predicted trajectories are within some distance threshold |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
146 Computes all the times including timeHorizon |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
147 |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
148 User has to check the first variable collision to know about a collision''' |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
149 t = 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
150 p1 = predictedTrajectory1.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
151 p2 = predictedTrajectory2.predictPosition(t) |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
152 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
153 while t < timeHorizon and not collision: |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
154 t += 1 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
155 p1 = predictedTrajectory1.predictPosition(t) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
156 p2 = predictedTrajectory2.predictPosition(t) |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
157 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
158 return collision, t, p1, p2 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
159 |
557
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
160 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
161 from matplotlib.pyplot import figure, axis, title, close, savefig |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
162 figure() |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
163 for et in predictedTrajectories1: |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
164 et.predictPosition(timeHorizon) |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
165 et.plot('rx') |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
166 |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
167 for et in predictedTrajectories2: |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
168 et.predictPosition(timeHorizon) |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
169 et.plot('bx') |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
170 obj1.plot('r') |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
171 obj2.plot('b') |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
172 title('instant {0}'.format(currentInstant)) |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
173 axis('equal') |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
174 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
175 close() |
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
176 |
613 | 177 def calculateProbability(nMatching,similarity,objects): |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
178 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
179 prototypeProbability={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
180 for i in similarity.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
181 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
182 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()]) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
183 probabilities={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
184 for i in prototypeProbability.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
185 probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
186 return probabilities |
613 | 187 |
188 def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180): | |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
189 ''' behaviour prediction first step''' |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
190 if route[0] not in noiseEntryNums: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
191 prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
192 elif route[1] not in noiseExitNums: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
193 prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
194 else: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
195 prototypesRoutes=[x for x in sorted(prototypes.keys())] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
196 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
197 similarity={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
198 for y in prototypesRoutes: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
199 if y in prototypes.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
200 prototypesIDs=prototypes[y] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
201 for x in prototypesIDs: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
202 s=lcss.computeNormalized(partialObjPositions, objects[x].positions) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
203 if s >= minSimilarity: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
204 similarity[x]=s |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
205 |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
206 if mostMatched==None: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
207 probabilities= calculateProbability(nMatching,similarity,objects) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
208 return probabilities |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
209 else: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
210 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
211 keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
212 newSimilarity={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
213 for i in keys: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
214 newSimilarity[i]=similarity[i] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
215 probabilities= calculateProbability(nMatching,newSimilarity,objects) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
216 return probabilities |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
217 |
613 | 218 def findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,spatialThreshold=1.0, delta=180): |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
219 if useDestination: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
220 prototypesRoutes=[route] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
221 else: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
222 if route[0] not in noiseEntryNums: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
223 prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
224 elif route[1] not in noiseExitNums: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
225 prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
226 else: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
227 prototypesRoutes=[x for x in sorted(prototypes.keys())] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
228 lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
229 similarity={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
230 for y in prototypesRoutes: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
231 if y in prototypes.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
232 prototypesIDs=prototypes[y] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
233 for x in prototypesIDs: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
234 s=lcss.computeNormalized(partialObjPositions, objects[x].positions) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
235 if s >= minSimilarity: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
236 similarity[x]=s |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
237 |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
238 newSimilarity={} |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
239 for i in similarity.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
240 if i in secondStepPrototypes.keys(): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
241 for j in secondStepPrototypes[i]: |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
242 newSimilarity[j]=similarity[i] |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
243 probabilities= calculateProbability(nMatching,newSimilarity,objects) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
244 return probabilities |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
245 |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
246 def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
247 partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant) |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
248 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
249 if useSpeedPrototype: |
613 | 250 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
251 else: |
613 | 252 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
253 return prototypeTrajectories |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
254 |
629
0a5e89d6fc62
minor cleaning
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
619
diff
changeset
|
255 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
256 '''returns the lists of collision points and crossing zones''' |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
257 if usePrototypes: |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
258 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
259 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
260 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
261 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
262 else: |
607 | 263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
265 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
266 collisionPoints = [] |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
267 crossingZones = [] |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
268 for et1 in predictedTrajectories1: |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
269 for et2 in predictedTrajectories2: |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
270 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
271 |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
272 if collision: |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
273 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
274 elif computeCZ: # check if there is a crossing zone |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
275 # TODO? zone should be around the points at which the traj are the closest |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
276 # look for CZ at different times, otherwise it would be a collision |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
277 # an approximation would be to look for close points at different times, ie the complementary of collision points |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
278 cz = None |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
279 t1 = 0 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
280 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
281 t2 = 0 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
282 while not cz and t2 < timeHorizon: |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
283 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
284 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
285 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
286 if cz is not None: |
607 | 287 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() |
288 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) | |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
289 t2 += 1 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
290 t1 += 1 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
291 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
292 if debug: |
557
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
293 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
294 |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
295 return currentInstant, collisionPoints, crossingZones |
556
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
296 |
dc58ad777a72
modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
515
diff
changeset
|
297 |
665
15e244d2a1b5
corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
662
diff
changeset
|
298 class PredictionParameters(object): |
266
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
299 def __init__(self, name, maxSpeed): |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
300 self.name = name |
266
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
301 self.maxSpeed = maxSpeed |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
302 |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
303 def __str__(self): |
aba9711b3149
small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
265
diff
changeset
|
304 return '{0} {1}'.format(self.name, self.maxSpeed) |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
305 |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
306 def generatePredictedTrajectories(self, obj, instant): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
307 return [] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
308 |
651
335f6641bf7a
changed the paramter usePrototypes to False by default
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
309 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
610
0dc36203973d
remove dublicated code for collision/crossing computations
MohamedGomaa
parents:
607
diff
changeset
|
310 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
311 |
651
335f6641bf7a
changed the paramter usePrototypes to False by default
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
636
diff
changeset
|
312 def computeCrossingsCollisions(self, 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): |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
313 #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): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
314 '''Computes all crossing and collision points at each common instant for two road users. ''' |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
315 collisionPoints={} |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
316 crossingZones={} |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
317 if timeInterval: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
318 commonTimeInterval = timeInterval |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
319 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
320 commonTimeInterval = obj1.commonTimeInterval(obj2) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
321 if nProcesses == 1: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
322 if usePrototypes: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
324 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
325 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
326 for i in commonTimeIntervalList2: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
327 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) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
328 if len(cp) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
329 collisionPoints[i] = cp |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
330 if len(cz) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
331 crossingZones[i] = cz |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
332 if collisionPoints!={} or crossingZones!={}: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
333 for i in commonTimeIntervalList1: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
334 if i not in commonTimeIntervalList2: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
335 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) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
336 if len(cp) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
337 collisionPoints[i] = cp |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
338 if len(cz) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
339 crossingZones[i] = cz |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
340 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
341 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
342 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) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
343 if len(cp) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
344 collisionPoints[i] = cp |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
345 if len(cz) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
346 crossingZones[i] = cz |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
347 else: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
348 from multiprocessing import Pool |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
349 pool = Pool(processes = nProcesses) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
350 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]] |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
351 #results = [j.get() for j in jobs] |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
352 #results.sort() |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
353 for j in jobs: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
354 i, cp, cz = j.get() |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
355 #if len(cp) != 0 or len(cz) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
356 if len(cp) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
357 collisionPoints[i] = cp |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
358 if len(cz) != 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
359 crossingZones[i] = cz |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
360 pool.close() |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
361 return collisionPoints, crossingZones |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
362 #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) |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
363 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
364 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
365 '''Computes only collision probabilities |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
366 Returns for each instant the collision probability and number of samples drawn''' |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
367 collisionProbabilities = {} |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
368 if timeInterval: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
369 commonTimeInterval = timeInterval |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
370 else: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
371 commonTimeInterval = obj1.commonTimeInterval(obj2) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
372 for i in list(commonTimeInterval)[:-1]: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
373 nCollisions = 0 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
374 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
375 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
376 for et1 in predictedTrajectories1: |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
377 for et2 in predictedTrajectories2: |
662
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
378 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
72174e66aba5
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
661
diff
changeset
|
379 if collision: |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
380 nCollisions += 1 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
381 # take into account probabilities ?? |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
382 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2)) |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
383 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
384 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
385 if debug: |
557
b91f33e098ee
refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
556
diff
changeset
|
386 savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) |
358
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
387 |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
388 return collisionProbabilities |
c41ff9f3c263
moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
357
diff
changeset
|
389 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
390 class ConstantPredictionParameters(PredictionParameters): |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
391 def __init__(self, maxSpeed): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
392 PredictionParameters.__init__(self, 'constant velocity', maxSpeed) |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
393 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
394 def generatePredictedTrajectories(self, obj, instant): |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
395 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
396 maxSpeed = self.maxSpeed)] |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
397 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
398 class NormalAdaptationPredictionParameters(PredictionParameters): |
460
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
399 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
400 '''An example of acceleration and steering distributions is |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
401 lambda: random.triangular(-self.maxAcceleration, self.maxAcceleration, 0.) |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
402 ''' |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
403 if useFeatures: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
404 name = 'point set normal adaptation' |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
405 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
406 name = 'normal adaptation' |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
407 PredictionParameters.__init__(self, name, maxSpeed) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
408 self.nPredictedTrajectories = nPredictedTrajectories |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
409 self.useFeatures = useFeatures |
460
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
410 self.accelerationDistribution = accelerationDistribution |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
411 self.steeringDistribution = steeringDistribution |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
412 |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
413 def __str__(self): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
414 return PredictionParameters.__str__(self)+' {0} {1} {2}'.format(self.nPredictedTrajectories, |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
415 self.maxAcceleration, |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
416 self.maxSteering) |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
417 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
418 def generatePredictedTrajectories(self, obj, instant): |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
419 predictedTrajectories = [] |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
420 if self.useFeatures and obj.hadFeatures(): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
421 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
422 positions = [f.getPositionAtInstant(instant) for f in features] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
423 velocities = [f.getVelocityAtInstant(instant) for f in features] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
424 else: |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
425 positions = [obj.getPositionAtInstant(instant)] |
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
426 velocities = [obj.getVelocityAtInstant(instant)] |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
427 for i in xrange(self.nPredictedTrajectories): |
352
72aa44072093
safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
350
diff
changeset
|
428 for initialPosition,initialVelocity in zip(positions, velocities): |
466
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
429 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition, |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
430 initialVelocity, |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
431 self.accelerationDistribution, |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
432 self.steeringDistribution, |
e891a41c6c75
name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
464
diff
changeset
|
433 maxSpeed = self.maxSpeed)) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
434 return predictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
435 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
436 class PointSetPredictionParameters(PredictionParameters): |
269
a9988971aac8
removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
437 # todo generate several trajectories with normal adaptatoins from each position (feature) |
489
000bddf84ad0
corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
484
diff
changeset
|
438 def __init__(self, maxSpeed): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
439 PredictionParameters.__init__(self, 'point set', maxSpeed) |
489
000bddf84ad0
corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
484
diff
changeset
|
440 #self.nPredictedTrajectories = nPredictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
441 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
442 def generatePredictedTrajectories(self, obj, instant): |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
443 predictedTrajectories = [] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
444 if obj.hasFeatures(): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
445 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
446 positions = [f.getPositionAtInstant(instant) for f in features] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
447 velocities = [f.getVelocityAtInstant(instant) for f in features] |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
448 #for i in xrange(self.nPredictedTrajectories): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
449 for initialPosition,initialVelocity in zip(positions, velocities): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
450 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
451 maxSpeed = self.maxSpeed)) |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
452 return predictedTrajectories |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
453 else: |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
454 print('Object {} has no features'.format(obj.getNum())) |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
455 return None |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
456 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
457 class EvasiveActionPredictionParameters(PredictionParameters): |
460
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
458 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
459 '''Suggested acceleration distribution may not be symmetric, eg |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
460 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)''' |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
461 |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
462 if useFeatures: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
463 name = 'point set evasive action' |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
464 else: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
465 name = 'evasive action' |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
466 PredictionParameters.__init__(self, name, maxSpeed) |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
467 self.nPredictedTrajectories = nPredictedTrajectories |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
468 self.useFeatures = useFeatures |
460
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
469 self.accelerationDistribution = accelerationDistribution |
55b424d98b68
change of interface, distributions are now passed to the prediction paramters constructors if needed
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
387
diff
changeset
|
470 self.steeringDistribution = steeringDistribution |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
471 |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
472 def __str__(self): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
473 return PredictionParameters.__str__(self)+' {0} {1} {2} {3}'.format(self.nPredictedTrajectories, self.minAcceleration, self.maxAcceleration, self.maxSteering) |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
474 |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
475 def generatePredictedTrajectories(self, obj, instant): |
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
476 predictedTrajectories = [] |
661
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
477 if self.useFeatures and obj.hasFeatures(): |
dc70d9e711f5
some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
651
diff
changeset
|
478 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)] |
267
32e88b513f5c
added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
266
diff
changeset
|
479 positions = [f.getPositionAtInstant(instant) for f in features] |
32e88b513f5c
added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
266
diff
changeset
|
480 velocities = [f.getVelocityAtInstant(instant) for f in features] |
268
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
481 else: |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
482 positions = [obj.getPositionAtInstant(instant)] |
0c0b92f621f6
reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
267
diff
changeset
|
483 velocities = [obj.getVelocityAtInstant(instant)] |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
484 for i in xrange(self.nPredictedTrajectories): |
267
32e88b513f5c
added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
266
diff
changeset
|
485 for initialPosition,initialVelocity in zip(positions, velocities): |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
486 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, |
350
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
487 initialVelocity, |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
488 moving.NormAngle(self.accelerationDistribution(), |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
489 self.steeringDistribution()), |
7e9ad2d9d79c
added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
336
diff
changeset
|
490 maxSpeed = self.maxSpeed)) |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
491 return predictedTrajectories |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
492 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
493 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
494 class CVDirectPredictionParameters(PredictionParameters): |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
495 '''Prediction parameters of prediction at constant velocity |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
496 using direct computation of the intersecting point''' |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
497 |
387
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
498 def __init__(self): |
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
499 PredictionParameters.__init__(self, 'constant velocity (direct computation)', None) |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
500 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
501 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
502 collisionPoints = [] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
503 crossingZones = [] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
504 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
505 p1 = obj1.getPositionAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
506 p2 = obj2.getPositionAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
507 if (p1-p2).norm2() <= collisionDistanceThreshold: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
508 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)] |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
509 else: |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
510 v1 = obj1.getVelocityAtInstant(currentInstant) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
511 v2 = obj2.getVelocityAtInstant(currentInstant) |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
512 intersection = moving.intersection(p1, p1+v1, p2, p2+v2) |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
513 |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
514 if intersection is not None: |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
515 dp1 = intersection-p1 |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
516 dp2 = intersection-p2 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
517 dot1 = moving.Point.dot(dp1, v1) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
518 dot2 = moving.Point.dot(dp2, v2) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
519 #print dot1, dot2 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
520 # (computeCZ and (dot1 > 0 or dot2 > 0)) or ( |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
521 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 |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
522 dist1 = dp1.norm2() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
523 dist2 = dp2.norm2() |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
524 s1 = math.copysign(v1.norm2(), dot1) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
525 s2 = math.copysign(v2.norm2(), dot2) |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
526 halfCollisionDistanceThreshold = collisionDistanceThreshold/2. |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
527 timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
528 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
529 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
530 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
531 if collisionTimeInterval.empty(): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
532 if computeCZ: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] |
387
91679eb2ff2c
cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
359
diff
changeset
|
534 else: |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
536 # elif computeCZ and (dot1 > 0 or dot2 > 0): |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
537 # if dot1 > 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
538 # firstUser = obj2 # first through crossingzone |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
539 # secondUser = obj1 # second through crossingzone |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
540 # elif dot2 > 0: |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
541 # firstUser = obj1 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
542 # secondUser = obj2 |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
543 # p2 = secondUser.getPositionAtInstant(currentInstant) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
544 # v2 = secondUser.getVelocityAtInstant(currentInstant) |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
545 # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2) |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
546 # if indices is not None: |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
547 # pass |
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
548 # else: # one has to predict !!! |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
549 |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
550 if debug and intersection is not None: |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
551 from matplotlib.pyplot import plot, figure, axis, title |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
552 figure() |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
553 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
554 plot([p2.x, intersection.x], [p2.y, intersection.y], 'b') |
515
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
489
diff
changeset
|
555 intersection.plot() |
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
489
diff
changeset
|
556 obj1.plot('r') |
727e3c529519
renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
489
diff
changeset
|
557 obj2.plot('b') |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
558 title('instant {0}'.format(currentInstant)) |
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
559 axis('equal') |
289
e56c34c1ebac
refactored and commented functions (saving data is now outside of the computation functions)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
271
diff
changeset
|
560 |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
561 return currentInstant, collisionPoints, crossingZones |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
562 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
563 class CVExactPredictionParameters(PredictionParameters): |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
564 '''Prediction parameters of prediction at constant velocity |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
565 using direct computation of the intersecting point (solving for the equation''' |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
566 |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
567 def __init__(self): |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
568 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None) |
359
619ae9a9a788
implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
358
diff
changeset
|
569 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
570 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
571 'TODO add collision point coordinates, compute pPET' |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
572 #collisionPoints = [] |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
573 #crossingZones = [] |
269
a9988971aac8
removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
268
diff
changeset
|
574 |
464
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
575 p1 = obj1.getPositionAtInstant(currentInstant) |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
576 p2 = obj2.getPositionAtInstant(currentInstant) |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
577 v1 = obj1.getVelocityAtInstant(currentInstant) |
dcc821b98efc
integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
460
diff
changeset
|
578 v2 = obj2.getVelocityAtInstant(currentInstant) |
630
69a98f84f3eb
corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
629
diff
changeset
|
579 intersection = moving.intersection(p1, p1+v1, p2, p2+v2) |
484
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
580 |
636
3058e00887bc
removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
630
diff
changeset
|
581 if intersection is not None: |
484
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
582 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) |
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
583 if ttc: |
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
584 return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5) |
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
585 else: |
6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
470
diff
changeset
|
586 return [],[] |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
587 |
357
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
588 #### |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
589 # Other Methods |
e5fe0e6d48a1
corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
355
diff
changeset
|
590 #### |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
591 class PrototypePredictionParameters(PredictionParameters): |
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
592 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True): |
607 | 593 name = 'prototype' |
594 PredictionParameters.__init__(self, name, maxSpeed) | |
595 self.nPredictedTrajectories = nPredictedTrajectories | |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
596 self.constantSpeed = constantSpeed |
607 | 597 |
598 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): | |
599 predictedTrajectories = [] | |
600 initialPosition = obj.getPositionAtInstant(instant) | |
601 initialVelocity = obj.getVelocityAtInstant(instant) | |
602 for prototypeTraj in prototypeTrajectories.keys(): | |
619
dc2d0a0d7fe1
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
613
diff
changeset
|
603 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) |
607 | 604 return predictedTrajectories |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
605 |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
606 if __name__ == "__main__": |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
607 import doctest |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
608 import unittest |
271
bbd9c09e6869
changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
270
diff
changeset
|
609 suite = doctest.DocFileSuite('tests/prediction.txt') |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
610 #suite = doctest.DocTestSuite() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
611 unittest.TextTestRunner().run(suite) |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
612 #doctest.testmod() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
613 #doctest.testfile("example.txt") |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
614 |