Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/storage.py @ 1203:7b3384a8e409
second version of code loading kitti data, to clean
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 22 Mar 2023 15:40:33 -0400 |
parents | 059b7282aa09 |
children | a12d126346ff |
comparison
equal
deleted
inserted
replaced
1202:059b7282aa09 | 1203:7b3384a8e409 |
---|---|
1349 'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] | 1349 'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] |
1350 data = read_csv(filename, delimiter = ' ', names = header) | 1350 data = read_csv(filename, delimiter = ' ', names = header) |
1351 data = data[data.trackingid > -1] | 1351 data = data[data.trackingid > -1] |
1352 | 1352 |
1353 objects = [] | 1353 objects = [] |
1354 featureNum = 0 | |
1354 for objNum in data.trackingid.unique(): | 1355 for objNum in data.trackingid.unique(): |
1355 tmp = data[data.trackingid == objNum].sort_values('frame') | 1356 tmp = data[data.trackingid == objNum].sort_values('frame') |
1356 t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]]) | 1357 t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]]) |
1358 featureTrajectories = [moving.Trajectory() for i in range(4)] | |
1357 for i, r in tmp.iterrows(): | 1359 for i, r in tmp.iterrows(): |
1358 _, Tr_imu2w = oxts[r.frame] | 1360 _, Tr_imu2w = oxts[r.frame] |
1359 transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), | 1361 # transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), |
1360 'R_rect': kittiCalibration['R_rect'], | 1362 # 'R_rect': kittiCalibration['R_rect'], |
1361 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'], | 1363 # 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'], |
1362 # transfer_matrix['Tr_velo_to_cam'] = kittiCalibration['Tr_velo_to_cam'] | 1364 # # transfer_matrix['Tr_velo_to_cam'] = kittiCalibration['Tr_velo_to_cam'] |
1363 'Tr_velo_to_imu': kittiCalibration['Tr_velo_imu'], | 1365 # 'Tr_velo_to_imu': kittiCalibration['Tr_velo_imu'], |
1364 # transfer_matrix['Tr_imu_to_velo'] = kittiCalibration['Tr_imu_to_velo'] | 1366 # # transfer_matrix['Tr_imu_to_velo'] = kittiCalibration['Tr_imu_to_velo'] |
1365 'Tr_imu_to_world': Tr_imu2w} | 1367 # 'Tr_imu_to_world': Tr_imu2w} |
1366 # 3D object | 1368 # 3D object |
1367 # compute rotational matrix around yaw axis | 1369 # compute rotational matrix around yaw axis |
1368 R = roty(r.ry) | 1370 R = roty(r.ry) |
1369 | 1371 |
1370 # 3d bounding box corners | 1372 # 3d bounding box corners |
1378 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] | 1380 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] |
1379 # corners3d = transpose(corners3d) | 1381 # corners3d = transpose(corners3d) |
1380 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) | 1382 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) |
1381 veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) | 1383 veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) |
1382 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix) | 1384 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix) |
1383 Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu'] | 1385 #Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu'] |
1384 Tr_imu_to_world = transfer_matrix['Tr_imu_to_world'] | 1386 #Tr_imu_to_world = transfer_matrix['Tr_imu_to_world'] |
1385 homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1)) | 1387 homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1)) |
1386 homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column | 1388 homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column |
1387 imuCorners = dot(Tr_velo_to_imu, homVeloCorners.T).T # 8x3 | 1389 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 |
1388 | 1390 |
1389 homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1)) | 1391 homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1)) |
1390 homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column | 1392 homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column |
1391 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 | 1393 worldCorners = dot(Tr_imu2w, homImuCorners.T).T # 8x3 |
1392 | 1394 |
1393 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] | 1395 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] |
1394 xCoords = worldCorners[:4,0] | 1396 xCoords = worldCorners[:4,0] |
1395 yCoords = worldCorners[:4,1] | 1397 yCoords = worldCorners[:4,1] |
1396 t.addPositionXY(xCoords.mean(), yCoords.mean()) | 1398 t.addPositionXY(xCoords.mean(), yCoords.mean()) |
1399 for i in range(4): | |
1400 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) | |
1397 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c | 1401 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c |
1398 objects.append(moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), | 1402 newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = t, userType = tmp.iloc[0].type, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = featureTrajectories[i]) for i in range(4)]) |
1399 positions = t, | 1403 #newObj.featureNumbers = [f.getNum() for f in newObj.getFeatures()] |
1400 userType = tmp.iloc[0].type)) | 1404 objects.append(newObj) |
1405 featureNum += 4 | |
1401 return objects | 1406 return objects |
1402 | 1407 |
1403 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): | 1408 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): |
1404 '''Reads data from the trajectory data provided by NGSIM project | 1409 '''Reads data from the trajectory data provided by NGSIM project |
1405 and converts to our current format.''' | 1410 and converts to our current format.''' |