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

annotate python/prediction.py @ 826:8b74a5176549

explicitly computed the probabilities for predicted trajectories

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 27 Jun 2016 23:27:05 -0400
parents	4cc56ff82c3c
children	07c5eab11eba

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
696 ae137e3b1990 minor correction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 692 diff changeset	9 from multiprocessing import Pool
665 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
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	12 class PredictedTrajectory(object):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	13 '''Class for predicted trajectories with lazy evaluation
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	14 if the predicted position has not been already computed, compute it
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	15
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	16 it should also have a probability'''
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	17
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	18 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	19 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	20 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	21 self.predictedSpeedOrientations = {}
300 f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	22 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	23 #self.crossingZones = {}
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	24
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	25 def predictPosition(self, nTimeSteps):
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	26 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	27 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	28 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	29 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	30
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	31 def getPredictedTrajectory(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	32 return moving.Trajectory.fromPointList(self.predictedPositions.values())
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	33
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	34 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	35 return [so.norm for so in self.predictedSpeedOrientations.values()]
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	36
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	37 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	38 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	39
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	40 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	41 '''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	42 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	43
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	44 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	45 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	46 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	47 self.probability = probability
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	48 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	49 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	50
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	51 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	52 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	53
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	54 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	55 ''' 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	56 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	57 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	58 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	59 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	60 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	61
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	62 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	63 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	64 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	65 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	66 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	67
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	68 Prediction can be done
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	69 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	70 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	71 (applying a constant ratio equal
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	72 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	73
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	74 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	75 self.prototypeTrajectory = prototypeTrajectory
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	76 self.constantSpeed = constantSpeed
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	77 self.probability = probability
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	78 self.predictedPositions = {0: initialPosition}
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	79 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	80
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	81 def predictPosition(self, nTimeSteps):
a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	82 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	83 if self.constantSpeed:
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	84 # calculate cumulative distance
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	85 speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	86 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	87 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	88 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	89
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	90 else: # see c++ code, calculate ratio
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	91 speedNorm= self.predictedSpeedOrientations[0].norm
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	92 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	93 prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	94 ratio=float(speedNorm)/prototypeSpeeds[0]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	95 resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	96 anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	97 if nTimeSteps<len(resampledSpeeds):
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	98 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	99 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	100 else:
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	101 self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	102 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	103
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	104 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	105
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	106 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	107 '''Random vehicle control: suitable for normal adaptation'''
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	108 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	109 '''Constructor
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	110 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	111 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	112 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	113 self.steeringDistribution = steeringDistribution
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	114 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	115 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	116 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	117 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	118
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	119 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	120 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	121
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	122 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	123 '''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	124 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	125 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	126 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	127 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	128 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	129 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	130
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	131 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	132 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	133
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	134 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	135 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	136 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	137 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	138
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	139 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	140 def computeExpectedIndicator(points):
696 ae137e3b1990 minor correction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 692 diff changeset	141 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	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:
696 ae137e3b1990 minor correction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 692 diff changeset	164 et.predictPosition(int(np.round(timeHorizon)))
557 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:
696 ae137e3b1990 minor correction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 692 diff changeset	168 et.predictPosition(int(np.round(timeHorizon)))
557 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 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	349 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	350 #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	351 #results.sort()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	352 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	353 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	354 #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	355 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	356 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	357 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	358 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	359 pool.close()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	360 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	361 #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	362
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 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	364 '''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	365 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	366 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	367 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	368 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	369 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	370 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	371 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	372 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	373 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	374 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	375 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	376 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	377 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	378 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	379 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	380 # 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	381 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	382 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	383
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 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	385 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	386
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 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	388
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	389 class ConstantPredictionParameters(PredictionParameters):
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	390 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	391 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	392
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	393 def generatePredictedTrajectories(self, obj, instant):
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	394 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), maxSpeed = self.maxSpeed)]
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	395
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	396 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	397 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	398 '''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	399 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	400 '''
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	401 if useFeatures:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	402 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	403 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	404 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	405 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	406 self.nPredictedTrajectories = nPredictedTrajectories
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	407 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	408 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	409 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	410
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	411 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	412 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	413 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	414 self.maxSteering)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	415
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	416 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	417 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	418 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	419 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	420 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	421 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	422 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	423 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	424 velocities = [obj.getVelocityAtInstant(instant)]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	425 probability = 1./float(len(positions)*self.nPredictedTrajectories)
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	426 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	427 for initialPosition,initialVelocity in zip(positions, velocities):
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	428 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	429 initialVelocity,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	430 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	431 self.steeringDistribution,
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	432 probability,
466 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]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	448 probability = 1./float(len(positions))
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	449 for initialPosition,initialVelocity in zip(positions, velocities):
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	450 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, probability = probability, maxSpeed = self.maxSpeed))
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	451 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	452 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	453 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	454 return None
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	455
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	456 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	457 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	458 '''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	459 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	460
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	461 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	462 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	463 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	464 name = 'evasive action'
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	465 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	466 self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	467 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	468 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	469 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	470
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	471 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	472 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	473
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	474 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	475 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	476 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	477 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	478 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	479 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	480 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	481 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	482 velocities = [obj.getVelocityAtInstant(instant)]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	483 probability = 1./float(self.nPredictedTrajectories)
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()),
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	490 probability,
8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	491 self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	492 return predictedTrajectories
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	493
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	494
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	495 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	496 '''Prediction parameters of prediction at constant velocity
698 8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	497 using direct computation of the intersecting point
8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	498 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	499
387 91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 359 diff changeset	500 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	501 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	502
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	503 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	504 collisionPoints = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	505 crossingZones = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	506
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	507 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	508 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	509 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	510 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	511 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	512 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	513 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	514 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	515
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	516 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	517 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	518 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	519 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	520 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	521 #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	522 # (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	523 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	524 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	525 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	526 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	527 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	528 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	529 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	530 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	531 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	532
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	533 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	534 if computeCZ:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	535 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	536 else:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	537 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	538
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	539 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	540 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	541 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	542 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	543 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	544 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	545 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	546 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	547 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	548 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	549
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	550 return currentInstant, collisionPoints, crossingZones
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	551
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	552 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	553 '''Prediction parameters of prediction at constant velocity
698 8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	554 using direct computation of the intersecting point (solving the equation)
8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	555 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	556
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	557 def __init__(self):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	558 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	559
701 4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 698 diff changeset	560 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	561 '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	562 #collisionPoints = []
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	563 #crossingZones = []
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	564
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	565 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	566 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	567 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	568 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	569 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	570
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	571 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	572 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	573 if ttc:
701 4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 698 diff changeset	574 return currentInstant, [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5)
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	575 else:
701 4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 698 diff changeset	576 return currentInstant, [],[]
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	577
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	578 ####
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	579 # 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	580 ####
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	581 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	582 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	583 name = 'prototype'
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	584 PredictionParameters.__init__(self, name, maxSpeed)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	585 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	586 self.constantSpeed = constantSpeed
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	587
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	588 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	589 predictedTrajectories = []
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	590 initialPosition = obj.getPositionAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	591 initialVelocity = obj.getVelocityAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	592 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	593 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	594 return predictedTrajectories
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	595
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	596 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	597 import doctest
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	598 import unittest
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	599 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	600 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	601 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	602 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	603 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	604

Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 826:8b74a5176549