Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/storage.py @ 1253:ef68d4ba7dae

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added loading ego vehicle in kitti 2D format and method to plot outline
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 25 Mar 2024 17:05:20 -0400
parents 2b1c8fe8f7e4
children a477ad82ab66
comparison
equal deleted inserted replaced
1252:fe35473acee3 1253:ef68d4ba7dae
1393 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1393 t.addPositionXY(xCoords.mean(), yCoords.mean())
1394 for i in range(4): 1394 for i in range(4):
1395 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) 1395 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])
1396 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1396 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1397 if interval.length()>1: 1397 if interval.length()>1:
1398 newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType, features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]) 1398 objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType, features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]))
1399 objects.append(newObj)
1400 featureNum += 4 1399 featureNum += 4
1401 1400
1402 # ego vehicle 1401 # ego vehicle
1403 t = moving.Trajectory() 1402 t = moving.Trajectory()
1404 featureTrajectories = [moving.Trajectory() for i in range(4)] 1403 featureTrajectories = [moving.Trajectory() for i in range(4)]
1432 yCoords = worldCorners[:4,1] 1431 yCoords = worldCorners[:4,1]
1433 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1432 t.addPositionXY(xCoords.mean(), yCoords.mean())
1434 for i in range(4): 1433 for i in range(4):
1435 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) 1434 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])
1436 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1435 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1437 newObj = moving.MovingObject(num = objNum+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]) 1436 objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]))
1438 objects.append(newObj)
1439 1437
1440 return objects 1438 return objects
1441 1439
1442 def loadTrajectoriesFromKITTI2D(filename): 1440 def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None):
1443 '''Loads x,y coordinate series 1441 '''Loads x,y coordinate series
1444 e.g. obtained by projecting from image to ground world plane using a homography 1442 e.g. obtained by projecting from image to ground world plane using a homography
1445 Format: frame_id,track_id,usertype,x,y,score ''' 1443 Format: frame_id,track_id,usertype,x,y,score
1444
1445 if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
1446 to generate the movement of the ego vehicle'''
1446 1447
1447 header = ['frame','trackingid','usertype','x','y','score'] 1448 header = ['frame','trackingid','usertype','x','y','score']
1448 data = read_csv(filename, delimiter=' ', names = header) 1449 data = read_csv(filename, delimiter=' ', names = header)
1449 objects = [] 1450 objects = []
1450 for objNum in data.trackingid.unique(): 1451 for objNum in data.trackingid.unique():
1457 missing = concat([tmp[['frame', 'x', 'y']], DataFrame([[t, NaN, NaN] for t in instants], columns = ['frame', 'x', 'y'])], ignore_index=True).sort_values('frame') 1458 missing = concat([tmp[['frame', 'x', 'y']], DataFrame([[t, NaN, NaN] for t in instants], columns = ['frame', 'x', 'y'])], ignore_index=True).sort_values('frame')
1458 tmp = missing.interpolate() 1459 tmp = missing.interpolate()
1459 #print(tmp.info()) 1460 #print(tmp.info())
1460 if interval.length()>1: 1461 if interval.length()>1:
1461 t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()]) 1462 t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()])
1462 newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType) 1463 objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType))
1463 objects.append(newObj)
1464 1464
1465 # ego vehicle
1466 if kittiCalibration is not None and oxts is not None:
1467 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
1468 transCam2Velo = transpose(kittiCalibration['Tr_cam_velo'])
1469 t = moving.Trajectory()
1470 featureTrajectories = [moving.Trajectory() for i in range(4)]
1471 interval = moving.TimeInterval(0, len(oxts)-1)#int(data.frame.min()), int(data.frame.max()))
1472 #from pykitti.utils import roty
1473 from trafficintelligence.cvutils import cartesian2Homogeneous
1474 for _, Tr_imu_to_world in oxts:
1475 #R = roty(pi/2)
1476 #corners3d = dot(R, [0,0,0])
1477 #print(corners3d)
1478 #refCorners = transpose(dot(invR0, corners3d))
1479 #print(invR0, refCorners) # 000
1480 #homRefCorners = cartesian2Homogeneous(array([refCorners]))
1481 veloCorners = transCam2Velo[3, :] # dot(homRefCorners, transCam2Velo)
1482 #print(transCam2Velo, veloCorners) # last column of transCam2Velo
1483 homVeloCorners = cartesian2Homogeneous(veloCorners)
1484 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3
1485 #print(kittiCalibration['Tr_velo_imu'], imuCorners)
1486 homImuCorners = cartesian2Homogeneous(imuCorners)
1487 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
1488 #print(Tr_imu_to_world, worldCorners)
1489 xCoords = worldCorners[:4,0]
1490 yCoords = worldCorners[:4,1]
1491 t.addPositionXY(xCoords.mean(), yCoords.mean())
1492 objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car'))
1465 return objects 1493 return objects
1466 1494
1467 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): 1495 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):
1468 '''Reads data from the trajectory data provided by NGSIM project 1496 '''Reads data from the trajectory data provided by NGSIM project
1469 and converts to our current format.''' 1497 and converts to our current format.'''