nsaunier/traffic-intelligence
correcting bug before interpolation
Commit 158eee1aeb21 · Nicolas Saunier · 2024-04-15 11:29 -0400
Comments
No comments yet.
Diff
diff --git a/trafficintelligence/storage.py b/trafficintelligence/storage.py
--- a/trafficintelligence/storage.py
+++ b/trafficintelligence/storage.py
@@ -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]