Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/prediction.py @ 1278:8e61ff3cd503 default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
correct bug to take into account first frame num in config, and other related bugs in dltrack.py
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 27 Jun 2024 15:31:36 -0400
parents ca70a79688ae
children
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
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
4 import math, random
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
5 from copy import copy
1029
c6cf75a2ed08 reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1028
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 numpy as np
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
8
1029
c6cf75a2ed08 reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1028
diff changeset
9 from trafficintelligence import moving
c6cf75a2ed08 reorganization of imports
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1028
diff changeset
10 from trafficintelligence.utils import LCSS
665
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):
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
26 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
256
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):
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
32 return moving.Trajectory.fromPointList(list(self.predictedPositions.values()))
256
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
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
37 def hasCollisionWith(self, other, t, collisionDistanceThreshold):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
38 p1 = self.predictPosition(t)
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
39 p2 = other.predictPosition(t)
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
40 return (p1-p2).norm2() <= collisionDistanceThreshold, p1, p2
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
41
515
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
42 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
43 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
44
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
45 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
46 '''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
47 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
48
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
49 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
50 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
51 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
52 self.probability = probability
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
53 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
55
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
56 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
57 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
58
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
59 class PredictedTrajectoryConstantVelocity(PredictedTrajectory):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
60 '''Predicted trajectory at constant speed or acceleration
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
61 TODO generalize by passing a series of velocities/accelerations'''
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
62
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
63 def __init__(self, initialPosition, initialVelocity, probability = 1.):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
64 self.probability = probability
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
65 self.predictedPositions = {0: initialPosition}
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
66 self.initialVelocity = initialVelocity
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
67 #self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
68
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
69 def predictPosition(self, nTimeSteps):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
70 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
71 self.predictPosition(nTimeSteps-1)
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
72 self.predictedPositions[nTimeSteps] = self.predictedPositions[nTimeSteps-1]+self.initialVelocity
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
73 return self.predictedPositions[nTimeSteps]
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
74
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
75 class PredictedPolyTrajectoryConstantVelocity(PredictedTrajectory):
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
76 '''Predicted trajectory of an object with an outline represented by a polygon
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
77 Simple method that just translates the polygon corners (set of moving.Point)'''
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
78 def __init__(self, polyCorners, initialVelocity, probability = 1.):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
79 self.probability = probability
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
80 self.predictedPositions = {0: moving.pointsToShapely(polyCorners)}
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
81 self.initialVelocity = initialVelocity
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
82 self.predictedTrajectories = [PredictedTrajectoryConstantVelocity(p, initialVelocity) for p in polyCorners]
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
83
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
84 def predictPosition(self, nTimeSteps):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
85 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
86 self.predictedPositions[nTimeSteps] = moving.pointsToShapely([t.predictPosition(nTimeSteps) for t in self.predictedTrajectories])
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
87 return self.predictedPositions[nTimeSteps]
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
88
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
89 def hasCollisionWith(self, other, t, collisionDistanceThreshold):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
90 poly1 = self.predictPosition(t)
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
91 poly2 = other.predictPosition(t)
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
92 return poly1.overlaps(poly2), poly1, poly2
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
93
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
94 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
95 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
96 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
97 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
98 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
99
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
100 Prediction can be done
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
101 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
102 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
103 (applying a constant ratio equal
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
104 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
105
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
106 def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, nFramesIgnore = 3, probability = 1.):
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
107 ''' 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
108
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
109 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
110 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
111 but is good for realistic motion (eg features)'''
1154
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
112 self.valid = True
940
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 939
diff changeset
113 self.prototype = prototype
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
114 self.constantSpeed = constantSpeed
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
115 self.nFramesIgnore = nFramesIgnore
467
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
116 self.probability = probability
08b67c9baca2 finished description and constructor
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 466
diff changeset
117 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
118 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
119 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) #should be computed in relative coordinates to position
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
120 self.theta = prototype.getVelocityAt(self.closestPointIdx).angle()
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
121 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
122 if not constantSpeed:
1154
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
123 while prototype.getVelocityAt(self.closestPointIdx).norm2() == 0. and self.closestPointIdx < prototype.length():
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
124 self.closestPointIdx += 1
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
125 if self.closestPointIdx < prototype.length():
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
126 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
127 else:
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
128 self.valid = False
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
129
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
130 def predictPosition(self, nTimeSteps):
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
131 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
132 deltaPosition = copy(self.deltaPosition)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
133 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
134 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
135 trajLength = traj.length()
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
136 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
137 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
138 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
139 i += 1
942
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
140 if i == trajLength:
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
141 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
142 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
939
a2f3f3ca241e work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 937
diff changeset
143 else:
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
144 v = self.prototype.getVelocityAt(min(i-1, int(self.prototype.length())-1-self.nFramesIgnore))
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
145 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+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
146 else:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
147 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
148 trajLength = traj.length()
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 940
diff changeset
149 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
150 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
151 if nSteps < trajLength-1:
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
152 v = self.prototype.getVelocityAt(min(i, int(self.prototype.length())-1-self.nFramesIgnore))
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
153 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
154 else:
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
155 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
156 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[-1]+v*(nSteps-trajLength+1)
470
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
157 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 467
diff changeset
158
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
159 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
160 '''Random vehicle control: suitable for normal adaptation'''
336
124f85c6cfae modifed default probability to float
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 300
diff changeset
161 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
162 '''Constructor
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
163 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
164 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
165 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
166 self.steeringDistribution = steeringDistribution
250
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
167 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
168 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
169 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 245
diff changeset
170 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
171
256
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
172 def getControl(self):
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
173 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 255
diff changeset
174
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
175 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
176 '''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
177 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
178 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
179 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
180 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
181 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
182 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
183
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
184 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
185 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
186
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
187 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
188 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
189 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
190 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
191
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
192 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
193 def computeExpectedIndicator(points):
696
ae137e3b1990 minor correction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
194 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
195
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
196 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
197 '''Computes the first instant
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
198 at which two predicted trajectories are colliding (depending on the trajectory format)
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
199 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
200
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
201 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
202 t = 1
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
203 collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
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
204 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
205 t += 1
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
206 collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
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
207 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
208
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
209 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
210 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
211 if printFigure:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
212 clf()
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
213 else:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
214 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
215 for et in predictedTrajectories1:
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
216 for t in range(int(np.round(timeHorizon))):
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
217 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
218 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
219 for et in predictedTrajectories2:
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
220 for t in range(int(np.round(timeHorizon))):
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
221 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
222 et.plot('bx')
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
223 obj1.plot('r', withOrigin = True)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
224 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
225 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
226 axis('equal')
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
227 if printFigure:
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
228 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
229
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
230 def calculateProbability(nMatching,similarity,objects):
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
231 sumFrequencies=sum([nMatching[p] for p in similarity])
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
232 prototypeProbability={}
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
233 for i in similarity:
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
234 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
235 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability])
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
236 probabilities={}
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
237 for i in prototypeProbability:
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
238 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
239 return probabilities
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
240
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
241 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
242 ''' 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
243 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
244 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
245 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
246 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
247 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
248 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
249 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
250 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
251 for y in prototypesRoutes:
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
252 if y in prototypes:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
253 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
254 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
255 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
256 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
257 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
258
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 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
260 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
261 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
262 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
263 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
264 keys=[k for k in similarity if similarity[k] in mostMatchedValues]
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
265 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
266 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
267 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
268 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
269 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
270
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
271 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
272 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
273 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
274 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
275 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
276 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
277 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
278 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
279 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
280 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
281 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
282 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
283 for y in prototypesRoutes:
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
284 if y in prototypes:
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
285 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
286 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
287 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
288 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
289 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
290
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
291 newSimilarity={}
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
292 for i in similarity:
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
293 if i in secondStepPrototypes:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
294 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
295 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
296 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
297 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
298
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
299 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
300 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
301 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
302 if useSpeedPrototype:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
303 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
304 else:
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 611
diff changeset
305 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
306 return prototypeTrajectories
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
307
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
308
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
309 class PredictionParameters(object):
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
310 def __init__(self, name, maxSpeed = None, useCurvilinear = False):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
311 self.name = name
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
312 self.maxSpeed = maxSpeed
1104
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
313 self.useCurvilinear = useCurvilinear
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
314
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
315 def __str__(self):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
316 return '{0} {1}'.format(self.name, self.maxSpeed)
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
317
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
318 def generatePredictedTrajectories(self, obj, instant):
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
319 return None
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
320
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
321 def computeCollisionPoint(self, p1, p2, probability, t):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
322 return SafetyPoint((p1+p2)*0.5, probability, t)
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
323
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
324 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
325 '''returns the lists of collision points and crossing zones'''
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
326 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
327 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant)
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
328
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
329 collisionPoints = []
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
330 if computeCZ:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
331 crossingZones = []
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
332 else:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
333 crossingZones = None
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
334 for et1 in predictedTrajectories1:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
335 for et2 in predictedTrajectories2:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
336 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
337 if collision:
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
338 collisionPoints.append(self.computeCollisionPoint(p1, p2, et1.probability*et2.probability, t))
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
339 elif computeCZ: # check if there is a crossing zone
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
340 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
341 cz = None
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
342 t1 = 0
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
343 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
344 t2 = 0
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
345 while not cz and t2 < timeHorizon:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
346 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
347 if cz is not None:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
348 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
349 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
350 t2 += 1
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
351 t1 += 1
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
352
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
353 if debug:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
354 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
355
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
356 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
357
1269
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
358 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, speedThreshold = 0.):
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
359 '''Computes all crossing and collision points at each common instant for two road users.
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
360 No movement prediction below a certain speedThreshold for both objects'''
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
361 collisionPoints = {}
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
362 if computeCZ:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
363 crossingZones = {}
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
364 else:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
365 crossingZones = None
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
366 if timeInterval is not None:
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
367 commonTimeInterval = timeInterval
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
368 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
369 commonTimeInterval = obj1.commonTimeInterval(obj2)
1269
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
370 speedThreshold2 = max(0,speedThreshold)**2
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
371 for t in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
372 if obj1.getVelocityAtInstant(t).norm2Squared() > speedThreshold2 and obj2.getVelocityAtInstant(t).norm2Squared() > speedThreshold2:
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
373 cp, cz = self.computeCrossingsCollisionsAtInstant(t, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
374 if len(cp) != 0:
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
375 collisionPoints[t] = cp
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
376 if computeCZ and len(cz) != 0:
ca70a79688ae adding a speed threshold to avoid computing TTC at very low speeds
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1215
diff changeset
377 crossingZones[t] = cz
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
378 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
379
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
380 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
381 '''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
382 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
383 collisionProbabilities = {}
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
384 if timeInterval is not None:
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
385 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
386 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
387 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
388 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
389 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
390 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
391 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
392 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
393 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
394 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
395 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
396 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
397 # 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
398 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
399 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
400
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
401 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
402 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
403
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
404 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
405
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
406 class ConstantPredictionParameters(PredictionParameters):
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
407 def __init__(self):
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
408 PredictionParameters.__init__(self, 'constant velocity')
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
409
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
410 def generatePredictedTrajectories(self, obj, instant):
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
411 return [PredictedTrajectoryConstantVelocity(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant))]
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
412
1212
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
413 class ConstantPolyPredictionParameters(PredictionParameters):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
414 def __init__(self):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
415 PredictionParameters.__init__(self, 'constant velocity for polygon representation')
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
416
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
417 def generatePredictedTrajectories(self, obj, instant):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
418 return [PredictedPolyTrajectoryConstantVelocity(obj.getBoundingPolygon(instant), obj.getVelocityAtInstant(instant))]
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
419
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
420 def computeCollisionPoint(self, p1, p2, probability, t):
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
421 polyRepr1 = p1.representative_point()
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
422 polyRepr2 = p2.representative_point()
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
423 p = moving.Point(polyRepr1.x+polyRepr2.x, polyRepr1.y+polyRepr2.y)
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
424 return SafetyPoint(p, probability, t)
af329f3330ba work in progress on 3D safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1211
diff changeset
425
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
426 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
427 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
428 '''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
429 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
430 '''
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
431 if useFeatures:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
432 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
433 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
434 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
435 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
436 self.nPredictedTrajectories = nPredictedTrajectories
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
437 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
438 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
439 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
440
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
441 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
442 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
443 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
444 self.maxSteering)
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
445
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
446 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
447 predictedTrajectories = []
860
07c5eab11eba fixing bug, thanks to Ryan Louie <Ryan.Louie@students.olin.edu>
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 826
diff changeset
448 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
449 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
450 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
451 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
452 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
453 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
454 velocities = [obj.getVelocityAtInstant(instant)]
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
455 probability = 1./float(len(positions)*self.nPredictedTrajectories)
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
456 for i in range(self.nPredictedTrajectories):
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
457 for initialPosition,initialVelocity in zip(positions, velocities):
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
458 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
459 initialVelocity,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
460 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
461 self.steeringDistribution,
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
462 probability,
466
e891a41c6c75 name change in prediction.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 464
diff changeset
463 maxSpeed = self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
464 return predictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
465
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
466 class PointSetPredictionParameters(PredictionParameters):
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 484
diff changeset
467 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
468 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
469
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
470 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
471 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
472 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
473 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
474 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
475 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
476 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
477 for initialPosition,initialVelocity in zip(positions, velocities):
1211
a095d4fbb2ea work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1210
diff changeset
478 predictedTrajectories.append(PredictedTrajectoryConstantVelocity(initialPosition, initialVelocity, probability = probability))
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
479 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
480 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
481 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
482 return None
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
483
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
484 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
485 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
486 '''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
487 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
488
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
489 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
490 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
491 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
492 name = 'evasive action'
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
493 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
494 self.nPredictedTrajectories = nPredictedTrajectories
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
495 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
496 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
497 self.steeringDistribution = steeringDistribution
268
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
498
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
499 def __str__(self):
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
500 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
501
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
502 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
503 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
504 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
505 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
506 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
507 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
508 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
509 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 267
diff changeset
510 velocities = [obj.getVelocityAtInstant(instant)]
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
511 probability = 1./float(self.nPredictedTrajectories)
998
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 987
diff changeset
512 for i in range(self.nPredictedTrajectories):
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
513 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
514 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
515 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
516 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 336
diff changeset
517 self.steeringDistribution()),
826
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
518 probability,
8b74a5176549 explicitly computed the probabilities for predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 701
diff changeset
519 self.maxSpeed))
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
520 return predictedTrajectories
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
521
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
522 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
523 '''Prediction parameters of prediction at constant velocity
698
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
524 using direct computation of the intersecting point
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
525 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
526
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
527 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
528 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
529
630
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
530 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
531 collisionPoints = []
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
532 if computeCZ:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
533 crossingZones = []
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
534 else:
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
535 crossingZones = None
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
536
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
537 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
538 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
539 if (p1-p2).norm2() <= collisionDistanceThreshold:
985
668a85c963c3 work on processing and managing large video datasets
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 949
diff changeset
540 collisionPoints = [SafetyPoint((p1+p2)*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
541 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
542 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
543 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
544 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
545
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
546 if 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
547 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
548 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
549 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
550 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
551 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
552 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
553 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
554 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
555 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
556 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
557 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
558 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
559 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
560
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 629
diff changeset
561 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
562 if computeCZ:
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
563 crossingZones.append(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
564 else:
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
565 collisionPoints.append(SafetyPoint(intersection, 1., collisionTimeInterval.center()))
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
566
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
567 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
568 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
569 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
570 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
571 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
572 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
573 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 489
diff changeset
574 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
575 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
576 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
577
987
f026ce2af637 found bug with direct ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 985
diff changeset
578 return collisionPoints, crossingZones
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
579
464
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
580 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 460
diff changeset
581 '''Prediction parameters of prediction at constant velocity
698
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
582 using direct computation of the intersecting point (solving the equation)
8d99a9e16644 added clarification comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 696
diff changeset
583 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
584
1104
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
585 def __init__(self, useCurvilinear = False):
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
586 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear)
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 358
diff changeset
587
701
4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 698
diff changeset
588 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
589 'TODO compute pPET'
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
590 collisionPoints = []
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
591 if computeCZ:
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
592 crossingZones = []
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
593 else:
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
594 crossingZones = None
269
a9988971aac8 removed legacy code + tweaks
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 268
diff changeset
595
1104
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
596 if self.useCurvilinear:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
597 pass # Lionel
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
598 else:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
599 p1 = obj1.getPositionAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
600 p2 = obj2.getPositionAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
601 v1 = obj1.getVelocityAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
602 v2 = obj2.getVelocityAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
603 #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
604
1104
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
605 if not moving.Point.parallel(v1, v2):
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
606 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
607 if ttc is not None:
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
608 collisionPoints.append(SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc))
1104
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
609 else:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1029
diff changeset
610 pass # compute pPET
1209
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
611 return collisionPoints, crossingZones
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 860
diff changeset
612
1214
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
613 class CVExactPolyPredictionParameters(PredictionParameters):
1209
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
614 '''Prediction parameters of prediction at constant velocity
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
615 for objects represented by boxes (bird eye view boxes)
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
616 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
617
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
618 def __init__(self):
1214
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
619 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None)
1209
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
620
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
621 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
622 'TODO compute pPET'
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
623 collisionPoints = []
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
624 if computeCZ:
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
625 crossingZones = []
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
626 else:
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
627 crossingZones = None
1209
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
628
1214
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
629 if self.useCurvilinear:
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
630 pass # Lionel
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
631 else:
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
632 poly1 = obj1.getBoundingPolygon(currentInstant)
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
633 poly2 = obj2.getBoundingPolygon(currentInstant)
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
634 v1 = obj1.getVelocityAtInstant(currentInstant)
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
635 v2 = obj2.getVelocityAtInstant(currentInstant)
1209
2064e52019db work on TTC for BEV
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1154
diff changeset
636
1214
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
637 if not moving.Point.parallel(v1, v2):
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
638 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2)
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
639 if ttc is not None:
1215
1b472cddf9b1 updated reading by interpolating missing frames
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1214
diff changeset
640 collisionPoints.append(SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc))
1214
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
641 else:
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
642 pass # compute pPET
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
643 return collisionPoints, crossingZones
01c24c1cdb70 implemented direct method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1212
diff changeset
644
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
645
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
646 class PrototypePredictionParameters(PredictionParameters):
944
84ebe1b031f1 works with object trajectory, features todo
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
647 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
648 PredictionParameters.__init__(self, 'prototypes', None)
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
649 self.prototypes = prototypes
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
650 self.nPredictedTrajectories = nPredictedTrajectories
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
651 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
652 self.minSimilarity = minSimilarity
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
653 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
654 self.constantSpeed = constantSpeed
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
655 self.useFeatures = useFeatures
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
656
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
657 def getLcss(self):
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
658 return self.lcss
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
659
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
660 def addPredictedTrajectories(self, predictedTrajectories, obj, instant):
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 948
diff changeset
661 obj.computeTrajectorySimilarities(self.prototypes, self.lcss)
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
662 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
663 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
664 initialPosition = obj.getPositionAtInstant(instant)
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
665 initialVelocity = obj.getVelocityAtInstant(instant)
1154
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
666 predictedTrajectory = PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings())
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
667 if predictedTrajectory.valid:
2795d0e114c9 deal with possibility of prototype with speed 0 that crashes motion prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 1104
diff changeset
668 predictedTrajectories.append(predictedTrajectory)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
669
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
670 def generatePredictedTrajectories(self, obj, instant):
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
671 predictedTrajectories = []
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
672 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
673 if self.useFeatures and obj.hasFeatures():
946
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
674 if not hasattr(obj, 'currentPredictionFeatures'):
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
675 obj.currentPredictionFeatures = []
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
676 else:
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
677 obj.currentPredictionFeatures[:] = [f for f in obj.currentPredictionFeatures if f.existsAtInstant(instant)]
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
678 firstInstants = [(f,f.getFirstInstant()) for f in obj.getFeatures() if f.existsAtInstant(instant) and f not in obj.currentPredictionFeatures]
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
679 firstInstants.sort(key = lambda t: t[1])
946
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
680 for f,t1 in firstInstants[:min(self.nPredictedTrajectories, len(firstInstants), self.nPredictedTrajectories-len(obj.currentPredictionFeatures))]:
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
681 obj.currentPredictionFeatures.append(f)
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 945
diff changeset
682 for f in obj.currentPredictionFeatures:
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
683 self.addPredictedTrajectories(predictedTrajectories, f, instant)
943
b1e8453c207c work on motion prediction using motion patterns
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 942
diff changeset
684 else:
945
05d4302bf67e working motion pattern prediction with rotation and features
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 944
diff changeset
685 self.addPredictedTrajectories(predictedTrajectories, obj, instant)
607
84690dfe5560 add some functions for behaviour analysis
MohamedGomaa
parents: 571
diff changeset
686 return predictedTrajectories
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
687
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
688 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
689 import doctest
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
690 import unittest
271
bbd9c09e6869 changed the names to prediction methods and predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 270
diff changeset
691 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
692 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
693 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
694 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
695 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
696