Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/storage.py @ 1203:7b3384a8e409

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