Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 944:84ebe1b031f1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
works with object trajectory, features todo
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 20 Jul 2017 12:12:28 -0400
parents b1e8453c207c
children 05d4302bf67e
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
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
5 from utils import LCSS
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
6
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
7 import math, random
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
8 import numpy as np
696
ae137e3b1990 minor correction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
9 from multiprocessing import Pool
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
10
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
11
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
12 class PredictedTrajectory(object):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
13 '''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
14 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
15
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
16 it should also have a probability'''
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
17
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
18 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
19 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
20 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
21 self.predictedSpeedOrientations = {}
300
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
22 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 296
diff changeset
23 #self.crossingZones = {}
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
24
244
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
25 def predictPosition(self, nTimeSteps):
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
26 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
27 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
28 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
29 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
30
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
31 def getPredictedTrajectory(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
32 return moving.Trajectory.fromPointList(self.predictedPositions.values())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
33
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
34 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
35 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
36
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
37 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
38 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
39
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
40 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
41 '''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
42 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
43
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
44 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
45 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
46 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
47 self.probability = probability
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
48 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
49 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
50
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
51 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
52 return self.control
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
53
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
54 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
55 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
56 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
57 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
58 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
59
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
60 Prediction can be done
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
61 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
62 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
63 (applying a constant ratio equal
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
64 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
65
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
66 def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, probability = 1.):
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
67 ''' prototype is a MovingObject
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
68
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
69 Prediction at constant speed will not work for unrealistic trajectories
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
70 that do not follow a slowly changing velocity (eg moving object trajectories,
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
71 but is good for realistic motion (eg features)'''
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
72 self.prototype = prototype
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
73 self.constantSpeed = constantSpeed
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
74 self.probability = probability
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
75 self.predictedPositions = {0: initialPosition}
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
76 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
77 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx)
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
78 self.initialSpeed = initialVelocity.norm2()
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
79 if not constantSpeed:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
81
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
82 def predictPosition(self, nTimeSteps):
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
84 if self.constantSpeed:
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
85 traj = self.prototype.getPositions()
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
86 trajLength = traj.length()
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
87 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
88 i = self.closestPointIdx
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
89 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
90 i += 1
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
91 if i == trajLength:
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
92 v = self.prototype.getVelocityAt(-1)
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
93 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
94 else:
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
95 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
96 else:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
97 traj = self.prototype.getPositions()
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
98 trajLength = traj.length()
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
99 nSteps = self.ratio*nTimeSteps+self.closestPointIdx
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
100 i = int(np.floor(nSteps))
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
101 if nSteps < trajLength-1:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
102 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
103 else:
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
104 v = self.prototype.getVelocityAt(-1)
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
105 self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1)
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
106 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
107
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
108 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
109 '''Random vehicle control: suitable for normal adaptation'''
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
110 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
111 '''Constructor
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
112 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
113 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
114 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
115 self.steeringDistribution = steeringDistribution
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
116 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
117 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
118 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
119 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
120
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
121 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
122 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
123
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
124 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
125 '''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
126 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
127 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
128 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
129 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
130 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
131 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
132
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
133 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
134 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
135
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
136 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
137 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
138 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
139 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
140
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
141 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
142 def computeExpectedIndicator(points):
696
ae137e3b1990 minor correction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
143 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
144
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
145 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
662
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
146 '''Computes the first instant
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
147 at which two predicted trajectories are within some distance threshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
148 Computes all the times including timeHorizon
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
149
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
150 User has to check the first variable collision to know about a collision'''
358
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
151 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
152 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
153 p2 = predictedTrajectory2.predictPosition(t)
662
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
154 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
155 while t < timeHorizon and not collision:
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
156 t += 1
358
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
157 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
158 p2 = predictedTrajectory2.predictPosition(t)
662
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
159 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
160 return collision, t, p1, p2
358
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
161
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
162 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
163 from matplotlib.pyplot import figure, axis, title, clf, savefig
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
164 if printFigure:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
165 clf()
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
166 else:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
167 figure()
557
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
168 for et in predictedTrajectories1:
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
169 for t in xrange(int(np.round(timeHorizon))):
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
170 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
171 et.plot('rx')
557
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
172 for et in predictedTrajectories2:
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
173 for t in xrange(int(np.round(timeHorizon))):
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
174 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
175 et.plot('bx')
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
176 obj1.plot('r', withOrigin = True)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
177 obj2.plot('b', withOrigin = True)
557
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
178 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
179 axis('equal')
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
180 if printFigure:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
181 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
557
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 556
diff changeset
182
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
183 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
184 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
185 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
186 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
187 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
188 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
189 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
190 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
191 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
192 return probabilities
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
193
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
194 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
195 ''' 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
196 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
197 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
198 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
199 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
200 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
201 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
202 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
203 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
204 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
205 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
206 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
207 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
208 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
209 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
210 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
211
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 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
213 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
214 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
215 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
216 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
217 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
218 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
219 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
220 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
221 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
222 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
223
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
224 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
225 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
226 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
227 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
228 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
229 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
230 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
231 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
232 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
233 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
234 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
235 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
236 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
237 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
238 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
239 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
240 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
241 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
242 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
243
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
244 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
245 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
246 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
247 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
248 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
249 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
250 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
251
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
252 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
253 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
254 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
255 if useSpeedPrototype:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
256 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
257 else:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
258 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
259 return prototypeTrajectories
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
260
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
261 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
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
262 '''returns the lists of collision points and crossing zones'''
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
556
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
265
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
266 collisionPoints = []
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
267 crossingZones = []
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
268 for et1 in predictedTrajectories1:
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
269 for et2 in predictedTrajectories2:
662
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
270 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
271 if collision:
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
272 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
556
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
273 elif computeCZ: # check if there is a crossing zone
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
274 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
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
275 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
276 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
277 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
278 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
279 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
280 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
281 if cz is not None:
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
282 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
283 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
284 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
285 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
286
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
287 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
288 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
289
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
290 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
291
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 515
diff changeset
292
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
293 class PredictionParameters(object):
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
294 def __init__(self, name, maxSpeed):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
295 self.name = name
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
296 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
297
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
298 def __str__(self):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
299 return '{0} {1}'.format(self.name, self.maxSpeed)
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
300
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
301 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
302 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
303
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
304 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
305 '''Computes all crossing and collision points at each common instant for two road users. '''
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
306 collisionPoints={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
307 crossingZones={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
308 if timeInterval:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
309 commonTimeInterval = timeInterval
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
310 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
311 commonTimeInterval = obj1.commonTimeInterval(obj2)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
312 if nProcesses == 1:
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
313 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
314 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
315 if len(cp) != 0:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
316 collisionPoints[i] = cp
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
317 if len(cz) != 0:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
318 crossingZones[i] = cz
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
319 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
320 pool = Pool(processes = nProcesses)
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
321 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
322 for j in jobs:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
323 i, cp, cz = j.get()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
324 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
325 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
326 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
327 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
328 pool.close()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
329 return collisionPoints, crossingZones
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
330
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
331 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
332 '''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
333 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
334 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
335 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
336 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
337 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
338 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
339 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
340 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
341 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
342 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
343 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
344 for et2 in predictedTrajectories2:
662
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
345 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 661
diff changeset
346 if collision:
358
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
347 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
348 # 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
349 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
350 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
351
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
352 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
353 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
354
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
355 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
356
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
357 class ConstantPredictionParameters(PredictionParameters):
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
358 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
359 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
360
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
361 def generatePredictedTrajectories(self, obj, instant):
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
362 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), maxSpeed = self.maxSpeed)]
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
363
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
364 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
365 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
366 '''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
367 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
368 '''
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
369 if useFeatures:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
370 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
371 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
372 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
373 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
374 self.nPredictedTrajectories = nPredictedTrajectories
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
375 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
376 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
377 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
378
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
379 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
380 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
381 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
382 self.maxSteering)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
383
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
384 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
385 predictedTrajectories = []
860
07c5eab11eba fixing bug, thanks to Ryan Louie <Ryan.Louie@students.olin.edu>
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 826
diff changeset
386 if self.useFeatures and obj.hasFeatures():
661
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
387 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
388 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
389 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
390 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
391 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
392 velocities = [obj.getVelocityAtInstant(instant)]
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
393 probability = 1./float(len(positions)*self.nPredictedTrajectories)
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
394 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
395 for initialPosition,initialVelocity in zip(positions, velocities):
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
396 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
397 initialVelocity,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
398 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
399 self.steeringDistribution,
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
400 probability,
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
401 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
402 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
403
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
404 class PointSetPredictionParameters(PredictionParameters):
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
405 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
406 PredictionParameters.__init__(self, 'point set', maxSpeed)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
407
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
408 def generatePredictedTrajectories(self, obj, instant):
661
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
409 predictedTrajectories = []
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
410 if obj.hasFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
411 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
412 positions = [f.getPositionAtInstant(instant) for f in features]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
413 velocities = [f.getVelocityAtInstant(instant) for f in features]
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
414 probability = 1./float(len(positions))
661
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
415 for initialPosition,initialVelocity in zip(positions, velocities):
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
416 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, probability = probability, maxSpeed = self.maxSpeed))
661
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
417 return predictedTrajectories
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
418 else:
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
419 print('Object {} has no features'.format(obj.getNum()))
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
420 return None
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
421
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
422
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
423 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
424 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
425 '''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
426 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
427
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
428 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
429 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
430 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
431 name = 'evasive action'
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, name, maxSpeed)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
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 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
435 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
436 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
437
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
438 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
439 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
440
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
441 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
442 predictedTrajectories = []
661
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
443 if self.useFeatures and obj.hasFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 651
diff changeset
444 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
445 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
446 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
447 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
448 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
449 velocities = [obj.getVelocityAtInstant(instant)]
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
450 probability = 1./float(self.nPredictedTrajectories)
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
451 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
452 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
453 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
454 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
455 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
456 self.steeringDistribution()),
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
457 probability,
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
458 self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
459 return predictedTrajectories
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
460
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
461
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
462 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
463 '''Prediction parameters of prediction at constant velocity
698
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
464 using direct computation of the intersecting point
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
465 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
466
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
467 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
468 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
469
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
470 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
471 collisionPoints = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
472 crossingZones = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
473
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
474 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
475 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
476 if (p1-p2).norm2() <= collisionDistanceThreshold:
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
477 collisionPoints = [SafetyPoint((p1+p1)*0.5, 1., 0.)]
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
478 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
479 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
480 v2 = obj2.getVelocityAtInstant(currentInstant)
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
481 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
482
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
483 if intersection is not None:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
484 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
485 dp2 = intersection-p2
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
486 dot1 = moving.Point.dot(dp1, v1)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
487 dot2 = moving.Point.dot(dp2, v2)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
488 if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
489 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
490 dist2 = dp2.norm2()
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
491 s1 = math.copysign(v1.norm2(), dot1)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
492 s2 = math.copysign(v2.norm2(), dot2)
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
493 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
494 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
495 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
496 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
497
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
498 if collisionTimeInterval.empty():
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
499 if computeCZ:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
500 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
501 else:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
502 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
503
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
504 if debug and intersection is not None:
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
505 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
506 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
507 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
508 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
509 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
510 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
511 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
512 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
513 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
514
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
515 return currentInstant, collisionPoints, crossingZones
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
516
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
517 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
518 '''Prediction parameters of prediction at constant velocity
698
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
519 using direct computation of the intersecting point (solving the equation)
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
520 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
521
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
522 def __init__(self):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
523 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
524
701
4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 698
diff changeset
525 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
526 'TODO compute pPET'
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
527 collisionPoints = []
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
528 crossingZones = []
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
529
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
530 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
531 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
532 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
533 v2 = obj2.getVelocityAtInstant(currentInstant)
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
534 intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
535
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
536 if intersection is not None:
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
537 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
538 if ttc is not None:
888
40994bb43148 corrected bug
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 881
diff changeset
539 collisionPoints = [SafetyPoint(intersection, 1., ttc)]
484
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 470
diff changeset
540 else:
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
541 pass # compute pPET
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
542
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
543 return currentInstant, collisionPoints, crossingZones
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
544
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
545 class PrototypePredictionParameters(PredictionParameters):
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
546 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
547 PredictionParameters.__init__(self, 'prototypes', None)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
548 self.prototypes = prototypes
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
549 self.nPredictedTrajectories = nPredictedTrajectories
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
550 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
551 self.minSimilarity = minSimilarity
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
552 self.minFeatureTime = minFeatureTime
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
553 self.constantSpeed = constantSpeed
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
554 self.useFeatures = useFeatures
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
555
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
556 def generatePredictedTrajectories(self, obj, instant):
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
557 predictedTrajectories = []
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
558 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
559 if self.useFeatures and obj.hasFeatures():
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
560 # get current features existing for the most time, sort on first instant of feature and take n first
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
561 pass
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
562 else:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
563 if not hasattr(obj, 'prototypeSimilarities'):
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
564 obj.prototypeSimilarities = []
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
565 for proto in self.prototypes:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
566 self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
567 similarities = self.lcss.similarityTable[-1, :-1].astype(float)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
568 obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities))))
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
569 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
570 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
571 initialPosition = obj.getPositionAtInstant(instant)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
572 initialVelocity = obj.getVelocityAtInstant(instant)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
573 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
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