--- a/trafficintelligence/moving.py	Tue May 02 07:12:26 2023 -0400
+++ b/trafficintelligence/moving.py	Tue May 02 17:11:24 2023 -0400
@@ -1828,7 +1828,7 @@
                         relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
                         relativePositions[(j,i)] = -relativePositions[(i,j)]
 
-    def computeBoundingPolygon(self, t, shape = 'raw'):
+    def getBoundingPolygon(self, t, shape = 'raw'):
         '''Returns a bounding box for the feature positions at instant
         bounding box format is a list of points
         shape is the type of output shape: 

--- a/trafficintelligence/prediction.py	Tue May 02 07:12:26 2023 -0400
+++ b/trafficintelligence/prediction.py	Tue May 02 17:11:24 2023 -0400
@@ -72,15 +72,25 @@
             self.predictedPositions[nTimeSteps] = self.predictedPositions[nTimeSteps-1]+self.initialVelocity
         return self.predictedPositions[nTimeSteps]
 
-class PredictedPolyTrajectoryConstant(PredictedTrajectory):
+class PredictedPolyTrajectoryConstantVelocity(PredictedTrajectory):
     '''Predicted trajectory of an object with an outline represented by a polygon
-    Simple method that just translates the polygon corners'''
+    Simple method that just translates the polygon corners (set of moving.Point)'''
     def __init__(self, polyCorners, initialVelocity, probability = 1.):
         self.probability = probability
-        self.predictedPositions = {0: polyCorners}
+        self.predictedPositions = {0: moving.pointsToShapely(polyCorners)}
         self.initialVelocity = initialVelocity
-        self.predictedTrajectories = [PredictedTrajectoryConstant(p, initialVelocity) for p in polyCorners]
-        
+        self.predictedTrajectories = [PredictedTrajectoryConstantVelocity(p, initialVelocity) for p in polyCorners]
+
+    def predictPosition(self, nTimeSteps):
+        if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions:
+            self.predictedPositions[nTimeSteps] = moving.pointsToShapely([t.predictPosition(nTimeSteps) for t in self.predictedTrajectories])
+        return self.predictedPositions[nTimeSteps]
+
+    def hasCollisionWith(self, other, t, collisionDistanceThreshold):
+        poly1 = self.predictPosition(t)
+        poly2 = other.predictPosition(t)
+        return poly1.overlaps(poly2), poly1, poly2
+
 class PredictedTrajectoryPrototype(PredictedTrajectory):
     '''Predicted trajectory that follows a prototype trajectory
     The prototype is in the format of a moving.Trajectory: it could be
@@ -196,23 +206,6 @@
         collision, p1, p2 = predictedTrajectory1.hasCollisionWith(predictedTrajectory2, t, collisionDistanceThreshold)
     return collision, t, p1, p2
 
-def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon):
-    '''Computes the first instant 
-    at which two objects represented by a series of points (eg box)
-    Computes all the times including timeHorizon
-    
-    User has to check the first variable collision to know about a collision'''
-    t = 1
-    poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
-    poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
-    collision = poly1.overlaps(poly2)
-    while t < timeHorizon and not collision:
-        t += 1
-        poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
-        poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
-        collision = poly1.overlaps(poly2)
-    return collision, t
-
 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
     from matplotlib.pyplot import figure, axis, title, clf, savefig
     if printFigure:
@@ -314,7 +307,7 @@
 
 
 class PredictionParameters(object):
-    def __init__(self, name, maxSpeed, useCurvilinear = False):
+    def __init__(self, name, maxSpeed = None, useCurvilinear = False):
         self.name = name
         self.maxSpeed = maxSpeed
         self.useCurvilinear = useCurvilinear
@@ -325,6 +318,9 @@
     def generatePredictedTrajectories(self, obj, instant):
         return None
 
+    def computeCollisionPoint(self, p1, p2, probability, t):
+        return SafetyPoint((p1+p2)*0.5, probability, t)
+
     def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
         '''returns the lists of collision points and crossing zones'''
         predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
@@ -339,7 +335,7 @@
             for et2 in predictedTrajectories2:
                 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
                 if collision:
-                    collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
+                    collisionPoints.append(self.computeCollisionPoint(p1, p2, et1.probability*et2.probability, t))
                 elif computeCZ: # check if there is a crossing zone
                     # TODO same computation as PET with metric + concatenate past trajectory with future trajectory
                     cz = None
@@ -412,6 +408,19 @@
     def generatePredictedTrajectories(self, obj, instant):
         return [PredictedTrajectoryConstantVelocity(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant))]
 
+class ConstantPolyPredictionParameters(PredictionParameters):
+    def __init__(self):
+        PredictionParameters.__init__(self, 'constant velocity for polygon representation')
+
+    def generatePredictedTrajectories(self, obj, instant):
+        return [PredictedPolyTrajectoryConstantVelocity(obj.getBoundingPolygon(instant), obj.getVelocityAtInstant(instant))]
+
+    def computeCollisionPoint(self, p1, p2, probability, t):
+        polyRepr1 = p1.representative_point()
+        polyRepr2 = p2.representative_point()
+        p = moving.Point(polyRepr1.x+polyRepr2.x, polyRepr1.y+polyRepr2.y)
+        return SafetyPoint(p, probability, t)
+    
 class NormalAdaptationPredictionParameters(PredictionParameters):
     def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
         '''An example of acceleration and steering distributions is
@@ -469,7 +478,6 @@
         else:
             print('Object {} has no features'.format(obj.getNum()))
             return None
-
         
 class EvasiveActionPredictionParameters(PredictionParameters):
     def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
@@ -509,7 +517,6 @@
                                                                          self.maxSpeed))
         return predictedTrajectories
 
-
 class CVDirectPredictionParameters(PredictionParameters):
     '''Prediction parameters of prediction at constant velocity
     using direct computation of the intersecting point

--- a/trafficintelligence/storage.py	Tue May 02 07:12:26 2023 -0400
+++ b/trafficintelligence/storage.py	Tue May 02 17:11:24 2023 -0400
@@ -7,7 +7,7 @@
 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
+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 trafficintelligence import utils, moving, events, indicators, cvutils
@@ -1385,6 +1385,47 @@
         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
+    
+    # ego vehicle
+    t = moving.Trajectory()
+    featureTrajectories = [moving.Trajectory() for i in range(4)]
+    interval = moving.TimeInterval(int(data.frame.min()), int(data.frame.max()))
+    R = roty(pi/2)
+    for frame in interval:
+        _, Tr_imu_to_world = oxts[frame]
+
+        l = 4.5 # m
+        w = 1.8 # m
+        h = 0.
+        # 3d bounding box corners
+        x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]
+        y_corners = [0, 0, 0, 0, -h, -h, -h, -h]
+        z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]
+        # rotate and translate 3d bounding box
+        corners3d = dot(R, vstack([x_corners, y_corners, z_corners]))
+        corners3d[0, :] = corners3d[0, :]
+        corners3d[1, :] = corners3d[1, :]
+        corners3d[2, :] = corners3d[2, :]
+        refCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) in pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
+        homRefCorners = cvutils.cartesian2Homogeneous(refCorners)
+        veloCorners = dot(homRefCorners, transCam2Velo)
+        homVeloCorners = cvutils.cartesian2Homogeneous(veloCorners)
+        imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3
+
+        homImuCorners = cvutils.cartesian2Homogeneous(imuCorners)
+        worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
+            
+        # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0]
+        xCoords = worldCorners[:4,0]
+        yCoords = worldCorners[:4,1]
+        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)])
+    objects.append(newObj)
+
     return objects
 
 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):

--- a/trafficintelligence/tests/events.txt	Tue May 02 07:12:26 2023 -0400
+++ b/trafficintelligence/tests/events.txt	Tue May 02 17:11:24 2023 -0400
@@ -34,7 +34,7 @@
 >>> o2 = MovingObject.generate(2, Point(0.,-5.), Point(0.,1.), TimeInterval(0,10))
 >>> inter = Interaction(roadUser1 = o1, roadUser2 = o2)
 >>> inter.computeIndicators()
->>> predictionParams = ConstantPredictionParameters(10.)
+>>> predictionParams = ConstantPredictionParameters()
 >>> inter.computeCrossingsCollisions(predictionParams, 0.1, 10)
 >>> ttc = inter.getIndicator("Time to Collision")
 >>> ttc[0]