repo/traffic-intelligence: python/prediction.py annotate

annotate python/prediction.py @ 692:9a258687af4c dev

corrected some errors for ttc

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 29 Jun 2015 23:09:58 -0400
parents	15e244d2a1b5
children	ae137e3b1990

rev	line source
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	1 #! /usr/bin/env python
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	2 '''Library for motion prediction methods'''
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	3
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	4 import moving
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	5 from utils import LCSS
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	6
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	7 import math, random
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	8 import numpy as np
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	9
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	10
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	11 class PredictedTrajectory(object):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	12 '''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	13 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	14
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	15 it should also have a probability'''
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	16
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	17 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	18 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	19 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	20 self.predictedSpeedOrientations = {}
300 f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	21 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	22 #self.crossingZones = {}
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	23
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	24 def predictPosition(self, nTimeSteps):
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	25 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	26 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	27 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	28 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	29
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	30 def getPredictedTrajectory(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	31 return moving.Trajectory.fromPointList(self.predictedPositions.values())
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	32
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	33 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	34 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	35
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	36 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	37 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	38
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	39 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	40 '''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	41 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	42
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	43 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	44 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	45 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	46 self.probability = probability
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	47 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	48 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	49
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	50 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	51 return self.control
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	52
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	53 def findNearestParams(initialPosition,prototypeTrajectory):
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	54 ''' nearest parameters are the index of minDistance and the orientation '''
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	55 distances=[]
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	56 for position in prototypeTrajectory.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	57 distances.append(moving.Point.distanceNorm2(initialPosition, position))
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	58 minDistanceIndex= np.argmin(distances)
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	59 return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	60
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	61 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	62 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	63 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	64 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	65 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	66
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	67 Prediction can be done
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	68 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	69 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	70 (applying a constant ratio equal
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	71 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	72
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	73 def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.):
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	74 self.prototypeTrajectory = prototypeTrajectory
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	75 self.constantSpeed = constantSpeed
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	76 self.probability = probability
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	77 self.predictedPositions = {0: initialPosition}
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	78 self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	79
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	80 def predictPosition(self, nTimeSteps):
a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	81 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	82 if self.constantSpeed:
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	83 # calculate cumulative distance
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	84 speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	85 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	86 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	87 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
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	88
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	89 else: # see c++ code, calculate ratio
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	90 speedNorm= self.predictedSpeedOrientations[0].norm
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	91 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	92 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	93 ratio=float(speedNorm)/prototypeSpeeds[0]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	94 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	95 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	96 if nTimeSteps<len(resampledSpeeds):
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	97 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
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	98 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	99 else:
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	100 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	101 self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	102
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	103 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	104
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	105 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	106 '''Random vehicle control: suitable for normal adaptation'''
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	107 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	108 '''Constructor
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	109 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	110 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	111 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	112 self.steeringDistribution = steeringDistribution
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	113 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	114 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	115 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	116 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	117
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	118 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	119 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	120
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	121 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	122 '''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	123 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	124 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	125 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	126 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	127 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	128 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	129
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	130 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	131 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	132
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	133 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	134 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	135 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	136 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	137
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	138 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	139 def computeExpectedIndicator(points):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	140 from numpy import sum
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	141 return sum([p.indicator*p.probability for p in points])/sum([p.probability 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	142
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	143 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	144 '''Computes the first instant
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	145 at which two predicted trajectories are within some distance threshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	146 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	147
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	148 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	149 t = 1
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	150 p1 = predictedTrajectory1.predictPosition(t)
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	151 p2 = predictedTrajectory2.predictPosition(t)
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	152 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	153 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	154 t += 1
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	155 p1 = predictedTrajectory1.predictPosition(t)
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	156 p2 = predictedTrajectory2.predictPosition(t)
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	157 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	158 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	159
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	160 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	161 from matplotlib.pyplot import figure, axis, title, close, savefig
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	162 figure()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	163 for et in predictedTrajectories1:
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	164 et.predictPosition(timeHorizon)
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	165 et.plot('rx')
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	166
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	167 for et in predictedTrajectories2:
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	168 et.predictPosition(timeHorizon)
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	169 et.plot('bx')
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	170 obj1.plot('r')
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	171 obj2.plot('b')
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	172 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	173 axis('equal')
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	174 savefig('predicted-trajectories-t-{0}.png'.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	175 close()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	176
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	177 def calculateProbability(nMatching,similarity,objects):
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	178 sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	179 prototypeProbability={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	180 for i in similarity.keys():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	181 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	182 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	183 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	184 for i in prototypeProbability.keys():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	185 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	186 return probabilities
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	187
306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	188 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	189 ''' 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	190 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	191 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	192 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	193 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	194 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	195 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	196 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	197 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	198 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	199 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	200 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	201 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	202 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	203 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	204 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	205
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	206 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	207 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	208 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	209 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	210 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	211 keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	212 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	213 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	214 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	215 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	216 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	217
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	218 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	219 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	220 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	221 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	222 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	223 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	224 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	225 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	226 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	227 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	228 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	229 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	230 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	231 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	232 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	233 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	234 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	235 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	236 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	237
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 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	239 for i in similarity.keys():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	240 if i in secondStepPrototypes.keys():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	241 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	242 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	243 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	244 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	245
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	246 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	247 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	248 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	249 if useSpeedPrototype:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	250 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	251 else:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	252 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	253 return prototypeTrajectories
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	254
629 0a5e89d6fc62 minor cleaning Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 diff changeset	255 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
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	256 '''returns the lists of collision points and crossing zones'''
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	257 if usePrototypes:
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	258 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
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	259 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
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	260 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
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 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	262 else:
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, 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	264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	265
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	266 collisionPoints = []
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	267 crossingZones = []
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	268 for et1 in predictedTrajectories1:
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	269 for et2 in predictedTrajectories2:
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	270 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
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	271
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	272 if collision:
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	273 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
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	274 elif computeCZ: # check if there is a crossing zone
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	275 # TODO? zone should be around the points at which the traj are the closest
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	276 # look for CZ at different times, otherwise it would be a collision
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	277 # an approximation would be to look for close points at different times, ie the complementary of collision points
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	278 cz = None
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	279 t1 = 0
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	280 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	281 t2 = 0
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	282 while not cz and t2 < timeHorizon:
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	283 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	284 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	285 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	286 if cz is not None:
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	287 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	288 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	289 t2 += 1
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	290 t1 += 1
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	291
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	292 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	293 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	294
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 return currentInstant, collisionPoints, crossingZones
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	296
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	297
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	298 class PredictionParameters(object):
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	299 def __init__(self, name, maxSpeed):
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	300 self.name = name
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	301 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	302
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	303 def __str__(self):
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	304 return '{0} {1}'.format(self.name, self.maxSpeed)
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	305
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	306 def generatePredictedTrajectories(self, obj, instant):
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	307 return []
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	308
651 335f6641bf7a changed the paramter usePrototypes to False by default Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	309 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	310 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
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	311
651 335f6641bf7a changed the paramter usePrototypes to False by default Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	312 def computeCrossingsCollisions(self, obj1, obj2, 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):
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	313 #def computeCrossingsCollisions(predictionParams, obj1, obj2, 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):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	314 '''Computes all crossing and collision points at each common instant for two road users. '''
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	315 collisionPoints={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	316 crossingZones={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	317 if timeInterval:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	318 commonTimeInterval = timeInterval
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	319 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	320 commonTimeInterval = obj1.commonTimeInterval(obj2)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	321 if nProcesses == 1:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	322 if usePrototypes:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
692 9a258687af4c corrected some errors for ttc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	324 commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
9a258687af4c corrected some errors for ttc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	325 commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	326 for i in commonTimeIntervalList2:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	327 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	328 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	329 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	330 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	331 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	332 if collisionPoints!={} or crossingZones!={}:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	333 for i in commonTimeIntervalList1:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	334 if i not in commonTimeIntervalList2:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	335 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	336 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	337 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	338 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	339 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	340 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	341 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	342 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	343 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	344 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	345 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	346 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	347 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	348 from multiprocessing import Pool
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	349 pool = Pool(processes = nProcesses)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	350 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	351 #results = [j.get() for j in jobs]
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	352 #results.sort()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	353 for j in jobs:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	354 i, cp, cz = j.get()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	355 #if len(cp) != 0 or len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	356 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	357 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	358 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	359 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	360 pool.close()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	361 return collisionPoints, crossingZones
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	362 #return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
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	363
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	364 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	365 '''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	366 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	367 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	368 if timeInterval:
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	369 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	370 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	371 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	372 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	373 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	374 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	375 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	376 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	377 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	378 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	379 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	380 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	381 # 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	382 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	383 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	384
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 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	386 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	387
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 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	389
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	390 class ConstantPredictionParameters(PredictionParameters):
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	391 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	392 PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	393
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	394 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	395 return [PredictedTrajectoryConstant(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	396 maxSpeed = self.maxSpeed)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	397
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	398 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	399 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	400 '''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	401 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	402 '''
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	403 if useFeatures:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	404 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	405 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	406 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	407 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	408 self.nPredictedTrajectories = nPredictedTrajectories
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	409 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	410 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	411 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	412
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	413 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	414 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	415 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	416 self.maxSteering)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	417
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	418 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	419 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	420 if self.useFeatures and obj.hadFeatures():
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	421 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	422 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	423 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	424 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	425 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	426 velocities = [obj.getVelocityAtInstant(instant)]
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	427 for i in xrange(self.nPredictedTrajectories):
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	428 for initialPosition,initialVelocity in zip(positions, velocities):
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	429 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	430 initialVelocity,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	431 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	432 self.steeringDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	433 maxSpeed = self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	434 return predictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	435
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	436 class PointSetPredictionParameters(PredictionParameters):
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	437 # todo generate several trajectories with normal adaptatoins from each position (feature)
489 000bddf84ad0 corrected bugs in safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	438 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	439 PredictionParameters.__init__(self, 'point set', maxSpeed)
489 000bddf84ad0 corrected bugs in safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	440 #self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	441
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	442 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	443 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	444 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	445 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	446 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	447 velocities = [f.getVelocityAtInstant(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	448 #for i in xrange(self.nPredictedTrajectories):
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 for initialPosition,initialVelocity in zip(positions, velocities):
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	450 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity,
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	451 maxSpeed = self.maxSpeed))
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	452 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	453 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	454 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	455 return None
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	456
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	457 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	458 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	459 '''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	460 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	461
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	462 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	463 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	464 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	465 name = 'evasive action'
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	466 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	467 self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	468 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	469 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	470 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	471
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	472 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	473 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	474
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	475 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	476 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	477 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	478 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	479 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	480 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	481 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	482 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	483 velocities = [obj.getVelocityAtInstant(instant)]
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	484 for i in xrange(self.nPredictedTrajectories):
267 32e88b513f5c added code to compute probability of collision Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 266 diff changeset	485 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	486 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350 7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	487 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	488 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	489 self.steeringDistribution()),
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	490 maxSpeed = self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	491 return predictedTrajectories
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	492
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	493
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	494 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	495 '''Prediction parameters of prediction at constant velocity
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	496 using direct computation of the intersecting point'''
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	497
387 91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 359 diff changeset	498 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	499 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	500
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	501 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	502 collisionPoints = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	503 crossingZones = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	504
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	505 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	506 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	507 if (p1-p2).norm2() <= collisionDistanceThreshold:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	508 collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)]
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	509 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	510 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	511 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	512 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	513
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	514 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	515 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	516 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	517 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	518 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	519 #print dot1, dot2
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	520 # (computeCZ and (dot1 > 0 or dot2 > 0)) or (
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	521 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	522 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	523 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	524 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	525 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	526 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	527 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	528 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	529 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	530
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	531 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	532 if computeCZ:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
387 91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 359 diff changeset	534 else:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	536
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	537 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	538 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	539 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	540 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	541 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	542 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	543 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	544 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	545 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	546 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	547
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	548 return currentInstant, collisionPoints, crossingZones
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	549
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	550 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	551 '''Prediction parameters of prediction at constant velocity
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	552 using direct computation of the intersecting point (solving for the equation'''
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	553
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	554 def __init__(self):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	555 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None)
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	556
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	557 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	558 'TODO add collision point coordinates, compute pPET'
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	559 #collisionPoints = []
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	560 #crossingZones = []
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	561
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	562 p1 = obj1.getPositionAtInstant(currentInstant)
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	563 p2 = obj2.getPositionAtInstant(currentInstant)
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	564 v1 = obj1.getVelocityAtInstant(currentInstant)
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	565 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	566 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	567
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	568 if intersection is not None:
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	569 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	570 if ttc:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	571 return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5)
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	572 else:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	573 return [],[]
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	574
357 e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 355 diff changeset	575 ####
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 355 diff changeset	576 # Other Methods
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 355 diff changeset	577 ####
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	578 class PrototypePredictionParameters(PredictionParameters):
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	579 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	580 name = 'prototype'
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	581 PredictionParameters.__init__(self, name, maxSpeed)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	582 self.nPredictedTrajectories = nPredictedTrajectories
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	583 self.constantSpeed = constantSpeed
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	584
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	585 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	586 predictedTrajectories = []
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	587 initialPosition = obj.getPositionAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	588 initialVelocity = obj.getVelocityAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	589 for prototypeTraj in prototypeTrajectories.keys():
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	590 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj]))
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	591 return predictedTrajectories
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	592
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	593 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	594 import doctest
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	595 import unittest
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	596 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	597 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	598 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	599 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	600 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	601

Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 692:9a258687af4c dev