Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/storage.py @ 1260:158eee1aeb21

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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):