Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/storage.py @ 1202:059b7282aa09

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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
comparison
equal deleted inserted replaced
1201:28bf2420c412 1202:059b7282aa09
5 from pathlib import Path 5 from pathlib import Path
6 import shutil 6 import shutil
7 from copy import copy 7 from copy import copy
8 import sqlite3, logging 8 import sqlite3, logging
9 9
10 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 10 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
11 from pandas import read_csv, merge 11 from pandas import read_csv, merge
12 12
13 from trafficintelligence import utils, moving, events, indicators 13 from trafficintelligence import utils, moving, events, indicators
14 from trafficintelligence.base import VideoFilenameAddable 14 from trafficintelligence.base import VideoFilenameAddable
15 15
1326 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti 1326 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
1327 1327
1328 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' 1328 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt'''
1329 from pykitti.utils import roty 1329 from pykitti.utils import roty
1330 1330
1331 invR0 = np.linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) 1331 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
1332 1332
1333 header = ['frame', # 0, 1, ..., n 1333 header = ['frame', # 0, 1, ..., n
1334 'trackingid', # -1, 0 , 1, ..., k 1334 'trackingid', # -1, 0 , 1, ..., k
1335 'type', # 'Car', 'Pedestrian', ... 1335 'type', # 'Car', 'Pedestrian', ...
1336 'truncation', # truncated pixel ratio [0..1] 1336 'truncation', # truncated pixel ratio [0..1]
1347 'l', # box length (in meters) 1347 'l', # box length (in meters)
1348 'x', 'y', 'z', # location (x,y,z) in camera coord 1348 'x', 'y', 'z', # location (x,y,z) in camera coord
1349 'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] 1349 'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
1350 data = read_csv(filename, delimiter = ' ', names = header) 1350 data = read_csv(filename, delimiter = ' ', names = header)
1351 data = data[data.trackingid > -1] 1351 data = data[data.trackingid > -1]
1352
1353 objects = []
1352 for objNum in data.trackingid.unique(): 1354 for objNum in data.trackingid.unique():
1353 tmp = data[data.trackingid == objNum].sort_values('frame') 1355 tmp = data[data.trackingid == objNum].sort_values('frame')
1354 obj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), 1356 t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]])
1355 positions = moving.Trajectory([[float(numbers[6])],[float(numbers[7])]]),
1356 userType = tmp.iloc[0].type)
1357 for i, r in tmp.iterrows(): 1357 for i, r in tmp.iterrows():
1358 _, Tr_imu2w = oxts[r.frame] 1358 _, Tr_imu2w = oxts[r.frame]
1359 transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), 1359 transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)),
1360 'R_rect': kittiCalibration['R_rect'], 1360 'R_rect': kittiCalibration['R_rect'],
1361 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'], 1361 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'],
1376 corners3d[0, :] = corners3d[0, :] + r.x#obj.t[0] 1376 corners3d[0, :] = corners3d[0, :] + r.x#obj.t[0]
1377 corners3d[1, :] = corners3d[1, :] + r.y#obj.t[1] 1377 corners3d[1, :] = corners3d[1, :] + r.y#obj.t[1]
1378 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] 1378 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2]
1379 # corners3d = transpose(corners3d) 1379 # corners3d = transpose(corners3d)
1380 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) 1380 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
1381 cornersVelo = transpose(dot(invR0, corners3d)) # avoid double transpose np.transpose(pts_3d_rect))) 1381 veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect)))
1382 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix) 1382 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
1383 1383 Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu']
1384 Tr_imu_to_world = transfer_matrix['Tr_imu_to_world']
1385 homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1))
1386 homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column
1387 imuCorners = dot(Tr_velo_to_imu, homVeloCorners.T).T # 8x3
1388
1389 homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1))
1390 homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column
1391 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
1392
1393 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0]
1394 xCoords = worldCorners[:4,0]
1395 yCoords = worldCorners[:4,1]
1396 t.addPositionXY(xCoords.mean(), yCoords.mean())
1384 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1397 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1385 1398 objects.append(moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()),
1386 1399 positions = t,
1400 userType = tmp.iloc[0].type))
1401 return objects
1387 1402
1388 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): 1403 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):
1389 '''Reads data from the trajectory data provided by NGSIM project 1404 '''Reads data from the trajectory data provided by NGSIM project
1390 and converts to our current format.''' 1405 and converts to our current format.'''
1391 if append: 1406 if append: