Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/storage.py @ 1253:ef68d4ba7dae
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.''' |