# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1679000598 14400
# Node ID 059b7282aa096fa0ef10309be8af2436bc8ce946
# Parent  28bf2420c412c2bfac240e4d0c6846b4212a8741
first version of kitti loader

diff -r 28bf2420c412 -r 059b7282aa09 trafficintelligence/storage.py
--- 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