Mercurial Hosting > traffic-intelligence
changeset 1260:158eee1aeb21
correcting bug before interpolation
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 15 Apr 2024 11:29:31 -0400 |
parents | 3bfdb2ffd29d |
children | 28aeec1f2788 |
files | trafficintelligence/storage.py |
diffstat | 1 files changed, 26 insertions(+), 57 deletions(-) [+] |
line wrap: on
line diff
--- 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]