Mercurial > hg > nsaunier > traffic-intelligence

comparison python/events.py @ 662:72174e66aba5

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 18 May 2015 17:17:06 +0200
parents dc70d9e711f5
children 455f9b93819c
comparison
equal deleted inserted replaced
661:dc70d9e711f5 662:72174e66aba5
96 self.roadUser1 = roadUser1 96 self.roadUser1 = roadUser1
97 self.roadUser2 = roadUser2 97 self.roadUser2 = roadUser2
98 if roaduserNum1 is not None and roaduserNum2 is not None: 98 if roaduserNum1 is not None and roaduserNum2 is not None:
99 self.roadUserNumbers = set([roaduserNum1, roaduserNum2]) 99 self.roadUserNumbers = set([roaduserNum1, roaduserNum2])
100 elif roadUser1 is not None and roadUser2 is not None: 100 elif roadUser1 is not None and roadUser2 is not None:
101 self.roadUserNumbers = set(roadUser1.getNum(), roadUser2.getNum()) 101 self.roadUserNumbers = set([roadUser1.getNum(), roadUser2.getNum()])
102 else: 102 else:
103 self.roadUserNumbers = None 103 self.roadUserNumbers = None
104 self.categoryNum = categoryNum 104 self.categoryNum = categoryNum
105 self.indicators = {} 105 self.indicators = {}
106 self.interactionInterval = None 106 self.interactionInterval = None
107 self.collisionPoints = None # distionary for collison points with different prediction methods 107 # list for collison points and crossing zones
108 self.collisionPoints = None
109 self.crossingZones = None
108 110
109 def getRoadUserNumbers(self): 111 def getRoadUserNumbers(self):
110 return self.roadUserNumbers 112 return self.roadUserNumbers
111 113
112 def setRoadUsers(self, objects): 114 def setRoadUsers(self, objects):
178 collisionCourseDotProducts[instant] = moving.Point.dot(deltap, deltav) 180 collisionCourseDotProducts[instant] = moving.Point.dot(deltap, deltav)
179 distances[instant] = deltap.norm2() 181 distances[instant] = deltap.norm2()
180 speedDifferentials[instant] = deltav.norm2() 182 speedDifferentials[instant] = deltav.norm2()
181 if collisionCourseDotProducts[instant] > 0: 183 if collisionCourseDotProducts[instant] > 0:
182 interactionInstants.append(instant) 184 interactionInstants.append(instant)
183 collisionCourseAngles[instant] = arccos(collisionCourseDotProducts[instant]/(distances[instant]*speedDifferentials[instant])) 185 if distances[instant] != 0 and speedDifferentials[instant] != 0:
186 collisionCourseAngles[instant] = arccos(collisionCourseDotProducts[instant]/(distances[instant]*speedDifferentials[instant]))
184 187
185 if len(interactionInstants) >= 2: 188 if len(interactionInstants) >= 2:
186 self.interactionInterval = moving.TimeInterval(interactionInstants[0], interactionInstants[-1]) 189 self.interactionInterval = moving.TimeInterval(interactionInstants[0], interactionInstants[-1])
187 else: 190 else:
188 self.interactionInterval = moving.TimeInterval() 191 self.interactionInterval = moving.TimeInterval()
199 minDistance[instant] = moving.MovingObject.minDistance(self.roadUser1, self.roadUser2, instant) 202 minDistance[instant] = moving.MovingObject.minDistance(self.roadUser1, self.roadUser2, instant)
200 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[3], minDistance)) 203 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[3], minDistance))
201 204
202 def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1, usePrototypes=False, route1= (-1,-1), route2=(-1,-1), prototypes={}, secondStepPrototypes={}, nMatching={}, objects=[], noiseEntryNums=[], noiseExitNums=[], minSimilarity=0.1, mostMatched=None, useDestination=True, useSpeedPrototype=True, acceptPartialLength=30, step=1): 205 def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1, usePrototypes=False, route1= (-1,-1), route2=(-1,-1), prototypes={}, secondStepPrototypes={}, nMatching={}, objects=[], noiseEntryNums=[], noiseExitNums=[], minSimilarity=0.1, mostMatched=None, useDestination=True, useSpeedPrototype=True, acceptPartialLength=30, step=1):
203 '''Computes all crossing and collision points at each common instant for two road users. ''' 206 '''Computes all crossing and collision points at each common instant for two road users. '''
204 self.collisionPoints={}
205 self.crossingZones={}
206 TTCs = {} 207 TTCs = {}
207 if usePrototypes: 208 if usePrototypes:
208 route1= getRoute(self.roadUser1,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) 209 route1= getRoute(self.roadUser1,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination)
209 route2= getRoute(self.roadUser2,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) 210 route2= getRoute(self.roadUser2,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination)
210 211
211 if timeInterval is not None: 212 if timeInterval is not None:
212 commonTimeInterval = timeInterval 213 commonTimeInterval = timeInterval
213 else: 214 else:
214 commonTimeInterval = self.timeInterval 215 commonTimeInterval = self.timeInterval
215 self.collisionPoints[predictionParameters.name], crossingZones = predictionParameters.computeCrossingsCollisions(self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step) 216 self.collisionPoints, crossingZones = predictionParameters.computeCrossingsCollisions(self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
217 for i, cp in self.collisionPoints.iteritems():
218 TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp)
219 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs, mostSevereIsMax=False))
220
221 # crossing zones and pPET
216 if computeCZ: 222 if computeCZ:
217 self.crossingZones[predictionParameters.name] = crossingZones 223 self.crossingZones[predictionParameters.name] = crossingZones
218 for i, cp in self.collisionPoints.iteritems():
219 TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp)
220 # add probability of collision, and probability of successful evasive action
221 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs, mostSevereIsMax=False))
222
223 if computeCZ:
224 pPETs = {} 224 pPETs = {}
225 for i, cz in self.crossingZones.iteritems(): 225 for i, cz in self.crossingZones.iteritems():
226 pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(cz) 226 pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(cz)
227 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs, mostSevereIsMax=False)) 227 self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs, mostSevereIsMax=False))
228 # TODO add probability of collision, and probability of successful evasive action
228 229
229 def computePET(self, collisionDistanceThreshold): 230 def computePET(self, collisionDistanceThreshold):
230 # TODO add crossing zone 231 # TODO add crossing zone
231 self.pet = moving.MovingObject.computePET(self.roadUser1, self.roadUser2, collisionDistanceThreshold) 232 self.pet = moving.MovingObject.computePET(self.roadUser1, self.roadUser2, collisionDistanceThreshold)
232 233
236 def addInteractionType(self,interactionType): 237 def addInteractionType(self,interactionType):
237 ''' interaction types: conflict or collision if they are known''' 238 ''' interaction types: conflict or collision if they are known'''
238 self.interactionType = interactionType 239 self.interactionType = interactionType
239 240
240 def getCrossingZones(self, predictionMethodName): 241 def getCrossingZones(self, predictionMethodName):
241 if self.hasattr(self, 'crossingZones'): 242 if self.crossingZones is not None:
242 return self.crossingZones[predictionMethodName] 243 return self.crossingZones[predictionMethodName]
243 else: 244 else:
244 return None 245 return None
245 246
246 def getCollisionPoints(self, predictionMethodName): 247 def getCollisionPoints(self, predictionMethodName):
275 if i<len(interactions): 276 if i<len(interactions):
276 return interactions[i] 277 return interactions[i]
277 else: 278 else:
278 return None 279 return None
279 280
280 def aggregateSafetyPoints(interactions, pointType = 'collision'): 281 def aggregateSafetyPoints(interactions, predictionMethodName = None, pointType = 'collision'):
281 '''Put all collision points or crossing zones in a list for display''' 282 '''Put all collision points or crossing zones in a list for display'''
283 if predictionMethodName is None and len(interactions)>0:
284 predictionMethodName = interactions[0].collisionPoints.keys()[0]
285
282 allPoints = [] 286 allPoints = []
283 if pointType == 'collision': 287 if pointType == 'collision':
284 for i in interactions: 288 for i in interactions:
285 for points in i.collisionPoints.values(): 289 for points in i.collisionPoints[predictionMethodName].values():
286 allPoints += points 290 allPoints += points
287 elif pointType == 'crossing': 291 elif pointType == 'crossing':
288 for i in interactions: 292 for i in interactions:
289 for points in i.crossingZones.values(): 293 for points in i.crossingZones[predictionMethodName].values():
290 allPoints += points 294 allPoints += points
291 else: 295 else:
292 print('unknown type of point '+pointType) 296 print('unknown type of point '+pointType)
293 return allPoints 297 return allPoints
294 298