nsaunier/traffic-intelligence: python/prediction.py comparison

comparison python/prediction.py @ 289:e56c34c1ebac

refactored and commented functions (saving data is now outside of the computation functions)

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 29 Jan 2013 11:21:42 -0500
parents	bbd9c09e6869
children	df58d361f19e

comparison

equal deleted inserted replaced

-:e0d41c7f53d4
+:e56c34c1ebac
 self.x = p.x
 self.y = p.y
 self.probability = probability
 self.indicator = indicator
+def __str__(self):
+return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator)
 @staticmethod
-def save(out, points, objNum1, objNum2, instant):
+def save(out, points, predictionInstant, objNum1, objNum2):
 for p in points:
-out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator))
+out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
 def computeExpectedIndicator(points):
 from numpy import sum
 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
+'''Computes the first instant at which two predicted trajectories are within some distance threshold'''
 t = 1
 p1 = predictedTrajectory1.predictPosition(t)
 p2 = predictedTrajectory2.predictPosition(t)
 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
 p1 = predictedTrajectory1.predictPosition(t)
 p2 = predictedTrajectory2.predictPosition(t)
 t += 1
 return t, p1, p2
-def computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False):
+def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False):
 '''returns the lists of collision points and crossing zones
 Check: Predicting all the points together, as if they represent the whole vehicle'''
-predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, i)
+predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant)
-predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, i)
+predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant)
 collisionPoints = []
 crossingZones = []
 for et1 in predictedTrajectories1:
 for et2 in predictedTrajectories2:
 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
 if t <= timeHorizon:
-collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
+collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), currentInstant, et1.probability*et2.probability, t))
 else: # check if there is a crossing zone
 # TODO? zone should be around the points at which the traj are the closest
 # look for CZ at different times, otherwise it would be a collision
 # an approximation would be to look for close points at different times, ie the complementary of collision points
 cz = None
 title('instant {0}'.format(i))
 axis('equal')
 return collisionPoints, crossingZones
-def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None):
+def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
+'''Computes all crossing and collision points at each common instant for two road users. '''
 collisionPoints={}
 crossingZones={}
 if timeInterval:
 commonTimeInterval = timeInterval
 else:
 commonTimeInterval = obj1.commonTimeInterval(obj2)
 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
 print(obj1.num, obj2.num, i)
-collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug)
+collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug)
-SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i)
-SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i)
 return collisionPoints, crossingZones
-def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, out, debug = False, timeInterval = None):
+def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
+'''Computes only collision probabilities
+Returns for each instant the collision probability and number of samples drawn'''
 collisionProbabilities = {}
 if timeInterval:
 commonTimeInterval = timeInterval
 else:
 commonTimeInterval = obj1.commonTimeInterval(obj2)
 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
 if t <= timeHorizon:
 nCollisions += 1
 # take into account probabilities ??
 nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
-collisionProbabilities[i] = float(nCollisions)/nSamples
+collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
-out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, nSamples, i, collisionProbabilities[i]))
 if debug:
 from matplotlib.pyplot import figure, axis, title
 figure()
 for et in predictedTrajectories1:

Mercurial > hg > nsaunier > traffic-intelligence

comparison python/prediction.py @ 289:e56c34c1ebac