Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/storage.py @ 1272:785c86013d2c
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 |