Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/storage.py @ 1272:785c86013d2c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added moving average smoothing for kitti loading
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 17 Jun 2024 22:49:22 -0400
parents 0f5bebd62a55
children 8e61ff3cd503
comparison
equal deleted inserted replaced
1271:b2f90cada58f 1272:785c86013d2c
1310 return calib 1310 return calib
1311 1311
1312 # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py 1312 # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
1313 1313
1314 1314
1315 def loadTrajectoriesFromKITTI(filename, kittiCalibration = None, oxts = None, resultFile = False): 1315 def loadTrajectoriesFromKITTI(filename, kittiCalibration = None, oxts = None, resultFile = False, halfWidth = None):
1316 '''Reads data from KITTI ground truth or output from an object detection and tracking method 1316 '''Reads data from KITTI ground truth or output from an object detection and tracking method
1317 1317
1318 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt 1318 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt
1319 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti 1319 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
1320 1320 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt
1321 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' 1321
1322 non-null halfWidth indicates positions are smoothed using moving window avefraging'''
1322 from pykitti.utils import roty, rotz 1323 from pykitti.utils import roty, rotz
1323 from trafficintelligence.cvutils import cartesian2Homogeneous 1324 from trafficintelligence.cvutils import cartesian2Homogeneous
1324 1325
1325 if kittiCalibration is not None: 1326 if kittiCalibration is not None:
1326 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) 1327 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
1429 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1430 t.addPositionXY(xCoords.mean(), yCoords.mean())
1430 for j in range(4): 1431 for j in range(4):
1431 featureTrajectories[j].addPositionXY(xCoords[j], yCoords[j]) 1432 featureTrajectories[j].addPositionXY(xCoords[j], yCoords[j])
1432 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1433 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1433 if interval.length()>1: 1434 if interval.length()>1:
1435 if halfWidth is not None:
1436 for i in range(4):
1437 featureTrajectories[i] = featureTrajectories[i].filterMovingWindow(halfWidth)
1434 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)])) 1438 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)]))
1435 featureNum += 4 1439 featureNum += 4
1436 1440
1437 # ego vehicle 1441 # ego vehicle
1438 if oxts is not None: 1442 if oxts is not None:
1468 yCoords = worldCorners[:4,1] 1472 yCoords = worldCorners[:4,1]
1469 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1473 t.addPositionXY(xCoords.mean(), yCoords.mean())
1470 for i in range(4): 1474 for i in range(4):
1471 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) 1475 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])
1472 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1476 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1477 if halfWidth is not None:
1478 for i in range(4):
1479 featureTrajectories[i] = featureTrajectories[i].filterMovingWindow(halfWidth)
1473 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)])) 1480 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)]))
1474 1481
1475 return objects 1482 return objects
1476 1483
1477 def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None): 1484 def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None, halfWidth = None):
1478 '''Loads x,y coordinate series 1485 '''Loads x,y coordinate series
1479 e.g. obtained by projecting from image to ground world plane using a homography 1486 e.g. obtained by projecting from image to ground world plane using a homography
1480 Format: frame_id,track_id,usertype,x,y,score 1487 Format: frame_id,track_id,usertype,x,y,score
1481 1488
1482 if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti 1489 if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
1483 to generate the movement of the ego vehicle''' 1490 to generate the movement of the ego vehicle
1491
1492 non-null halfWidth indicates positions are smoothed using moving window avefraging'''
1484 1493
1485 header = ['frame','trackingid','usertype','x','y','score'] 1494 header = ['frame','trackingid','usertype','x','y','score']
1486 data = read_csv(filename, delimiter=' ', names = header) 1495 data = read_csv(filename, delimiter=' ', names = header)
1487 objects = [] 1496 objects = []
1488 for objNum in data.trackingid.unique(): 1497 for objNum in data.trackingid.unique():
1495 missing = concat([tmp[['frame', 'x', 'y']], DataFrame([[t, NaN, NaN] for t in instants], columns = ['frame', 'x', 'y'])], ignore_index=True).sort_values('frame') 1504 missing = concat([tmp[['frame', 'x', 'y']], DataFrame([[t, NaN, NaN] for t in instants], columns = ['frame', 'x', 'y'])], ignore_index=True).sort_values('frame')
1496 tmp = missing.interpolate() 1505 tmp = missing.interpolate()
1497 #print(tmp.info()) 1506 #print(tmp.info())
1498 if interval.length()>1: 1507 if interval.length()>1:
1499 t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()]) 1508 t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()])
1509 if halfWidth is not None:
1510 t = t.filterMovingWindow(halfWidth)
1500 objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType)) 1511 objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType))
1501 1512
1502 # ego vehicle 1513 # ego vehicle
1503 if kittiCalibration is not None and oxts is not None: 1514 if kittiCalibration is not None and oxts is not None:
1504 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) 1515 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
1524 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 1535 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
1525 #print(Tr_imu_to_world, worldCorners) 1536 #print(Tr_imu_to_world, worldCorners)
1526 xCoords = worldCorners[:4,0] 1537 xCoords = worldCorners[:4,0]
1527 yCoords = worldCorners[:4,1] 1538 yCoords = worldCorners[:4,1]
1528 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1539 t.addPositionXY(xCoords.mean(), yCoords.mean())
1540 if halfWidth is not None:
1541 t = t.filterMovingWindow(halfWidth)
1529 objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car')) 1542 objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car'))
1530 return objects 1543 return objects
1531 1544
1532 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): 1545 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):
1533 '''Reads data from the trajectory data provided by NGSIM project 1546 '''Reads data from the trajectory data provided by NGSIM project