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

annotate python/prediction.py @ 630:69a98f84f3eb

corrected major issue with pPET, only for CVDirect prediction for now

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 17 Feb 2015 02:21:31 +0100
parents	0a5e89d6fc62
children	3058e00887bc

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

Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 630:69a98f84f3eb