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

annotate python/prediction.py @ 664:455f9b93819c

added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Wed, 20 May 2015 12:04:22 +0200
parents	72174e66aba5
children	15e244d2a1b5

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):
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	143 '''Computes the first instant
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	144 at which two predicted trajectories are within some distance threshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	145 Computes all the times including timeHorizon
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	146
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	147 User has to check the first variable collision to know about a collision'''
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	148 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	149 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	150 p2 = predictedTrajectory2.predictPosition(t)
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	151 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	152 while t < timeHorizon and not collision:
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	153 t += 1
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	154 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	155 p2 = predictedTrajectory2.predictPosition(t)
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	156 collision = (p1-p2).norm2() <= collisionDistanceThreshold
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	157 return collision, t, p1, p2
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	158
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	159 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	160 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	161 figure()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	162 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	163 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	164 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	165
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	166 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	167 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	168 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	169 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	170 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	171 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	172 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	173 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	174 close()
b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	175
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	176 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	177 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	178 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	179 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	180 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	181 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	182 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	183 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	184 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	185 return probabilities
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	186
306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	187 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	188 ''' 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	189 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	190 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	191 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	192 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	193 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	194 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	195 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	196 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	197 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	198 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	199 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	200 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	201 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	202 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	203 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	204
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the 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 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	206 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	207 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	208 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	209 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	210 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	211 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	212 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	213 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	214 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	215 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	216
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	217 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	218 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	219 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	220 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	221 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	222 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	223 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	224 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	225 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	226 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	227 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	228 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	229 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	230 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	231 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	232 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	233 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	234 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	235 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	236
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the 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 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	238 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	239 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	240 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	241 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	242 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	243 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	244
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	245 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	246 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	247 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	248 if useSpeedPrototype:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	249 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	250 else:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	251 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	252 return prototypeTrajectories
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	253
629 0a5e89d6fc62 minor cleaning Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 diff changeset	254 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	255 '''returns the lists of collision points and crossing zones'''
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	256 if usePrototypes:
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	257 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	258 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	259 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	260 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	261 else:
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	262 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	263 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	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 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	266 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	267 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	268 for et2 in predictedTrajectories2:
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	269 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	270
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	271 if collision:
556 dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	272 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	273 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	274 # 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	275 # 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	276 # 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	277 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	278 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	279 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	280 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	281 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	282 #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	283 # 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	284 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	285 if cz is not None:
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	286 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	287 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	288 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	289 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	290
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	291 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	292 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	293
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	294 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	295
dc58ad777a72 modified prediction for multiprocessing, not sure how beneficial it is (single thread with instance method seems much faster Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	296
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	297 class PredictionParameters:
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	298 def __init__(self, name, maxSpeed):
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	299 self.name = name
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	300 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	301
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	302 def __str__(self):
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	303 return '{0} {1}'.format(self.name, self.maxSpeed)
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	304
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	305 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	306 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	307
651 335f6641bf7a changed the paramter usePrototypes to False by default Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	308 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	309 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	310
651 335f6641bf7a changed the paramter usePrototypes to False by default Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	311 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	312 #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	313 '''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	314 collisionPoints={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	315 crossingZones={}
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	316 if timeInterval:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	317 commonTimeInterval = timeInterval
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	318 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	319 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	320 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	321 if usePrototypes:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	322 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	323 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	324 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	325 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	326 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	327 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	328 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	329 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	330 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	331 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	332 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	333 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	334 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	335 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	336 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	337 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	338 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	339 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	340 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	341 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	342 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	343 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	344 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	345 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	346 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	347 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	348 pool = Pool(processes = nProcesses)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	349 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	350 #results = [j.get() for j in jobs]
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	351 #results.sort()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	352 for j in jobs:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	353 i, cp, cz = j.get()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	354 #if len(cp) != 0 or len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	355 if len(cp) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	356 collisionPoints[i] = cp
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	357 if len(cz) != 0:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	358 crossingZones[i] = cz
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	359 pool.close()
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	360 return collisionPoints, crossingZones
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	361 #return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	362
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	363 def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	364 '''Computes only collision probabilities
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	365 Returns for each instant the collision probability and number of samples drawn'''
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	366 collisionProbabilities = {}
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	367 if timeInterval:
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	368 commonTimeInterval = timeInterval
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	369 else:
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	370 commonTimeInterval = obj1.commonTimeInterval(obj2)
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	371 for i in list(commonTimeInterval)[:-1]:
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	372 nCollisions = 0
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	373 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i)
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	374 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i)
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	375 for et1 in predictedTrajectories1:
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	376 for et2 in predictedTrajectories2:
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	377 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	378 if collision:
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	379 nCollisions += 1
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	380 # take into account probabilities ??
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	381 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	382 collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	383
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	384 if debug:
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	385 savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
358 c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	386
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	387 return collisionProbabilities
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	388
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	389 class ConstantPredictionParameters(PredictionParameters):
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	390 def __init__(self, maxSpeed):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	391 PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	392
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	393 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	394 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	395 maxSpeed = self.maxSpeed)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	396
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	397 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	398 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	399 '''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	400 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	401 '''
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	402 if useFeatures:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	403 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	404 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	405 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	406 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	407 self.nPredictedTrajectories = nPredictedTrajectories
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	408 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	409 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	410 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	411
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	412 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	413 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	414 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	415 self.maxSteering)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	416
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	417 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	418 predictedTrajectories = []
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	419 if self.useFeatures and obj.hadFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	420 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	421 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	422 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	423 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	424 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	425 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	426 for i in xrange(self.nPredictedTrajectories):
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	427 for initialPosition,initialVelocity in zip(positions, velocities):
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	428 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	429 initialVelocity,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	430 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	431 self.steeringDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	432 maxSpeed = self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	433 return predictedTrajectories
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 class PointSetPredictionParameters(PredictionParameters):
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	436 # 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	437 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	438 PredictionParameters.__init__(self, 'point set', maxSpeed)
489 000bddf84ad0 corrected bugs in safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	439 #self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	440
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	441 def generatePredictedTrajectories(self, obj, instant):
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	442 predictedTrajectories = []
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	443 if obj.hasFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	444 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	445 positions = [f.getPositionAtInstant(instant) for f in features]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	446 velocities = [f.getVelocityAtInstant(instant) for f in features]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	447 #for i in xrange(self.nPredictedTrajectories):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	448 for initialPosition,initialVelocity in zip(positions, velocities):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	449 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity,
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	450 maxSpeed = self.maxSpeed))
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	451 return predictedTrajectories
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	452 else:
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	453 print('Object {} has no features'.format(obj.getNum()))
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	454 return None
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	455
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	456 class EvasiveActionPredictionParameters(PredictionParameters):
460 55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	457 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	458 '''Suggested acceleration distribution may not be symmetric, eg
55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	459 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)'''
55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	460
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	461 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	462 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	463 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	464 name = 'evasive action'
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	465 PredictionParameters.__init__(self, name, maxSpeed)
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	466 self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	467 self.useFeatures = useFeatures
460 55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	468 self.accelerationDistribution = accelerationDistribution
55b424d98b68 change of interface, distributions are now passed to the prediction paramters constructors if needed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 387 diff changeset	469 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	470
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	471 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	472 return PredictionParameters.__str__(self)+' {0} {1} {2} {3}'.format(self.nPredictedTrajectories, self.minAcceleration, self.maxAcceleration, self.maxSteering)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	473
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	474 def generatePredictedTrajectories(self, obj, instant):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	475 predictedTrajectories = []
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	476 if self.useFeatures and obj.hasFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	477 features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
267 32e88b513f5c added code to compute probability of collision Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 266 diff changeset	478 positions = [f.getPositionAtInstant(instant) for f in features]
32e88b513f5c added code to compute probability of collision Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 266 diff changeset	479 velocities = [f.getVelocityAtInstant(instant) for f in features]
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	480 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	481 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	482 velocities = [obj.getVelocityAtInstant(instant)]
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	483 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	484 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	485 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350 7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	486 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	487 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	488 self.steeringDistribution()),
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	489 maxSpeed = self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	490 return predictedTrajectories
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	491
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	492
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	493 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	494 '''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	495 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	496
387 91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 359 diff changeset	497 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	498 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	499
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	500 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	501 collisionPoints = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	502 crossingZones = []
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	503
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	504 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	505 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	506 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	507 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	508 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	509 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	510 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	511 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	512
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	513 if intersection is not None:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	514 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	515 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	516 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	517 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	518 #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	519 # (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	520 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	521 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	522 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	523 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	524 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	525 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	526 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	527 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	528 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	529
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	530 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	531 if computeCZ:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	532 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	533 else:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	534 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	535 # 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	536 # 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	537 # 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	538 # 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	539 # 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	540 # firstUser = obj1
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	541 # secondUser = obj2
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	542 # 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	543 # 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	544 # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	545 # if indices is not None:
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	546 # pass
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	547 # 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	548
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	549 if debug and intersection is not None:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	550 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	551 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	552 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	553 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	554 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	555 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	556 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	557 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	558 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	559
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	560 return currentInstant, collisionPoints, crossingZones
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	561
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	562 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	563 '''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	564 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	565
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	566 def __init__(self):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	567 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	568
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	569 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	570 '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	571 #collisionPoints = []
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	572 #crossingZones = []
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	573
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	574 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	575 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	576 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	577 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	578 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	579
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	580 if intersection is not None:
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	581 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	582 if ttc:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	583 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	584 else:
6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 470 diff changeset	585 return [],[]
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	586
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	587 ####
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	588 # 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	589 ####
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	590 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	591 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	592 name = 'prototype'
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	593 PredictionParameters.__init__(self, name, maxSpeed)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	594 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	595 self.constantSpeed = constantSpeed
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	596
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	597 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	598 predictedTrajectories = []
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	599 initialPosition = obj.getPositionAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	600 initialVelocity = obj.getVelocityAtInstant(instant)
84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	601 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	602 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	603 return predictedTrajectories
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	604
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	605 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	606 import doctest
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	607 import unittest
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	608 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	609 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	610 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	611 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	612 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	613

Mercurial Hosting > traffic-intelligence

annotate python/prediction.py @ 664:455f9b93819c