annotate python/events.py @ 949:d6c1c05d11f5

modified multithreading at the interaction level for safety computations
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 21 Jul 2017 17:52:56 -0400
parents b1e8453c207c
children 030b16ab4f64
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
56
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
1 #! /usr/bin/env python
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
2 '''Libraries for events
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
3 Interactions, pedestrian crossing...'''
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
4
731
b02431a8234c made prototypecluster generic, in ml module, and added randominitialization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 728
diff changeset
5 import moving, prediction, indicators, utils, cvutils, ml
665
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 664
diff changeset
6 from base import VideoFilenameAddable
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 664
diff changeset
7
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
8 import numpy as np
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
9
292
8b2c8a4015f1 class VehPairs updated. Now supports primitive multithreading.
Paul@BEAST-III
parents: 291
diff changeset
10 import multiprocessing
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
11 import itertools, logging
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
12
56
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
13
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 610
diff changeset
14 def findRoute(prototypes,objects,i,j,noiseEntryNums,noiseExitNums,minSimilarity= 0.3, 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
15 if i[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
16 prototypesRoutes= [ x for x in sorted(prototypes.keys()) if i[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
17 elif i[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
18 prototypesRoutes=[ x for x in sorted(prototypes.keys()) if i[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
19 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
20 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
21 routeSim={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
22 lcss = utils.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
23 for y in prototypesRoutes:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
24 if y in prototypes.keys():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
25 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
26 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
27 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
28 s=lcss.computeNormalized(objects[j].positions, 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
29 similarity.append(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
30 routeSim[y]=max(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
31 route=max(routeSim, key=routeSim.get)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
32 if routeSim[route]>=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
33 return 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
34 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
35 return i
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 610
diff changeset
36
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
37 def getRoute(obj,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination=True):
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
38 route=(obj.startRouteID,obj.endRouteID)
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
39 if useDestination:
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
40 if route not in prototypes.keys():
613
306db0f3c7a2 move 4 functions from trajLearning file
MohamedGomaa
parents: 610
diff changeset
41 route= findRoute(prototypes,objects,route,obj.getNum(),noiseEntryNums,noiseExitNums)
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
42 return route
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
43
664
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
44 class Interaction(moving.STObject, VideoFilenameAddable):
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
45 '''Class for an interaction between two road users
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
46 or a road user and an obstacle
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
47
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
48 link to the moving objects
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
49 contains the indicators in a dictionary with the names as keys
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
50 '''
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
51
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
52 categories = {'Head On': 0,
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
53 'rearend': 1,
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
54 'side': 2,
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
55 'parallel': 3}
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
56
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
57 indicatorNames = ['Collision Course Dot Product',
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
58 'Collision Course Angle',
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
59 'Distance',
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
60 'Minimum Distance',
341
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
61 'Velocity Angle',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
62 'Speed Differential',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
63 'Collision Probability',
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
64 'Time to Collision', # 7
341
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
65 'Probability of Successful Evasive Action',
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
66 'predicted Post Encroachment Time',
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
67 'Post Encroachment Time']
341
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
68
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
69 indicatorNameToIndices = utils.inverseEnumeration(indicatorNames)
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
70
388
6e0dedd34920 minor name change
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 387
diff changeset
71 indicatorShortNames = ['CCDP',
408
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
72 'CCA',
341
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
73 'Dist',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
74 'MinDist',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
75 'VA',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
76 'SD',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
77 'PoC',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
78 'TTC',
2f39c4ed0b62 first version of indicator saving to sqlite
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 340
diff changeset
79 'P(SEA)',
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
80 'pPET',
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
81 'PET']
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
82
408
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
83 indicatorUnits = ['',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
84 'rad',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
85 'm',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
86 'm',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
87 'rad',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
88 'm/s',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
89 '',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
90 's',
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
91 '',
881
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
92 's',
8ba82b371eea work on storing PET
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 735
diff changeset
93 's']
408
365d8dee44f3 last changes for TRB14
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 388
diff changeset
94
695
957126bfb456 corrected bug with indicator loading(also now correctly loading mostsevereismax)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 694
diff changeset
95 timeIndicators = ['Time to Collision', 'predicted Post Encroachment Time']
957126bfb456 corrected bug with indicator loading(also now correctly loading mostsevereismax)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 694
diff changeset
96
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
97 def __init__(self, num = None, timeInterval = None, roaduserNum1 = None, roaduserNum2 = None, roadUser1 = None, roadUser2 = None, categoryNum = None):
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
98 moving.STObject.__init__(self, num, timeInterval)
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
99 if timeInterval is None and roadUser1 is not None and roadUser2 is not None:
566
07b1bd0785cd simplifications to interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 559
diff changeset
100 self.timeInterval = roadUser1.commonTimeInterval(roadUser2)
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
101 self.roadUser1 = roadUser1
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
102 self.roadUser2 = roadUser2
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
103 if roaduserNum1 is not None and roaduserNum2 is not None:
566
07b1bd0785cd simplifications to interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 559
diff changeset
104 self.roadUserNumbers = set([roaduserNum1, roaduserNum2])
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
105 elif roadUser1 is not None and roadUser2 is not None:
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
106 self.roadUserNumbers = set([roadUser1.getNum(), roadUser2.getNum()])
566
07b1bd0785cd simplifications to interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 559
diff changeset
107 else:
07b1bd0785cd simplifications to interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 559
diff changeset
108 self.roadUserNumbers = None
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
109 self.categoryNum = categoryNum
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
110 self.indicators = {}
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 436
diff changeset
111 self.interactionInterval = None
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
112 # list for collison points and crossing zones
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
113 self.collisionPoints = None
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
114 self.crossingZones = None
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
115
340
1046b7346886 work in progress on storing indicator values
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
116 def getRoadUserNumbers(self):
1046b7346886 work in progress on storing indicator values
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
117 return self.roadUserNumbers
1046b7346886 work in progress on storing indicator values
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 339
diff changeset
118
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
119 def setRoadUsers(self, objects):
694
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
120 nums = sorted(list(self.getRoadUserNumbers()))
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
121 if nums[0]<len(objects) and objects[nums[0]].getNum() == nums[0]:
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
122 self.roadUser1 = objects[nums[0]]
694
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
123 if nums[1]<len(objects) and objects[nums[1]].getNum() == nums[1]:
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
124 self.roadUser2 = objects[nums[1]]
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
125
694
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
126 if self.roadUser1 is None or self.roadUser2 is None:
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
127 self.roadUser1 = None
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
128 self.roadUser2 = None
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
129 i = 0
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
130 while i < len(objects) and self.roadUser2 is None:
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
131 if objects[i].getNum() in nums:
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
132 if self.roadUser1 is None:
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
133 self.roadUser1 = objects[i]
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
134 else:
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
135 self.roadUser2 = objects[i]
c4363aa6f0e5 updated function to find road users for interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 692
diff changeset
136 i += 1
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
137
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
138 def getIndicator(self, indicatorName):
321
a5e40bd04cf4 rearranged LCSS indicator functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 320
diff changeset
139 return self.indicators.get(indicatorName, None)
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
140
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
141 def addIndicator(self, indicator):
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: 636
diff changeset
142 if indicator is not None:
321
a5e40bd04cf4 rearranged LCSS indicator functions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 320
diff changeset
143 self.indicators[indicator.name] = indicator
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
144
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: 636
diff changeset
145 def getIndicatorValueAtInstant(self, indicatorName, 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: 636
diff changeset
146 indicator = self.getIndicator(indicatorName)
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: 636
diff changeset
147 if indicator is not None:
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: 636
diff changeset
148 return indicator[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: 636
diff changeset
149 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: 636
diff changeset
150 return None
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: 636
diff changeset
151
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: 636
diff changeset
152 def getIndicatorValuesAtInstant(self, 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: 636
diff changeset
153 '''Returns list of indicator values at 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: 636
diff changeset
154 as dict (with keys from indicators dict)'''
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: 636
diff changeset
155 values = {}
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: 636
diff changeset
156 for k, indicator in self.indicators.iteritems():
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: 636
diff changeset
157 values[k] = indicator[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: 636
diff changeset
158 return values
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: 636
diff changeset
159
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
160 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs):
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
161 self.roadUser1.plot(options, withOrigin, timeStep, withFeatures, **kwargs)
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
162 self.roadUser2.plot(options, withOrigin, timeStep, withFeatures, **kwargs)
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
163
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
164 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, **kwargs):
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
165 self.roadUser1.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, **kwargs)
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
166 self.roadUser2.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, **kwargs)
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
167
887
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
168 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., allUserInstants = False):
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
169 if self.roadUser1 is not None and self.roadUser2 is not None:
887
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
170 if allUserInstants:
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
171 firstFrameNum = min(self.roadUser1.getFirstInstant(), self.roadUser2.getFirstInstant())
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
172 lastFrameNum = max(self.roadUser1.getLastInstant(), self.roadUser2.getLastInstant())
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
173 else:
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
174 firstFrameNum = self.getFirstInstant()
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
175 lastFrameNum = self.getLastInstant()
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
176 cvutils.displayTrajectories(videoFilename, [self.roadUser1, self.roadUser2], homography = homography, firstFrameNum = firstFrameNum, lastFrameNumArg = lastFrameNum, undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
177 else:
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
178 print('Please set the interaction road user attributes roadUser1 and roadUser1 through the method setRoadUsers')
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
179
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
180 def computeIndicators(self):
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
181 '''Computes the collision course cosine only if the cosine is positive'''
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
182 collisionCourseDotProducts = {}#[0]*int(self.timeInterval.length())
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
183 collisionCourseAngles = {}
325
6c9c7c956926 added velocity angle in indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 321
diff changeset
184 velocityAngles = {}
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
185 distances = {}#[0]*int(self.timeInterval.length())
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
186 speedDifferentials = {}
452
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
187 interactionInstants = []
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
188 for instant in self.timeInterval:
320
419f30491a4b renamed fields movingObject to roadUser, etc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 317
diff changeset
189 deltap = self.roadUser1.getPositionAtInstant(instant)-self.roadUser2.getPositionAtInstant(instant)
325
6c9c7c956926 added velocity angle in indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 321
diff changeset
190 v1 = self.roadUser1.getVelocityAtInstant(instant)
6c9c7c956926 added velocity angle in indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 321
diff changeset
191 v2 = self.roadUser2.getVelocityAtInstant(instant)
6c9c7c956926 added velocity angle in indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 321
diff changeset
192 deltav = v2-v1
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
193 velocityAngles[instant] = np.arccos(moving.Point.dot(v1, v2)/(v1.norm2()*v2.norm2()))
299
7e5fb4abd070 renaming event to events and correcting errors in indicator computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 294
diff changeset
194 collisionCourseDotProducts[instant] = moving.Point.dot(deltap, deltav)
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 310
diff changeset
195 distances[instant] = deltap.norm2()
294
1f253f218b9f evolution of indicators and their computation in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 293
diff changeset
196 speedDifferentials[instant] = deltav.norm2()
452
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
197 if collisionCourseDotProducts[instant] > 0:
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
198 interactionInstants.append(instant)
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 if distances[instant] != 0 and speedDifferentials[instant] != 0:
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
200 collisionCourseAngles[instant] = np.arccos(collisionCourseDotProducts[instant]/(distances[instant]*speedDifferentials[instant]))
306
93d851d0d21e bug correction, minor work on indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 299
diff changeset
201
452
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
202 if len(interactionInstants) >= 2:
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
203 self.interactionInterval = moving.TimeInterval(interactionInstants[0], interactionInstants[-1])
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
204 else:
c59a47ce0209 reorganized interactioninterval (in compute indicators) and comments
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 451
diff changeset
205 self.interactionInterval = moving.TimeInterval()
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 382
diff changeset
206 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[0], collisionCourseDotProducts))
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 382
diff changeset
207 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[1], collisionCourseAngles))
691
fa9aa5f08210 cleaned imports in indicators.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 665
diff changeset
208 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[2], distances, mostSevereIsMax = False))
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 382
diff changeset
209 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[4], velocityAngles))
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 382
diff changeset
210 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[5], speedDifferentials))
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
211
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 310
diff changeset
212 # if we have features, compute other indicators
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: 636
diff changeset
213 if self.roadUser1.hasFeatures() and self.roadUser2.hasFeatures():
691
fa9aa5f08210 cleaned imports in indicators.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 665
diff changeset
214 minDistances={}
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 310
diff changeset
215 for instant in self.timeInterval:
691
fa9aa5f08210 cleaned imports in indicators.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 665
diff changeset
216 minDistances[instant] = moving.MovingObject.minDistance(self.roadUser1, self.roadUser2, instant)
fa9aa5f08210 cleaned imports in indicators.py
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 665
diff changeset
217 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[3], minDistances, mostSevereIsMax = False))
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 310
diff changeset
218
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
219 def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
220 '''Computes all crossing and collision points at each common instant for two road users. '''
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
221 TTCs = {}
937
b67a784beb69 work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 887
diff changeset
222 collisionProbabilities = {}
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
223 if timeInterval is not None:
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
224 commonTimeInterval = timeInterval
317
d280b881e860 added indicator min distance
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 310
diff changeset
225 else:
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
226 commonTimeInterval = self.timeInterval
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
227 self.collisionPoints, crossingZones = predictionParameters.computeCrossingsCollisions(self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval)
937
b67a784beb69 work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 887
diff changeset
228 for i, cps in self.collisionPoints.iteritems():
b67a784beb69 work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 887
diff changeset
229 TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cps)
b67a784beb69 work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 887
diff changeset
230 collisionProbabilities[i] = sum([p.probability for p in cps])
692
9a258687af4c corrected some errors for ttc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 691
diff changeset
231 if len(TTCs) > 0:
9a258687af4c corrected some errors for ttc
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 691
diff changeset
232 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs, mostSevereIsMax=False))
937
b67a784beb69 work started on prototype prediction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 887
diff changeset
233 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[6], collisionProbabilities))
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
234
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
235 # crossing zones and pPET
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
236 if computeCZ:
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
237 self.crossingZones = crossingZones
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 325
diff changeset
238 pPETs = {}
610
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
239 for i, cz in self.crossingZones.iteritems():
0dc36203973d remove dublicated code for collision/crossing computations
MohamedGomaa
parents: 607
diff changeset
240 pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(cz)
631
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
241 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs, mostSevereIsMax=False))
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
242 # TODO add probability of collision, and probability of successful evasive action
631
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
243
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 630
diff changeset
244 def computePET(self, collisionDistanceThreshold):
887
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
245 pet, t1, t2= moving.MovingObject.computePET(self.roadUser1, self.roadUser2, collisionDistanceThreshold)
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
246 if pet is not None:
e2452abba0e7 added option to compute PET in safety analysis script, and save in database
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 882
diff changeset
247 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[10], {min(t1, t2): pet}, mostSevereIsMax = False))
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 613
diff changeset
248
664
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
249 def setCollision(self, collision):
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
250 '''indicates if it is a collision: argument should be boolean'''
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
251 self.collision = collision
310
f7ca78a11ea6 add functions to add videofilename and interaction type in Interaction class
Mohamed Gomaa
parents: 306
diff changeset
252
664
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
253 def isCollision(self):
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
254 if hasattr(self, 'collision'):
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
255 return self.collision
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
256 else:
455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 662
diff changeset
257 return None
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: 636
diff changeset
258
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
259 def getCollisionPoints(self):
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
260 return self.collisionPoints
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: 636
diff changeset
261
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
262 def getCrossingZones(self):
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
263 return self.crossingZones
451
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 436
diff changeset
264
487
e04b22ce2fcd generalized createInteractions to 2 lists of objects (for cars and pedestrians for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 454
diff changeset
265 def createInteractions(objects, _others = None):
e04b22ce2fcd generalized createInteractions to 2 lists of objects (for cars and pedestrians for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 454
diff changeset
266 '''Create all interactions of two co-existing road users'''
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
267 if _others is not None:
487
e04b22ce2fcd generalized createInteractions to 2 lists of objects (for cars and pedestrians for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 454
diff changeset
268 others = _others
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
269
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
270 interactions = []
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
271 num = 0
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
272 for i in xrange(len(objects)):
636
3058e00887bc removed all issues because of tests with None, using is instead of == or !=
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 632
diff changeset
273 if _others is None:
487
e04b22ce2fcd generalized createInteractions to 2 lists of objects (for cars and pedestrians for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 454
diff changeset
274 others = objects[:i]
e04b22ce2fcd generalized createInteractions to 2 lists of objects (for cars and pedestrians for example)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 454
diff changeset
275 for j in xrange(len(others)):
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 487
diff changeset
276 commonTimeInterval = objects[i].commonTimeInterval(others[j])
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
277 if not commonTimeInterval.empty():
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 487
diff changeset
278 interactions.append(Interaction(num, commonTimeInterval, objects[i].num, others[j].num, objects[i], others[j]))
293
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
279 num += 1
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
280 return interactions
ee3302528cdc rearranged new code by Paul (works now)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 292
diff changeset
281
628
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
282 def findInteraction(interactions, roadUserNum1, roadUserNum2):
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
283 'Returns the right interaction in the set'
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
284 i=0
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
285 while i<len(interactions) and set([roadUserNum1, roadUserNum2]) != interactions[i].getRoadUserNumbers():
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
286 i+=1
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
287 if i<len(interactions):
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
288 return interactions[i]
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
289 else:
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
290 return None
977407c9f815 corrected bugs in loading interactions (index shifted) and added functionalities to play/plot interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 627
diff changeset
291
949
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
292 def computeIndicators(interactions, computeMotionPrediction, computePET, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
293 for inter in interactions:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
294 print('processing interaction {}'.format(inter.getNum())) # logging.debug('processing interaction {}'.format(inter.getNum()))
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
295 inter.computeIndicators()
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
296 if computeMotionPrediction:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
297 inter.computeCrossingsCollisions(predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval)
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
298 if computePET:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
299 inter.computePET(collisionDistanceTreshold)
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
300 return interactions
d6c1c05d11f5 modified multithreading at the interaction level for safety computations
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 943
diff changeset
301
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
302 def aggregateSafetyPoints(interactions, pointType = 'collision'):
632
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
303 '''Put all collision points or crossing zones in a list for display'''
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
304 allPoints = []
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
305 if pointType == 'collision':
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
306 for i in interactions:
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
307 for points in i.collisionPoints.values():
632
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
308 allPoints += points
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
309 elif pointType == 'crossing':
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
310 for i in interactions:
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
311 for points in i.crossingZones.values():
632
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
312 allPoints += points
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
313 else:
705
0ceee3b1a96d cleanup crossing collisions and crossing zones in Interaction (not stored per type of prediction method)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 695
diff changeset
314 print('unknown type of point: '+pointType)
632
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
315 return allPoints
2f1a583bfd20 added utility for safety points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 631
diff changeset
316
735
0e875a7f5759 modified prototypeCluster algorithm to enforce similarity when re-assigning and to compute only the necessary similarities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 731
diff changeset
317 def prototypeCluster(interactions, similarities, indicatorName, minSimilarity, similarityFunc = None, minClusterSize = None, randomInitialization = False):
0e875a7f5759 modified prototypeCluster algorithm to enforce similarity when re-assigning and to compute only the necessary similarities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 731
diff changeset
318 return ml.prototypeCluster([inter.getIndicator(indicatorName) for inter in interactions], similarities, minSimilarity, similarityFunc, minClusterSize, randomInitialization)
291
9f81218e497a class VehPairs subsumes createInteractions(objects); legacy code remains
Paul@BEAST-III
parents: 283
diff changeset
319
59
f955e83da499 developed indicators in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 56
diff changeset
320 class Crossing(moving.STObject):
56
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
321 '''Class for the event of a street crossing
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
322
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
323 TODO: detecter passage sur la chaussee
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
324 identifier origines et destination (ou uniquement chaussee dans FOV)
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
325 carac traversee
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
326 detecter proximite veh (retirer si trop similaire simultanement
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
327 carac interaction'''
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
328
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
329 def __init__(self, roaduserNum = None, num = None, timeInterval = None):
59
f955e83da499 developed indicators in interactions
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 56
diff changeset
330 moving.STObject.__init__(self, num, timeInterval)
56
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
331 self.roaduserNum = roaduserNum
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
332
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
333
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
334
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
335 if __name__ == "__main__":
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
336 import doctest
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
337 import unittest
489
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 487
diff changeset
338 suite = doctest.DocFileSuite('tests/events.txt')
000bddf84ad0 corrected bugs in safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 487
diff changeset
339 #suite = doctest.DocTestSuite()
56
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
340 unittest.TextTestRunner().run(suite)
61fe73df2d36 created new package, moved Interaction class and created Crossing class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
341