Mercurial Hosting > traffic-intelligence
diff 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 |
line wrap: on
line diff
--- a/trafficintelligence/storage.py Fri Mar 22 14:33:25 2024 -0400 +++ b/trafficintelligence/storage.py Mon Mar 25 17:05:20 2024 -0400 @@ -1395,8 +1395,7 @@ featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c if interval.length()>1: - 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)]) - objects.append(newObj) + 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)])) featureNum += 4 # ego vehicle @@ -1434,15 +1433,17 @@ for i in range(4): featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c - 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)]) - objects.append(newObj) + 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)])) return objects -def loadTrajectoriesFromKITTI2D(filename): +def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None): '''Loads x,y coordinate series e.g. obtained by projecting from image to ground world plane using a homography - Format: frame_id,track_id,usertype,x,y,score ''' + Format: frame_id,track_id,usertype,x,y,score + + if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti + to generate the movement of the ego vehicle''' header = ['frame','trackingid','usertype','x','y','score'] data = read_csv(filename, delimiter=' ', names = header) @@ -1459,9 +1460,36 @@ #print(tmp.info()) if interval.length()>1: t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()]) - newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType) - objects.append(newObj) + objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType)) + # ego vehicle + if kittiCalibration is not None and oxts is not None: + invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) + transCam2Velo = transpose(kittiCalibration['Tr_cam_velo']) + t = moving.Trajectory() + featureTrajectories = [moving.Trajectory() for i in range(4)] + interval = moving.TimeInterval(0, len(oxts)-1)#int(data.frame.min()), int(data.frame.max())) + #from pykitti.utils import roty + from trafficintelligence.cvutils import cartesian2Homogeneous + for _, Tr_imu_to_world in oxts: + #R = roty(pi/2) + #corners3d = dot(R, [0,0,0]) + #print(corners3d) + #refCorners = transpose(dot(invR0, corners3d)) + #print(invR0, refCorners) # 000 + #homRefCorners = cartesian2Homogeneous(array([refCorners])) + veloCorners = transCam2Velo[3, :] # dot(homRefCorners, transCam2Velo) + #print(transCam2Velo, veloCorners) # last column of transCam2Velo + homVeloCorners = cartesian2Homogeneous(veloCorners) + imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 + #print(kittiCalibration['Tr_velo_imu'], imuCorners) + homImuCorners = cartesian2Homogeneous(imuCorners) + worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 + #print(Tr_imu_to_world, worldCorners) + xCoords = worldCorners[:4,0] + yCoords = worldCorners[:4,1] + t.addPositionXY(xCoords.mean(), yCoords.mean()) + objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car')) return objects def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):