--- a/trafficintelligence/storage.py	Tue Apr 09 15:21:55 2024 -0400
+++ b/trafficintelligence/storage.py	Mon Apr 15 11:29:31 2024 -0400
@@ -1271,50 +1271,6 @@
     inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3])
     return inv_Tr
 
-def compute3Dbox(anno_list):
-    #### LiDAR coordinate, rotate with y
-    '''.DS_Store
-        ^ z   
-        |
-        |
-        |
-        . - - - - - - - > x
-       /
-      /
-     /
-    v y
-    
-    '''
-    from pykitti.utils import rotz
-
-    R = rotz(anno_list['heading_3d'])
-    l = anno_list['l_3d']
-    w = anno_list['w_3d']
-    h = anno_list['h_3d']
-
-    x_3d = anno_list['x_3d']
-    y_3d = anno_list['y_3d']
-    z_3d = anno_list['z_3d']
-
-    # # 3d bounding box corners x,y,z as center of the 3d bbox
-    # #            0     1      2    3      4     5    6     7
-    x_corners = [l/2,  l/2, -l/2, -l/2,  l/2,  l/2, -l/2, -l/2]
-    y_corners = [w/2, -w/2, -w/2,  w/2,  w/2, -w/2, -w/2,  w/2]
-    z_corners = [h/2,  h/2,  h/2,  h/2, -h/2, -h/2, -h/2, -h/2]
-
-    # 3d bounding box corners x,y,z as center of the bottom surface of the 3d bbox, 
-    # the difference occurs at z_corners, as it rotates with z_corners.
-    # x_corners = [l/2,  l/2, -l/2, -l/2,  l/2,  l/2, -l/2, -l/2]
-    # y_corners = [w/2, -w/2, -w/2,  w/2,  w/2, -w/2, -w/2,  w/2]
-    # z_corners = [h,  h,  h,  h, 0, 0, 0, 0]   
-    corners_3d = dot(R, vstack([x_corners, y_corners, z_corners]))
-    # add the center
-    corners_3d[0, :] = corners_3d[0, :] + x_3d
-    corners_3d[1, :] = corners_3d[1, :] + y_3d
-    corners_3d[2, :] = corners_3d[2, :] + z_3d
-    
-    return transpose(corners_3d)
-
 def loadKITTICalibration(filename):
     '''Loads KITTI calibration data'''
     calib = {}
@@ -1355,7 +1311,7 @@
     oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
 
     Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt'''
-    from pykitti.utils import roty
+    from pykitti.utils import roty, rotz
     from trafficintelligence.cvutils import cartesian2Homogeneous
 
     if kittiCalibration is not None:
@@ -1394,7 +1350,7 @@
         if len(tmp) != interval.length(): #interpolate
             print(objNum, len(tmp), interval.length())
             instants = set(interval).difference(tmp.frame)
-            missing = concat([tmp[header[10:]], DataFrame([[t]+[NaN]*(len(header)-10) for t in instants], columns = ['frame']+header[10:])], ignore_index=True).sort_values('frame')
+            missing = concat([tmp[['frame']+header[10:]], DataFrame([[t]+[NaN]*(len(header)-10) for t in instants], columns = ['frame']+header[10:])], ignore_index=True).sort_values('frame')
             tmp = missing.interpolate()
         featureTrajectories = [moving.Trajectory() for i in range(4)]
         for i, r in tmp.iterrows():
@@ -1435,17 +1391,30 @@
                 homImuCorners = cartesian2Homogeneous(imuCorners)
                 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
             else: #LUMPI format
-                anno_list_temp = {}
-                #anno_list_temp['x_2d'] = r.xmin
-                #anno_list_temp['y_2d'] = r.ymin
-                anno_list_temp['l_3d'] = r.l
-                anno_list_temp['w_3d'] = r.w
-                anno_list_temp['h_3d'] = r.h
-                anno_list_temp['x_3d'] = r.x
-                anno_list_temp['y_3d'] = r.y
-                anno_list_temp['z_3d'] = r.z
-                anno_list_temp['heading_3d'] = r.ry
-                worldCorners = compute3Dbox(anno_list_temp)
+                '''
+                ^ z   
+                |
+                |
+                |
+                . - - - - - - - > x
+                /
+                /
+                /
+                v y
+                
+                '''
+                R = rotz(r.ry)
+                # # 3d bounding box corners x,y,z as center of the 3d bbox
+                # #            0     1      2    3      4     5    6     7
+                x_corners = [r.l/2,  r.l/2, -r.l/2, -r.l/2,  r.l/2,  r.l/2, -r.l/2, -r.l/2]
+                y_corners = [r.w/2, -r.w/2, -r.w/2,  r.w/2,  r.w/2, -r.w/2, -r.w/2,  r.w/2]
+                z_corners = [r.h/2,  r.h/2,  r.h/2,  r.h/2, -r.h/2, -r.h/2, -r.h/2, -r.h/2]
+                corners3d = dot(R, vstack([x_corners, y_corners, z_corners]))
+                # add the center
+                corners3d[0, :] = corners3d[0, :] + r.x
+                corners3d[1, :] = corners3d[1, :] + r.y
+                corners3d[2, :] = corners3d[2, :] + r.z
+                worldCorners = transpose(corners3d)
             # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0]
             xCoords = worldCorners[:4,0]
             yCoords = worldCorners[:4,1]