Mercurial Hosting > traffic-intelligence
annotate trafficintelligence/prediction.py @ 1273:655a1646f0d5
updated setRoadUsers to force the new objects
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 18 Jun 2024 15:29:28 -0400 |
parents | ca70a79688ae |
children |
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 | 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 | 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 | 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 | 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 | 240 |
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 | 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 | 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 | 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 | 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 | 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 | 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 | 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 |