--- 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):