Mercurial Hosting > traffic-intelligence

changeset 289:e56c34c1ebac

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 e0d41c7f53d4
children df58d361f19e
files python/prediction.py python/tests/prediction.txt
diffstat 2 files changed, 22 insertions(+), 13 deletions(-) [+]
line wrap: on
line diff
--- a/python/prediction.py	Tue Jan 29 10:36:17 2013 -0500
+++ b/python/prediction.py	Tue Jan 29 11:21:42 2013 -0500
@@ -167,16 +167,20 @@
         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)
@@ -186,12 +190,12 @@
         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)
-    predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, i)
+    predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant)
+    predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant)
 
     collisionPoints = []
     crossingZones = []
@@ -200,7 +204,7 @@
             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
@@ -235,7 +239,8 @@
 
     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:
@@ -244,13 +249,13 @@
         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)
-        SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i)
-        SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i)
+        collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug)
 
     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
@@ -268,8 +273,7 @@
                     nCollisions += 1
         # take into account probabilities ??
         nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
-        collisionProbabilities[i] = float(nCollisions)/nSamples
-        out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, nSamples, i, collisionProbabilities[i]))
+        collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
 
         if debug:
             from matplotlib.pyplot import figure, axis, title
--- a/python/tests/prediction.txt	Tue Jan 29 10:36:17 2013 -0500
+++ b/python/tests/prediction.txt	Tue Jan 29 11:21:42 2013 -0500
@@ -23,3 +23,8 @@
 >>> from numpy import max
 >>> max(et.getPredictedSpeeds()) <= 2.
 True
+
+>>> p = moving.Point(3,4)
+>>> sp = prediction.SafetyPoint(p, 0.1, 0)
+>>> print(sp)
+3 4 0.1 0