changeset 259:8ab76b95ee72

added code to save collision points and crossing zones in txt files
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 24 Jul 2012 15:18:12 -0400
parents d90be3c02267
children 36cb40c51a5e
files python/extrapolation.py python/moving.py python/storage.py
diffstat 3 files changed, 21 insertions(+), 12 deletions(-) [+]
line wrap: on
line diff
--- a/python/extrapolation.py	Tue Jul 24 12:37:47 2012 -0400
+++ b/python/extrapolation.py	Tue Jul 24 15:18:12 2012 -0400
@@ -76,19 +76,18 @@
         print('Unknown extrapolation hypothesis')
         return []
 
-class CollisionPoint(moving.Point):
-    def __init__(self, p, probability = 1., TTC = -1):
+class SafetyPoint(moving.Point):
+    '''Can represent a collision point or crossing zone 
+    with respective safety indicator, TTC or pPET'''
+    def __init__(self, p, probability = 1., indicator = -1):
         self.x = p.x
         self.y = p.y
         self.probability = probability
-        self.TTC = TTC
+        self.indicator = indicator
 
-class CrossingZone(moving.Point):
-    def __init__(self, p, probability = 1., pPET = -1):
-        self.x = p.x
-        self.y = p.y
-        self.probability = probability
-        self.pPET = pPET
+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 computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
     '''returns the lists of collision points and crossing zones '''
@@ -106,7 +105,7 @@
                 t += 1
 
             if t <= timeHorizon:
-                collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
+                collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), 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
@@ -120,7 +119,7 @@
                         #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
                         cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
                         if cz:
-                            crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2)))
+                            crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
                         t2 += 1
                     t1 += 1                        
     return collisionPoints, crossingZones
--- a/python/moving.py	Tue Jul 24 12:37:47 2012 -0400
+++ b/python/moving.py	Tue Jul 24 15:18:12 2012 -0400
@@ -240,7 +240,7 @@
         from math import atan2
         norm = p.norm2()
         if norm > 0:
-            angle = atan2(p.x, p.y)
+            angle = atan2(p.y, p.x)
         return NormAngle(norm, angle)
 
     def __add__(self, other):
--- a/python/storage.py	Tue Jul 24 12:37:47 2012 -0400
+++ b/python/storage.py	Tue Jul 24 15:18:12 2012 -0400
@@ -12,6 +12,16 @@
                   'car':2,
                   'truck':3}
 
+#########################
+# txt files
+#########################
+
+
+
+#########################
+# Sqlite
+#########################
+
 def writeTrajectoriesToSqlite(objects, outFile, trajectoryType, objectNumbers = -1):
     """
     This function writers trajectories to a specified sqlite file