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

annotate trafficintelligence/prediction.py @ 1104:1c59091853e0

generalization of motion prediction methods to curvilinear trajectories

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 11 Mar 2019 15:41:05 -0400
parents	c6cf75a2ed08
children	2795d0e114c9

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
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	4 import math, random
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	5 from copy import copy
1029 c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 diff changeset	6
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	7 import numpy as np
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	8
1029 c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 diff changeset	9 from trafficintelligence import moving
c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 diff changeset	10 from trafficintelligence.utils import LCSS
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	11
15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	12 class PredictedTrajectory(object):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	13 '''Class for predicted trajectories with lazy evaluation
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	14 if the predicted position has not been already computed, compute it
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	15
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	16 it should also have a probability'''
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	17
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	18 def __init__(self):
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	19 self.probability = 0.
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	20 self.predictedPositions = {}
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	21 self.predictedSpeedOrientations = {}
300 f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	22 #self.collisionPoints = {}
f65b828e5521 working on trajectory simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 296 diff changeset	23 #self.crossingZones = {}
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 257 diff changeset	24
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	25 def predictPosition(self, nTimeSteps):
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	26 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	27 self.predictPosition(nTimeSteps-1)
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	28 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	29 return self.predictedPositions[nTimeSteps]
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	30
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	31 def getPredictedTrajectory(self):
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	32 return moving.Trajectory.fromPointList(list(self.predictedPositions.values()))
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	33
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	34 def getPredictedSpeeds(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	35 return [so.norm for so in self.predictedSpeedOrientations.values()]
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	36
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	37 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	38 self.getPredictedTrajectory().plot(options, withOrigin, timeStep, **kwargs)
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	39
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	40 class PredictedTrajectoryConstant(PredictedTrajectory):
bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	41 '''Predicted trajectory at constant speed or acceleration
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	42 TODO generalize by passing a series of velocities/accelerations'''
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	43
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	44 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1., maxSpeed = None):
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	45 self.control = control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	46 self.maxSpeed = maxSpeed
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	47 self.probability = probability
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	48 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	49 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	50
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	51 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	52 return self.control
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	53
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	54 class PredictedTrajectoryPrototype(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	55 '''Predicted trajectory that follows a prototype trajectory
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	56 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	57 1. an observed trajectory (extracted from video)
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	58 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	59
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	60 Prediction can be done
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	61 1. at constant speed (the instantaneous user speed)
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	62 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	63 (applying a constant ratio equal
08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	64 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	65
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	66 def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, nFramesIgnore = 3, probability = 1.):
940 d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	67 ''' prototype is a MovingObject
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	68
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	69 Prediction at constant speed will not work for unrealistic trajectories
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	70 that do not follow a slowly changing velocity (eg moving object trajectories,
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	71 but is good for realistic motion (eg features)'''
d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	72 self.prototype = prototype
467 08b67c9baca2 finished description and constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 466 diff changeset	73 self.constantSpeed = constantSpeed
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	74 self.nFramesIgnore = nFramesIgnore
467 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}
940 d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	77 self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	78 self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx) #should be computed in relative coordinates to position
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	79 self.theta = prototype.getVelocityAt(self.closestPointIdx).angle()
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	80 self.initialSpeed = initialVelocity.norm2()
942 ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	81 if not constantSpeed:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	82 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	83
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	84 def predictPosition(self, nTimeSteps):
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	85 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	86 deltaPosition = copy(self.deltaPosition)
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	87 if self.constantSpeed:
940 d8ab183a7351 verified motion prediction with prototypes at constant speed (test needed) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	88 traj = self.prototype.getPositions()
942 ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	89 trajLength = traj.length()
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	90 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	91 i = self.closestPointIdx
942 ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	92 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	93 i += 1
942 ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	94 if i == trajLength:
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	95 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	96 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	97 else:
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	98 v = self.prototype.getVelocityAt(min(i-1, int(self.prototype.length())-1-self.nFramesIgnore))
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	99 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
942 ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	100 else:
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	101 traj = self.prototype.getPositions()
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	102 trajLength = traj.length()
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	103 nSteps = self.ratio*nTimeSteps+self.closestPointIdx
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	104 i = int(np.floor(nSteps))
ab13aaf41432 implemented motion prediction with prototypes at constant ratio, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 940 diff changeset	105 if nSteps < trajLength-1:
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	106 v = self.prototype.getVelocityAt(min(i, int(self.prototype.length())-1-self.nFramesIgnore))
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	107 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	108 else:
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	109 v = self.prototype.getVelocityAt(-1-self.nFramesIgnore)
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	110 self.predictedPositions[nTimeSteps] = deltaPosition.rotate(v.angle()-self.theta)+traj[-1]+v*(nSteps-trajLength+1)
470 a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	111 return self.predictedPositions[nTimeSteps]
a84b9ba9631f small progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 467 diff changeset	112
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	113 class PredictedTrajectoryRandomControl(PredictedTrajectory):
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	114 '''Random vehicle control: suitable for normal adaptation'''
336 124f85c6cfae modifed default probability to float Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 300 diff changeset	115 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	116 '''Constructor
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	117 accelerationDistribution and steeringDistribution are distributions
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	118 that return random numbers drawn from them'''
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	119 self.accelerationDistribution = accelerationDistribution
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	120 self.steeringDistribution = steeringDistribution
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	121 self.maxSpeed = maxSpeed
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	122 self.probability = probability
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	123 self.predictedPositions = {0: initialPosition}
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	124 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	125
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	126 def getControl(self):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	127 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	128
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	129 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	130 '''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	131 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	132 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	133 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	134 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	135 self.probability = probability
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	136 self.indicator = indicator
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	137
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	138 def __str__(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	139 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	140
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	141 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	142 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	143 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	144 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	145
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	146 @staticmethod
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	147 def computeExpectedIndicator(points):
696 ae137e3b1990 minor correction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 692 diff changeset	148 return np.sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	149
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	150 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	151 '''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	152 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	153 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	154
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	155 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	156 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	157 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	158 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	159 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	160 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	161 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	162 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	163 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	164 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	165 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	166
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	167 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	168 from matplotlib.pyplot import figure, axis, title, clf, savefig
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	169 if printFigure:
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	170 clf()
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	171 else:
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	172 figure()
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	173 for et in predictedTrajectories1:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	174 for t in range(int(np.round(timeHorizon))):
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	175 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	176 et.plot('rx')
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	177 for et in predictedTrajectories2:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	178 for t in range(int(np.round(timeHorizon))):
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	179 et.predictPosition(t)
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	180 et.plot('bx')
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	181 obj1.plot('r', withOrigin = True)
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	182 obj2.plot('b', withOrigin = True)
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	183 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	184 axis('equal')
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	185 if printFigure:
84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	186 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
557 b91f33e098ee refactored some more code in compute crossing and collisions (parallel code works) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 556 diff changeset	187
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	188 def calculateProbability(nMatching,similarity,objects):
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	189 sumFrequencies=sum([nMatching[p] for p in similarity])
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	190 prototypeProbability={}
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	191 for i in similarity:
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	192 prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	193 sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability])
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	194 probabilities={}
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	195 for i in prototypeProbability:
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	196 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	197 return probabilities
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	198
306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	199 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	200 ''' 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	201 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	202 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	203 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	204 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	205 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	206 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	207 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	208 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	209 for y in prototypesRoutes:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	210 if y in prototypes:
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	211 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	212 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	213 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	214 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	215 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	216
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	217 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	218 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	219 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	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 mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	222 keys=[k for k in similarity if similarity[k] in mostMatchedValues]
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	223 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	224 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	225 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	226 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	227 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	228
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	229 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	230 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	231 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	232 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	233 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	234 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	235 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	236 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	237 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	238 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	239 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	240 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	241 for y in prototypesRoutes:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	242 if y in prototypes:
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	243 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	244 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	245 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	246 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	247 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	248
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 613 diff changeset	249 newSimilarity={}
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	250 for i in similarity:
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	251 if i in secondStepPrototypes:
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 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	253 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	254 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	255 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	256
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	257 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	258 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	259 partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	260 if useSpeedPrototype:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	261 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	262 else:
613 306db0f3c7a2 move 4 functions from trajLearning file MohamedGomaa parents: 611 diff changeset	263 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	264 return prototypeTrajectories
610 0dc36203973d remove dublicated code for collision/crossing computations MohamedGomaa parents: 607 diff changeset	265
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	266
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	267 class PredictionParameters(object):
1104 1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	268 def __init__(self, name, maxSpeed, useCurvilinear = False):
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	269 self.name = name
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	270 self.maxSpeed = maxSpeed
1104 1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	271 self.useCurvilinear = useCurvilinear
266 aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	272
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	273 def __str__(self):
aba9711b3149 small modificatons and reorganization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 265 diff changeset	274 return '{0} {1}'.format(self.name, self.maxSpeed)
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	275
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	276 def generatePredictedTrajectories(self, obj, instant):
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	277 return None
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	278
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	279 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	280 '''returns the lists of collision points and crossing zones'''
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	281 predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	282 predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant)
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	283
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	284 collisionPoints = []
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	285 if computeCZ:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	286 crossingZones = []
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	287 else:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	288 crossingZones = None
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	289 for et1 in predictedTrajectories1:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	290 for et2 in predictedTrajectories2:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	291 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	292 if collision:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	293 collisionPoints.append(SafetyPoint((p1+p2)0.5, et1.probabilityet2.probability, t))
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	294 elif computeCZ: # check if there is a crossing zone
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	295 # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	296 cz = None
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	297 t1 = 0
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	298 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	299 t2 = 0
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	300 while not cz and t2 < timeHorizon:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	301 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	302 if cz is not None:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	303 deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	304 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	305 t2 += 1
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	306 t1 += 1
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	307
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	308 if debug:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	309 savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	310
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	311 return collisionPoints, crossingZones
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	312
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	313 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1):
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	314 '''Computes all crossing and collision points at each common instant for two road users. '''
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	315 collisionPoints = {}
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	316 if computeCZ:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	317 crossingZones = {}
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	318 else:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	319 crossingZones = None
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	320 if timeInterval 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	321 commonTimeInterval = timeInterval
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	322 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	323 commonTimeInterval = obj1.commonTimeInterval(obj2)
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	324 #if nProcesses == 1:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	325 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	326 cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	327 if len(cp) != 0:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	328 collisionPoints[i] = cp
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	329 if computeCZ and len(cz) != 0:
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	330 crossingZones[i] = cz
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	331 return collisionPoints, crossingZones
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	332
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	333 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	334 '''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	335 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	336 collisionProbabilities = {}
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	337 if timeInterval is not None:
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	338 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	339 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	340 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	341 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	342 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	343 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	344 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	345 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	346 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	347 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	348 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	349 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	350 # 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	351 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	352 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	353
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	354 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	355 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	356
c41ff9f3c263 moved current method for collision points and crossing zones computation into prediction parameters (put expectedindicator in SafetyPoint) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	357 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	358
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	359 class ConstantPredictionParameters(PredictionParameters):
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	360 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	361 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	362
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	363 def generatePredictedTrajectories(self, obj, instant):
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	364 return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), maxSpeed = self.maxSpeed)]
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	365
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	366 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	367 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	368 '''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	369 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	370 '''
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	371 if useFeatures:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	372 name = 'point set normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	373 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	374 name = 'normal adaptation'
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	375 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	376 self.nPredictedTrajectories = nPredictedTrajectories
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	377 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	378 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	379 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	380
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	381 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	382 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	383 self.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	384 self.maxSteering)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	385
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	386 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	387 predictedTrajectories = []
860 07c5eab11eba fixing bug, thanks to Ryan Louie <Ryan.Louie@students.olin.edu> Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 826 diff changeset	388 if self.useFeatures and obj.hasFeatures():
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	389 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	390 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	391 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	392 else:
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	393 positions = [obj.getPositionAtInstant(instant)]
72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	394 velocities = [obj.getVelocityAtInstant(instant)]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	395 probability = 1./float(len(positions)*self.nPredictedTrajectories)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	396 for i in range(self.nPredictedTrajectories):
352 72aa44072093 safety analysis script with option for prediction method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 350 diff changeset	397 for initialPosition,initialVelocity in zip(positions, velocities):
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	398 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	399 initialVelocity,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	400 self.accelerationDistribution,
e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	401 self.steeringDistribution,
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	402 probability,
466 e891a41c6c75 name change in prediction.py Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	403 maxSpeed = self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	404 return predictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	405
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	406 class PointSetPredictionParameters(PredictionParameters):
489 000bddf84ad0 corrected bugs in safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	407 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	408 PredictionParameters.__init__(self, 'point set', maxSpeed)
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	409
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	410 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	411 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	412 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	413 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	414 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	415 velocities = [f.getVelocityAtInstant(instant) for f in features]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	416 probability = 1./float(len(positions))
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	417 for initialPosition,initialVelocity in zip(positions, velocities):
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	418 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, probability = probability, maxSpeed = self.maxSpeed))
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 651 diff changeset	419 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	420 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	421 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	422 return None
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	423
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	424
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	425 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	426 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	427 '''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	428 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	429
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	430 if useFeatures:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	431 name = 'point set evasive action'
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	432 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	433 name = 'evasive action'
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	434 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	435 self.nPredictedTrajectories = nPredictedTrajectories
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	436 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	437 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	438 self.steeringDistribution = steeringDistribution
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	439
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	440 def __str__(self):
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	441 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	442
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	443 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	444 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	445 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	446 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	447 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	448 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	449 else:
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	450 positions = [obj.getPositionAtInstant(instant)]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 267 diff changeset	451 velocities = [obj.getVelocityAtInstant(instant)]
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	452 probability = 1./float(self.nPredictedTrajectories)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	453 for i in range(self.nPredictedTrajectories):
267 32e88b513f5c added code to compute probability of collision Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 266 diff changeset	454 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	455 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition,
350 7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	456 initialVelocity,
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	457 moving.NormAngle(self.accelerationDistribution(),
7e9ad2d9d79c added new parameters in safety analysis script Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 336 diff changeset	458 self.steeringDistribution()),
826 8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	459 probability,
8b74a5176549 explicitly computed the probabilities for predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 701 diff changeset	460 self.maxSpeed))
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	461 return predictedTrajectories
257 9281878ff19e untested collision/crossing computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	462
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	463
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	464 class CVDirectPredictionParameters(PredictionParameters):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	465 '''Prediction parameters of prediction at constant velocity
698 8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	466 using direct computation of the intersecting point
8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	467 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	468
387 91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 359 diff changeset	469 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	470 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	471
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	472 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	473 collisionPoints = []
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	474 if computeCZ:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	475 crossingZones = []
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	476 else:
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	477 crossingZones = None
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	478
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	479 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	480 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	481 if (p1-p2).norm2() <= collisionDistanceThreshold:
985 668a85c963c3 work on processing and managing large video datasets Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	482 collisionPoints = [SafetyPoint((p1+p2)*0.5, 1., 0.)]
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	483 else:
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	484 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	485 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	486 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	487
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	488 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	489 dp1 = intersection-p1
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	490 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	491 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	492 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	493 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	494 dist1 = dp1.norm2()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	495 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	496 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	497 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	498 halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	499 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	500 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	501 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	502
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	503 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	504 if computeCZ:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 629 diff changeset	505 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	506 else:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	507 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	508
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	509 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	510 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	511 figure()
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	512 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	513 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	514 intersection.plot()
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	515 obj1.plot('r')
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 489 diff changeset	516 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	517 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	518 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	519
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	520 return collisionPoints, crossingZones
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	521
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	522 class CVExactPredictionParameters(PredictionParameters):
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	523 '''Prediction parameters of prediction at constant velocity
698 8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	524 using direct computation of the intersecting point (solving the equation)
8d99a9e16644 added clarification comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 696 diff changeset	525 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 460 diff changeset	526
1104 1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	527 def __init__(self, useCurvilinear = False):
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	528 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear)
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 358 diff changeset	529
701 4cc56ff82c3c corrected bug for prediction at constant velocity, exact computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 698 diff changeset	530 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 860 diff changeset	531 'TODO compute pPET'
8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 860 diff changeset	532 collisionPoints = []
8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 860 diff changeset	533 crossingZones = []
269 a9988971aac8 removed legacy code + tweaks Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	534
1104 1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	535 if self.useCurvilinear:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	536 pass # Lionel
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	537 else:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	538 p1 = obj1.getPositionAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	539 p2 = obj2.getPositionAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	540 v1 = obj1.getVelocityAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	541 v2 = obj2.getVelocityAtInstant(currentInstant)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	542 #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	543
1104 1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	544 if not moving.Point.parallel(v1, v2):
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	545 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	546 if ttc is not None:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	547 collisionPoints = [SafetyPoint((p1+(v1ttc)+p2+(v2ttc))*0.5, 1., ttc)]
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	548 else:
1c59091853e0 generalization of motion prediction methods to curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	549 pass # compute pPET
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 860 diff changeset	550
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 985 diff changeset	551 return collisionPoints, crossingZones
260 36cb40c51a5e modified the organization of the code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	552
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	553 class PrototypePredictionParameters(PredictionParameters):
944 84ebe1b031f1 works with object trajectory, features todo Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	554 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	555 PredictionParameters.__init__(self, 'prototypes', None)
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	556 self.prototypes = prototypes
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	557 self.nPredictedTrajectories = nPredictedTrajectories
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	558 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	559 self.minSimilarity = minSimilarity
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	560 self.minFeatureTime = minFeatureTime
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	561 self.constantSpeed = constantSpeed
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	562 self.useFeatures = useFeatures
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	563
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	564 def getLcss(self):
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	565 return self.lcss
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	566
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	567 def addPredictedTrajectories(self, predictedTrajectories, obj, instant):
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	568 obj.computeTrajectorySimilarities(self.prototypes, self.lcss)
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	569 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	570 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	571 initialPosition = obj.getPositionAtInstant(instant)
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	572 initialVelocity = obj.getVelocityAtInstant(instant)
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 948 diff changeset	573 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings()))
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	574
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	575 def generatePredictedTrajectories(self, obj, instant):
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	576 predictedTrajectories = []
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	577 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	578 if self.useFeatures and obj.hasFeatures():
946 e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	579 if not hasattr(obj, 'currentPredictionFeatures'):
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	580 obj.currentPredictionFeatures = []
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	581 else:
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	582 obj.currentPredictionFeatures[:] = [f for f in obj.currentPredictionFeatures if f.existsAtInstant(instant)]
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	583 firstInstants = [(f,f.getFirstInstant()) for f in obj.getFeatures() if f.existsAtInstant(instant) and f not in obj.currentPredictionFeatures]
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	584 firstInstants.sort(key = lambda t: t[1])
946 e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	585 for f,t1 in firstInstants[:min(self.nPredictedTrajectories, len(firstInstants), self.nPredictedTrajectories-len(obj.currentPredictionFeatures))]:
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	586 obj.currentPredictionFeatures.append(f)
e5970606066f bug fix on list filtering (cannot remove while iterating) and motion prediction keeping the same features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	587 for f in obj.currentPredictionFeatures:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	588 self.addPredictedTrajectories(predictedTrajectories, f, instant)
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 942 diff changeset	589 else:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 944 diff changeset	590 self.addPredictedTrajectories(predictedTrajectories, obj, instant)
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 571 diff changeset	591 return predictedTrajectories
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 240 diff changeset	592
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	593 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	594 import doctest
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	595 import unittest
271 bbd9c09e6869 changed the names to prediction methods and predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 270 diff changeset	596 suite = doctest.DocFileSuite('tests/prediction.txt')
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	597 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	598 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	599 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	600 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	601

Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/prediction.py @ 1104:1c59091853e0