Mercurial Hosting > traffic-intelligence
changeset 1202:059b7282aa09
first version of kitti loader
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 16 Mar 2023 17:03:18 -0400 |
parents | 28bf2420c412 |
children | 7b3384a8e409 |
files | trafficintelligence/storage.py |
diffstat | 1 files changed, 23 insertions(+), 8 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/storage.py Tue Feb 28 17:16:49 2023 -0500 +++ b/trafficintelligence/storage.py Thu Mar 16 17:03:18 2023 -0400 @@ -7,7 +7,7 @@ from copy import copy import sqlite3, logging -from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose +from numpy import log, min as npmin, max as npmax, round as npround, array, sum as npsum, loadtxt, floor as npfloor, ceil as npceil, linalg, int32, int64, reshape, dot, vstack, transpose, ones from pandas import read_csv, merge from trafficintelligence import utils, moving, events, indicators @@ -1328,7 +1328,7 @@ Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' from pykitti.utils import roty - invR0 = np.linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) + invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) header = ['frame', # 0, 1, ..., n 'trackingid', # -1, 0 , 1, ..., k @@ -1349,11 +1349,11 @@ 'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] data = read_csv(filename, delimiter = ' ', names = header) data = data[data.trackingid > -1] + + objects = [] for objNum in data.trackingid.unique(): tmp = data[data.trackingid == objNum].sort_values('frame') - obj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), - positions = moving.Trajectory([[float(numbers[6])],[float(numbers[7])]]), - userType = tmp.iloc[0].type) + t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]]) for i, r in tmp.iterrows(): _, Tr_imu2w = oxts[r.frame] transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), @@ -1378,12 +1378,27 @@ corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] # corners3d = transpose(corners3d) # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) - cornersVelo = transpose(dot(invR0, corners3d)) # avoid double transpose np.transpose(pts_3d_rect))) + veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix) + Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu'] + Tr_imu_to_world = transfer_matrix['Tr_imu_to_world'] + homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1)) + homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column + imuCorners = dot(Tr_velo_to_imu, homVeloCorners.T).T # 8x3 - # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c + homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1)) + homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column + worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 - + # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] + xCoords = worldCorners[:4,0] + yCoords = worldCorners[:4,1] + t.addPositionXY(xCoords.mean(), yCoords.mean()) + # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c + objects.append(moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), + positions = t, + userType = tmp.iloc[0].type)) + return objects def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): '''Reads data from the trajectory data provided by NGSIM project