Mercurial Hosting > traffic-intelligence

changeset 1215:1b472cddf9b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
updated reading by interpolating missing frames
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 05 May 2023 22:52:25 -0400
parents 01c24c1cdb70
children 8356e15fd691
files trafficintelligence/prediction.py trafficintelligence/storage.py
diffstat 2 files changed, 27 insertions(+), 15 deletions(-) [+]
line wrap: on
line diff
--- a/trafficintelligence/prediction.py	Thu May 04 22:30:32 2023 -0400
+++ b/trafficintelligence/prediction.py	Fri May 05 22:52:25 2023 -0400
@@ -558,9 +558,9 @@
                     
                     if collisionTimeInterval.empty():
                         if computeCZ:
-                            crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
+                            crossingZones.append(SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2)))
                     else:
-                        collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
+                        collisionPoints.append(SafetyPoint(intersection, 1., collisionTimeInterval.center()))
     
         if debug and intersection is not None:
             from matplotlib.pyplot import plot, figure, axis, title
@@ -586,7 +586,10 @@
     def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
         'TODO compute pPET'
         collisionPoints = []
-        crossingZones = []
+        if computeCZ:
+            crossingZones = []
+        else:
+            crossingZones = None
 
         if self.useCurvilinear:
             pass # Lionel
@@ -600,7 +603,7 @@
             if not moving.Point.parallel(v1, v2):
                 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
                 if ttc is not None:
-                    collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)]
+                    collisionPoints.append(SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc))
                 else:
                     pass # compute pPET
         return collisionPoints, crossingZones
@@ -616,7 +619,10 @@
     def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
         'TODO compute pPET'
         collisionPoints = []
-        crossingZones = []
+        if computeCZ:
+            crossingZones = []
+        else:
+            crossingZones = None
 
         if self.useCurvilinear:
             pass # Lionel
@@ -629,7 +635,7 @@
             if not moving.Point.parallel(v1, v2):
                 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2)
                 if ttc is not None:
-                    collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)]
+                    collisionPoints.append(SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc))
                 else:
                     pass # compute pPET
         return collisionPoints, crossingZones
--- a/trafficintelligence/storage.py	Thu May 04 22:30:32 2023 -0400
+++ b/trafficintelligence/storage.py	Fri May 05 22:52:25 2023 -0400
@@ -7,8 +7,8 @@
 from copy import copy
 import sqlite3, logging
 
-from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose, ones, zeros_like, pi
-from pandas import read_csv, merge
+from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose, ones, zeros_like, pi, NaN
+from pandas import read_csv, merge, concat, DataFrame
 
 from trafficintelligence import utils, moving, events, indicators, cvutils
 from trafficintelligence.base import VideoFilenameAddable
@@ -1336,6 +1336,13 @@
     for objNum in data.trackingid.unique():
         tmp = data[data.trackingid == objNum].sort_values('frame')
         t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]])
+        interval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max()))
+        userType = tmp.iloc[0].usertype
+        if len(tmp) != interval.length(): #interpolate
+            print(objNum, len(tmp), interval.length())
+            instants = set(interval).difference(tmp.frame)
+            tmp = concat([tmp, DataFrame([[t, objNum, userType]+[NaN]*(len(header)-3) for t in instants], columns = header)], ignore_index=True).sort_values('frame')
+            tmp.interpolate(inplace=True)
         featureTrajectories = [moving.Trajectory() for i in range(4)]
         for i, r in tmp.iterrows():
             _, Tr_imu_to_world = oxts[r.frame]
@@ -1381,10 +1388,10 @@
             for i in range(4):
                 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])                
             # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
-        
-        newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = t, velocities = t.differentiate(True), userType = tmp.iloc[0].usertype, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])
-        objects.append(newObj)
-        featureNum += 4
+        if interval.length()>1:
+            newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType, features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])
+            objects.append(newObj)
+            featureNum += 4
     
     # ego vehicle
     t = moving.Trajectory()
@@ -1420,9 +1427,8 @@
         t.addPositionXY(xCoords.mean(), yCoords.mean())
         for i in range(4):
             featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])                
-            # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
-        
-    newObj = moving.MovingObject(num = objNum+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = interval, positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])
+            # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c        
+    newObj = moving.MovingObject(num = objNum+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])
     objects.append(newObj)
 
     return objects