Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 619:dc2d0a0d7fe1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 10 Dec 2014 15:27:08 -0500
parents 306db0f3c7a2
children 0a5e89d6fc62
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
5 import math
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
6 import random
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
7 import numpy as np
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
8 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
9
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
10 class PredictedTrajectory:
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
11 '''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
12 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
13
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
14 it should also have a probability'''
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
15
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
16 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
17 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
18 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
19 self.predictedSpeedOrientations = {}
300
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
20 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
21 #self.crossingZones = {}
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
22
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
23 def predictPosition(self, nTimeSteps):
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
24 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
25 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
26 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
27 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
28
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
29 def getPredictedTrajectory(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
30 return moving.Trajectory.fromPointList(self.predictedPositions.values())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
31
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
32 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
33 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
34
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
35 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
36 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
37
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
38 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
39 '''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
40 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
41
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
42 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
43 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
44 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
45 self.probability = probability
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
46 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
47 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
48
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
49 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
50 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
51
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
52 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
53 ''' 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
54 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
55 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
56 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
57 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
58 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
59
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
60 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
61 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
62 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
63 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
64 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
65
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
66 Prediction can be done
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
67 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
68 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
69 (applying a constant ratio equal
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
70 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
71
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
72 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
73 self.prototypeTrajectory = prototypeTrajectory
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
74 self.constantSpeed = constantSpeed
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
75 self.probability = probability
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
76 self.predictedPositions = {0: initialPosition}
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
77 self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
78
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
79 def predictPosition(self, nTimeSteps):
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
80 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
81 if self.constantSpeed:
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
82 # calculate cumulative distance
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
83 speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
84 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
85 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
86 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
87
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
88 else: # see c++ code, calculate ratio
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
89 speedNorm= self.predictedSpeedOrientations[0].norm
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
90 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
91 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
92 ratio=float(speedNorm)/prototypeSpeeds[0]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
93 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
94 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
95 if nTimeSteps<len(resampledSpeeds):
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
96 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
97 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
98 else:
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
99 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
100 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
101
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
102 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
103
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
104 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
105 '''Random vehicle control: suitable for normal adaptation'''
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
106 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
107 '''Constructor
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
108 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
109 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
110 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
111 self.steeringDistribution = steeringDistribution
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
112 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
113 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
114 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
115 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
116
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
117 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
118 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
119
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
120 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
121 '''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
122 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
123 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
124 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
125 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
126 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
127 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
128
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
129 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
130 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
131
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
132 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
133 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
134 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
135 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
136
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
137 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
138 def computeExpectedIndicator(points):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
139 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
140 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
141
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
142 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
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 '''Computes the first instant at which two predicted trajectories are within some distance threshold'''
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
144 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
145 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
146 p2 = predictedTrajectory2.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
147 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
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
148 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
149 p2 = predictedTrajectory2.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
150 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
151 return t, p1, p2
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
152
557
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
153 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
154 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
155 figure()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
156 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
157 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
158 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
159
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
160 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
161 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
162 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
163 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
164 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
165 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
166 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
167 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
168 close()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
169
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
170 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
171 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
172 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
173 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
174 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
175 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
176 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
177 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
178 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
179 return probabilities
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
180
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
181 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
182 ''' 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
183 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
184 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
185 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
186 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
187 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
188 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
189 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
190 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
191 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
192 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
193 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
194 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
195 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
196 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
197 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
198
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 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
200 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
201 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
202 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
203 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
204 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
205 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
206 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
207 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
208 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
209 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
210
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
211 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
212 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
213 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
214 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
215 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
216 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
217 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
218 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
219 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
220 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
221 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
222 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
223 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
224 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
225 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
226 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
227 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
228 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
229 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
230
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 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
232 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
233 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
234 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
235 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
236 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
237 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
238
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
239 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
240 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
241 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
242 if useSpeedPrototype:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
243 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
244 else:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
245 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
246 return prototypeTrajectories
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
247
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
248 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,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
249 '''returns the lists of collision points and crossing zones'''
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
250 if usePrototypes:
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
251 prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
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
252 prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
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 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
254 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
255 else:
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
256 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
257 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
258
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
259 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
260 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
261 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
262 for et2 in predictedTrajectories2:
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
263 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, 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
264
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 if t <= 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
266 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
267 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
268 # 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
269 # 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
270 # 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
271 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
272 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
273 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
274 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
275 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
276 #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
277 # 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
278 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
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
279 if cz != None:
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
280 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
281 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
282 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
283 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
284
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 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
286 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
287
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
288 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
289
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
290 def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
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
291 '''Computes all crossing and collision points at each common instant for two road users. '''
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 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
293 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
294 if timeInterval:
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
295 commonTimeInterval = timeInterval
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 else:
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 commonTimeInterval = obj1.commonTimeInterval(obj2)
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
298 if nProcesses == 1:
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
299 if usePrototypes:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
300 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
611
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
301 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
302 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
303 for i in commonTimeIntervalList2:
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
304 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
305 if len(cp) != 0:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
306 collisionPoints[i] = cp
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
307 if len(cz) != 0:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
308 crossingZones[i] = cz
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
309 if collisionPoints!={} or crossingZones!={}:
611
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
310 for i in commonTimeIntervalList1:
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
311 if i not in commonTimeIntervalList2:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
312 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
313 if len(cp) != 0:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
314 collisionPoints[i] = cp
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
315 if len(cz) != 0:
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
316 crossingZones[i] = cz
611
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
317 else:
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
318 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
319 i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
320 if len(cp) != 0:
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
321 collisionPoints[i] = cp
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
322 if len(cz) != 0:
233867934190 corrected bug
MohamedGomaa
parents: 610
diff changeset
323 crossingZones[i] = cz
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
324 else:
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
325 from multiprocessing import Pool
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
326 pool = Pool(processes = nProcesses)
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
327 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
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
328 #results = [j.get() for j in jobs]
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
329 #results.sort()
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
330 for j in jobs:
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
331 i, cp, cz = j.get()
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
332 #if len(cp) != 0 or len(cz) != 0:
559
806df5f61c03 adapted safety-analysis script to use multi-threading
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 557
diff changeset
333 if len(cp) != 0:
806df5f61c03 adapted safety-analysis script to use multi-threading
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 557
diff changeset
334 collisionPoints[i] = cp
806df5f61c03 adapted safety-analysis script to use multi-threading
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 557
diff changeset
335 if len(cz) != 0:
806df5f61c03 adapted safety-analysis script to use multi-threading
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 557
diff changeset
336 crossingZones[i] = cz
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
337 pool.close()
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
338 return 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
339
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
340 class PredictionParameters:
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
341 def __init__(self, name, maxSpeed):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
342 self.name = name
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
343 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
344
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
345 def __str__(self):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
346 return '{0} {1}'.format(self.name, self.maxSpeed)
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
347
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
348 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
349 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
350
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
351 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),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
352 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
353
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
354 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
355 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
356
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
357 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
358 '''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
359 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
360 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
361 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
362 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
363 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
364 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
365 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
366 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
367 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
368 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
369 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
370 for et2 in 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
371 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
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 if t <= timeHorizon:
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 += 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
374 # 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
375 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
376 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
377
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
378 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
379 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
380
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 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
382
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
383 class ConstantPredictionParameters(PredictionParameters):
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
384 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
385 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
386
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
387 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
388 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
389 maxSpeed = self.maxSpeed)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
390
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
391 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
392 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
393 '''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
394 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
395 '''
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
396 if useFeatures:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
397 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
398 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
399 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
400 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
401 self.nPredictedTrajectories = nPredictedTrajectories
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
402 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
403 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
404 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
405
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
406 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
407 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
408 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
409 self.maxSteering)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
410
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
411 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
412 predictedTrajectories = []
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
413 if self.useFeatures:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
414 features = [f for f in obj.features if f.existsAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
415 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
416 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
417 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
418 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
419 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
420 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
421 for initialPosition,initialVelocity in zip(positions, velocities):
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
422 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
423 initialVelocity,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
424 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
425 self.steeringDistribution,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
426 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
427 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
428
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
429 class PointSetPredictionParameters(PredictionParameters):
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
430 # 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
431 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
432 PredictionParameters.__init__(self, 'point set', maxSpeed)
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
433 #self.nPredictedTrajectories = nPredictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
434
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
435 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
436 predictedTrajectories = []
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
437 features = [f for f in obj.features if f.existsAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
438 positions = [f.getPositionAtInstant(instant) for f in features]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
439 velocities = [f.getVelocityAtInstant(instant) for f in features]
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
440 #for i in xrange(self.nPredictedTrajectories):
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
441 for initialPosition,initialVelocity in zip(positions, velocities):
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
442 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity,
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
443 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
444 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
445
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
446 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
447 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
448 '''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
449 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
450
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
451 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
452 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
453 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
454 name = 'evasive action'
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
455 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
456 self.nPredictedTrajectories = nPredictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
457 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
458 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
459 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
460
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
461 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
462 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
463
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
464 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
465 predictedTrajectories = []
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
466 if self.useFeatures:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
467 features = [f for f in obj.features if f.existsAtInstant(instant)]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
468 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
469 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
470 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
471 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
472 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
473 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
474 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
475 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
476 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
477 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
478 self.steeringDistribution()),
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
479 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
480 return predictedTrajectories
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
481
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
482
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
483 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
484 '''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
485 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
486
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
487 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
488 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
489
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
490 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
491 collisionPoints = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
492 crossingZones = []
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 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
495 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
496 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
497 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
498 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
499 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
500 v2 = obj2.getVelocityAtInstant(currentInstant)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
501 intersection = moving.intersection(p1, p2, v1, v2)
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
502
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
503 if intersection != None:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
504 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
505 dp2 = intersection-p2
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
506 if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
507 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
508 dist2 = dp2.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
509 s1 = v1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
510 s2 = v2.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
511 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
512 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
513 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
514 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
515 if computeCZ and collisionTimeInterval.empty():
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
516 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
517 else:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
518 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
519
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
520 if debug and intersection!= None:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
521 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
522 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
523 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
524 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
525 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
526 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
527 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
528 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
529 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
530
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
531 return collisionPoints, crossingZones
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
532
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
533 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
534 '''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
535 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
536
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
537 def __init__(self):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
538 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
539
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
540 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
541 '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
542 #collisionPoints = []
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
543 #crossingZones = []
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
544
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
545 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
546 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
547 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
548 v2 = obj2.getVelocityAtInstant(currentInstant)
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
549 intersection = moving.intersection(p1, p2, v1, v2)
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
550
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
551 if intersection != None:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
552 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
553 if ttc:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
554 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
555 else:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
556 return [],[]
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
557
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
558 ####
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
559 # 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
560 ####
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
561 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
562 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
563 name = 'prototype'
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
564 PredictionParameters.__init__(self, name, maxSpeed)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
565 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
566 self.constantSpeed = constantSpeed
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
567
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
568 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
569 predictedTrajectories = []
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
570 initialPosition = obj.getPositionAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
571 initialVelocity = obj.getVelocityAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
572 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
573 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj]))
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
574 return predictedTrajectories
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
575
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
576 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
577 import doctest
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
578 import unittest
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
579 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
580 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
581 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
582 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
583 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
584