--- a/trafficintelligence/storage.py	Tue Feb 28 17:16:49 2023 -0500
+++ b/trafficintelligence/storage.py	Thu Mar 16 17:03:18 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
+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
 from pandas import read_csv, merge
 
 from trafficintelligence import utils, moving, events, indicators
@@ -1328,7 +1328,7 @@
     Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt'''
     from pykitti.utils import roty
 
-    invR0 = np.linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
+    invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
     
     header = ['frame', # 0, 1, ..., n
               'trackingid', # -1, 0 , 1, ..., k
@@ -1349,11 +1349,11 @@
               'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
     data = read_csv(filename, delimiter = ' ', names = header)
     data = data[data.trackingid > -1]
+
+    objects = []
     for objNum in data.trackingid.unique():
         tmp = data[data.trackingid == objNum].sort_values('frame')
-        obj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), 
-                                  positions = moving.Trajectory([[float(numbers[6])],[float(numbers[7])]]), 
-                                  userType = tmp.iloc[0].type)
+        t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]])
         for i, r in tmp.iterrows():
             _, Tr_imu2w = oxts[r.frame]
             transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)),
@@ -1378,12 +1378,27 @@
             corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2]
             # corners3d = transpose(corners3d)
             # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
-            cornersVelo = transpose(dot(invR0, corners3d)) # avoid double transpose np.transpose(pts_3d_rect)))
+            veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect)))
             #             boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
+            Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu']
+            Tr_imu_to_world = transfer_matrix['Tr_imu_to_world']
+            homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1))
+            homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column
+            imuCorners = dot(Tr_velo_to_imu, homVeloCorners.T).T # 8x3
 
-            # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
+            homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1))
+            homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column
+            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())
+            # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
+        objects.append(moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()),
+                                           positions = t,
+                                           userType = tmp.iloc[0].type))
+    return objects
 
 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):
     '''Reads data from the trajectory data provided by NGSIM project