--- a/trafficintelligence/storage.py	Fri Mar 22 14:33:25 2024 -0400
+++ b/trafficintelligence/storage.py	Mon Mar 25 17:05:20 2024 -0400
@@ -1395,8 +1395,7 @@
                 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])                
             # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
         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)
+            objects.append(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)]))
             featureNum += 4
     
     # ego vehicle
@@ -1434,15 +1433,17 @@
         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 = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])
-    objects.append(newObj)
+    objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+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)]))
 
     return objects
 
-def loadTrajectoriesFromKITTI2D(filename):
+def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None):
     '''Loads x,y coordinate series
     e.g. obtained by projecting from image to ground world plane using a homography
-    Format: frame_id,track_id,usertype,x,y,score '''
+    Format: frame_id,track_id,usertype,x,y,score 
+
+    if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
+    to generate the movement of the ego vehicle'''
 
     header = ['frame','trackingid','usertype','x','y','score']
     data = read_csv(filename, delimiter=' ', names = header)
@@ -1459,9 +1460,36 @@
             #print(tmp.info())
         if interval.length()>1:
             t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()])
-            newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType)
-            objects.append(newObj)
+            objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType))
         
+    # ego vehicle
+    if kittiCalibration is not None and oxts is not None:
+        invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
+        transCam2Velo = transpose(kittiCalibration['Tr_cam_velo'])
+        t = moving.Trajectory()
+        featureTrajectories = [moving.Trajectory() for i in range(4)]
+        interval = moving.TimeInterval(0, len(oxts)-1)#int(data.frame.min()), int(data.frame.max()))
+        #from pykitti.utils import roty
+        from trafficintelligence.cvutils import cartesian2Homogeneous
+        for _, Tr_imu_to_world in oxts:
+            #R = roty(pi/2)
+            #corners3d = dot(R, [0,0,0])
+            #print(corners3d)
+            #refCorners = transpose(dot(invR0, corners3d))
+            #print(invR0, refCorners) # 000
+            #homRefCorners = cartesian2Homogeneous(array([refCorners]))
+            veloCorners = transCam2Velo[3, :] # dot(homRefCorners, transCam2Velo)
+            #print(transCam2Velo, veloCorners) # last column of transCam2Velo
+            homVeloCorners = cartesian2Homogeneous(veloCorners)
+            imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3
+            #print(kittiCalibration['Tr_velo_imu'], imuCorners)
+            homImuCorners = cartesian2Homogeneous(imuCorners)
+            worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
+            #print(Tr_imu_to_world, worldCorners)
+            xCoords = worldCorners[:4,0]
+            yCoords = worldCorners[:4,1]
+            t.addPositionXY(xCoords.mean(), yCoords.mean())
+        objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car'))
     return objects
     
 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):