Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/storage.py @ 1260:158eee1aeb21
correcting bug before interpolation
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 15 Apr 2024 11:29:31 -0400 |
parents | e59a0a475a0a |
children | 28aeec1f2788 |
comparison
equal
deleted
inserted
replaced
1259:3bfdb2ffd29d | 1260:158eee1aeb21 |
---|---|
1269 inv_Tr = zeros_like(Tr) # 3x4 | 1269 inv_Tr = zeros_like(Tr) # 3x4 |
1270 inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3]) | 1270 inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3]) |
1271 inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3]) | 1271 inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3]) |
1272 return inv_Tr | 1272 return inv_Tr |
1273 | 1273 |
1274 def compute3Dbox(anno_list): | |
1275 #### LiDAR coordinate, rotate with y | |
1276 '''.DS_Store | |
1277 ^ z | |
1278 | | |
1279 | | |
1280 | | |
1281 . - - - - - - - > x | |
1282 / | |
1283 / | |
1284 / | |
1285 v y | |
1286 | |
1287 ''' | |
1288 from pykitti.utils import rotz | |
1289 | |
1290 R = rotz(anno_list['heading_3d']) | |
1291 l = anno_list['l_3d'] | |
1292 w = anno_list['w_3d'] | |
1293 h = anno_list['h_3d'] | |
1294 | |
1295 x_3d = anno_list['x_3d'] | |
1296 y_3d = anno_list['y_3d'] | |
1297 z_3d = anno_list['z_3d'] | |
1298 | |
1299 # # 3d bounding box corners x,y,z as center of the 3d bbox | |
1300 # # 0 1 2 3 4 5 6 7 | |
1301 x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2] | |
1302 y_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2] | |
1303 z_corners = [h/2, h/2, h/2, h/2, -h/2, -h/2, -h/2, -h/2] | |
1304 | |
1305 # 3d bounding box corners x,y,z as center of the bottom surface of the 3d bbox, | |
1306 # the difference occurs at z_corners, as it rotates with z_corners. | |
1307 # x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2] | |
1308 # y_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2] | |
1309 # z_corners = [h, h, h, h, 0, 0, 0, 0] | |
1310 corners_3d = dot(R, vstack([x_corners, y_corners, z_corners])) | |
1311 # add the center | |
1312 corners_3d[0, :] = corners_3d[0, :] + x_3d | |
1313 corners_3d[1, :] = corners_3d[1, :] + y_3d | |
1314 corners_3d[2, :] = corners_3d[2, :] + z_3d | |
1315 | |
1316 return transpose(corners_3d) | |
1317 | |
1318 def loadKITTICalibration(filename): | 1274 def loadKITTICalibration(filename): |
1319 '''Loads KITTI calibration data''' | 1275 '''Loads KITTI calibration data''' |
1320 calib = {} | 1276 calib = {} |
1321 with open(filename, 'r') as f: | 1277 with open(filename, 'r') as f: |
1322 for l in f: | 1278 for l in f: |
1353 | 1309 |
1354 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt | 1310 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt |
1355 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti | 1311 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti |
1356 | 1312 |
1357 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' | 1313 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' |
1358 from pykitti.utils import roty | 1314 from pykitti.utils import roty, rotz |
1359 from trafficintelligence.cvutils import cartesian2Homogeneous | 1315 from trafficintelligence.cvutils import cartesian2Homogeneous |
1360 | 1316 |
1361 if kittiCalibration is not None: | 1317 if kittiCalibration is not None: |
1362 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) | 1318 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) |
1363 transCam2Velo = transpose(kittiCalibration['Tr_cam_velo']) | 1319 transCam2Velo = transpose(kittiCalibration['Tr_cam_velo']) |
1392 interval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())) | 1348 interval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())) |
1393 userType = tmp.iloc[0].usertype | 1349 userType = tmp.iloc[0].usertype |
1394 if len(tmp) != interval.length(): #interpolate | 1350 if len(tmp) != interval.length(): #interpolate |
1395 print(objNum, len(tmp), interval.length()) | 1351 print(objNum, len(tmp), interval.length()) |
1396 instants = set(interval).difference(tmp.frame) | 1352 instants = set(interval).difference(tmp.frame) |
1397 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') | 1353 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') |
1398 tmp = missing.interpolate() | 1354 tmp = missing.interpolate() |
1399 featureTrajectories = [moving.Trajectory() for i in range(4)] | 1355 featureTrajectories = [moving.Trajectory() for i in range(4)] |
1400 for i, r in tmp.iterrows(): | 1356 for i, r in tmp.iterrows(): |
1401 if kittiCalibration is not None: | 1357 if kittiCalibration is not None: |
1402 _, Tr_imu_to_world = oxts[r.frame] | 1358 _, Tr_imu_to_world = oxts[r.frame] |
1433 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 | 1389 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 |
1434 | 1390 |
1435 homImuCorners = cartesian2Homogeneous(imuCorners) | 1391 homImuCorners = cartesian2Homogeneous(imuCorners) |
1436 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 | 1392 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 |
1437 else: #LUMPI format | 1393 else: #LUMPI format |
1438 anno_list_temp = {} | 1394 ''' |
1439 #anno_list_temp['x_2d'] = r.xmin | 1395 ^ z |
1440 #anno_list_temp['y_2d'] = r.ymin | 1396 | |
1441 anno_list_temp['l_3d'] = r.l | 1397 | |
1442 anno_list_temp['w_3d'] = r.w | 1398 | |
1443 anno_list_temp['h_3d'] = r.h | 1399 . - - - - - - - > x |
1444 anno_list_temp['x_3d'] = r.x | 1400 / |
1445 anno_list_temp['y_3d'] = r.y | 1401 / |
1446 anno_list_temp['z_3d'] = r.z | 1402 / |
1447 anno_list_temp['heading_3d'] = r.ry | 1403 v y |
1448 worldCorners = compute3Dbox(anno_list_temp) | 1404 |
1405 ''' | |
1406 R = rotz(r.ry) | |
1407 # # 3d bounding box corners x,y,z as center of the 3d bbox | |
1408 # # 0 1 2 3 4 5 6 7 | |
1409 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] | |
1410 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] | |
1411 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] | |
1412 corners3d = dot(R, vstack([x_corners, y_corners, z_corners])) | |
1413 # add the center | |
1414 corners3d[0, :] = corners3d[0, :] + r.x | |
1415 corners3d[1, :] = corners3d[1, :] + r.y | |
1416 corners3d[2, :] = corners3d[2, :] + r.z | |
1417 worldCorners = transpose(corners3d) | |
1449 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] | 1418 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] |
1450 xCoords = worldCorners[:4,0] | 1419 xCoords = worldCorners[:4,0] |
1451 yCoords = worldCorners[:4,1] | 1420 yCoords = worldCorners[:4,1] |
1452 t.addPositionXY(xCoords.mean(), yCoords.mean()) | 1421 t.addPositionXY(xCoords.mean(), yCoords.mean()) |
1453 for j in range(4): | 1422 for j in range(4): |