Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/storage.py @ 1205:3905b393ade0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
kitti loading code seems to be working
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 22 Mar 2023 23:29:09 -0400
parents a12d126346ff
children 36f0d18e1fad
comparison
equal deleted inserted replaced
1204:a12d126346ff 1205:3905b393ade0
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, ones 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, zeros_like
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, cvutils
14 from trafficintelligence.base import VideoFilenameAddable 14 from trafficintelligence.base import VideoFilenameAddable
15 15
16 16
17 ngsimUserTypes = {'twowheels':1, 17 ngsimUserTypes = {'twowheels':1,
18 'car':2, 18 'car':2,
1259 # from https://github.com/kuixu/kitti_object_vis 1259 # from https://github.com/kuixu/kitti_object_vis
1260 def inverse_rigid_trans(Tr): 1260 def inverse_rigid_trans(Tr):
1261 """ Inverse a rigid body transform matrix (3x4 as [R|t]) 1261 """ Inverse a rigid body transform matrix (3x4 as [R|t])
1262 [R'|-R't; 0|1] 1262 [R'|-R't; 0|1]
1263 """ 1263 """
1264 import numpy as np 1264 inv_Tr = zeros_like(Tr) # 3x4
1265 inv_Tr = np.zeros_like(Tr) # 3x4
1266 inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3]) 1265 inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3])
1267 inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3]) 1266 inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3])
1268 return inv_Tr 1267 return inv_Tr
1269 1268
1270 def loadKITTICalibration(filename): 1269 def loadKITTICalibration(filename):
1271 '''Loads KITTI calibration data''' 1270 '''Loads KITTI calibration data'''
1272 calib = {} 1271 calib = {}
1273 with open(filename, 'r') as f: 1272 with open(filename, 'r') as f:
1274 for l in f.readlines(): 1273 for l in f:
1275 l = l.rstrip() 1274 l = l.rstrip()
1276 if len(l) == 0: 1275 if len(l) == 0:
1277 continue 1276 continue
1278 key, value = l.split(' ', 1) 1277 key, value = l.split(' ', 1)
1279 if ":" in key: 1278 if ":" in key:
1283 except ValueError as e: 1282 except ValueError as e:
1284 print(e) 1283 print(e)
1285 continue 1284 continue
1286 #calib['Tr_velo_to_cam'] = calib['Tr_velo_cam'] 1285 #calib['Tr_velo_to_cam'] = calib['Tr_velo_cam']
1287 #calib['Tr_imu_to_velo'] = calib['Tr_imu_velo'] 1286 #calib['Tr_imu_to_velo'] = calib['Tr_imu_velo']
1288 temps = reshape(calib['Tr_velo_cam'], (3, 4)) 1287 calib['Tr_cam_velo'] = inverse_rigid_trans(reshape(calib['Tr_velo_cam'], (3, 4)))
1289 calib['Tr_cam_velo'] = inverse_rigid_trans(temps) 1288 calib['Tr_velo_imu'] = inverse_rigid_trans(reshape(calib['Tr_imu_velo'], (3, 4)))
1290 temps = reshape(calib['Tr_imu_velo'], (3, 4))
1291 calib['Tr_velo_imu'] = inverse_rigid_trans(temps)
1292 1289
1293 P = reshape(calib['P2'], (3, 4)) 1290 P = reshape(calib['P2'], (3, 4))
1294 calib['c_u'] = P[0, 2] 1291 calib['c_u'] = P[0, 2]
1295 calib['c_v'] = P[1, 2] 1292 calib['c_v'] = P[1, 2]
1296 calib['f_u'] = P[0, 0] 1293 calib['f_u'] = P[0, 0]
1300 return calib 1297 return calib
1301 1298
1302 # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py 1299 # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
1303 1300
1304 1301
1305 # def get_cam_in_ex(calib):
1306 # """
1307 # Qingwu
1308 # :param calib: calib from kitti_tracking
1309 # :return: cam_parameters: Camera intrinsics and extrinsics
1310 # """
1311 # cam_parameters = {}
1312 # P = np.reshape(calib["P2"], [3, 4])
1313 # cam_parameters["c_u"] = P[0, 2]
1314 # cam_parameters["c_v"] = P[1, 2]
1315 # cam_parameters["f_u"] = P[0, 0]
1316 # cam_parameters["f_v"] = P[1, 1]
1317 # cam_parameters["b_x"] = P[0, 3] / (-cam_parameters["f_u"]) # relative
1318 # cam_parameters["b_y"] = P[1, 3] / (-cam_parameters["f_v"])
1319 # return cam_parameters
1320
1321
1322 def loadTrajectoriesFromKITTI(filename, kittiCalibration, oxts): 1302 def loadTrajectoriesFromKITTI(filename, kittiCalibration, oxts):
1323 '''Reads data from KITTI ground truth or output from an object detection and tracking method 1303 '''Reads data from KITTI ground truth or output from an object detection and tracking method
1324 1304
1325 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt 1305 kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt
1326 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti 1306 oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
1327 1307
1328 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt''' 1308 Ref: https://github.com/pratikac/kitti/blob/master/readme.tracking.txt'''
1329 from pykitti.utils import roty 1309 from pykitti.utils import roty
1330 1310
1331 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) 1311 invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3)))
1312 transCam2Velo = transpose(kittiCalibration['Tr_cam_velo'])
1332 1313
1333 header = ['frame', # 0, 1, ..., n 1314 header = ['frame', # 0, 1, ..., n
1334 'trackingid', # -1, 0 , 1, ..., k 1315 'trackingid', # -1, 0 , 1, ..., k
1335 'type', # 'Car', 'Pedestrian', ... 1316 'type', # 'Car', 'Pedestrian', ...
1336 'truncation', # truncated pixel ratio [0..1] 1317 'truncation', # truncated pixel ratio [0..1]
1355 for objNum in data.trackingid.unique(): 1336 for objNum in data.trackingid.unique():
1356 tmp = data[data.trackingid == objNum].sort_values('frame') 1337 tmp = data[data.trackingid == objNum].sort_values('frame')
1357 t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]]) 1338 t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]])
1358 featureTrajectories = [moving.Trajectory() for i in range(4)] 1339 featureTrajectories = [moving.Trajectory() for i in range(4)]
1359 for i, r in tmp.iterrows(): 1340 for i, r in tmp.iterrows():
1360 _, Tr_imu2w = oxts[r.frame] 1341 _, Tr_imu_to_world = oxts[r.frame]
1361 # transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), 1342 # transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)),
1362 # 'R_rect': kittiCalibration['R_rect'], 1343 # 'R_rect': kittiCalibration['R_rect'],
1363 # 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'], 1344 # 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'],
1364 # # transfer_matrix['Tr_velo_to_cam'] = kittiCalibration['Tr_velo_to_cam'] 1345 # # transfer_matrix['Tr_velo_to_cam'] = kittiCalibration['Tr_velo_to_cam']
1365 # 'Tr_velo_to_imu': kittiCalibration['Tr_velo_imu'], 1346 # 'Tr_velo_to_imu': kittiCalibration['Tr_velo_imu'],
1378 corners3d[0, :] = corners3d[0, :] + r.x#obj.t[0] 1359 corners3d[0, :] = corners3d[0, :] + r.x#obj.t[0]
1379 corners3d[1, :] = corners3d[1, :] + r.y#obj.t[1] 1360 corners3d[1, :] = corners3d[1, :] + r.y#obj.t[1]
1380 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] 1361 corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2]
1381 # corners3d = transpose(corners3d) 1362 # corners3d = transpose(corners3d)
1382 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) 1363 # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d)
1383 veloCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) 1364 refCorners = transpose(dot(invR0, corners3d)) # 8x3 avoid double transpose np.transpose(pts_3d_rect))) in pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
1384 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix) 1365 homRefCorners = cvutils.cartesian2Homogeneous(refCorners)
1366 veloCorners = dot(homRefCorners, transCam2Velo)
1367 # self.project_ref_to_velo(pts_3d_ref)
1368 # boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
1385 #Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu'] 1369 #Tr_velo_to_imu = transfer_matrix['Tr_velo_to_imu']
1386 #Tr_imu_to_world = transfer_matrix['Tr_imu_to_world'] 1370 #Tr_imu_to_world = transfer_matrix['Tr_imu_to_world']
1387 homVeloCorners = ones((veloCorners.shape[0], veloCorners.shape[1]+1)) 1371 homVeloCorners = cvutils.cartesian2Homogeneous(veloCorners)
1388 homVeloCorners[:,:-1] = veloCorners # 8x4 with ones in last column
1389 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 1372 imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3
1390 1373
1391 homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1)) 1374 homImuCorners = cvutils.cartesian2Homogeneous(imuCorners)
1392 homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column 1375 worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3
1393 worldCorners = dot(Tr_imu2w, homImuCorners.T).T # 8x3
1394 1376
1395 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0] 1377 # take first 4 lines of corners, x,y,_ # x0, y0, _ = boxes3d[0]
1396 xCoords = worldCorners[:4,0] 1378 xCoords = worldCorners[:4,0]
1397 yCoords = worldCorners[:4,1] 1379 yCoords = worldCorners[:4,1]
1398 t.addPositionXY(xCoords.mean(), yCoords.mean()) 1380 t.addPositionXY(xCoords.mean(), yCoords.mean())
1399 for i in range(4): 1381 for i in range(4):
1400 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) 1382 featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i])
1401 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c 1383 # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
1402 newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = t, userType = tmp.iloc[0].type, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(tmp.frame.min(), tmp.frame.max()), positions = featureTrajectories[i]) for i in range(4)]) 1384 newObj = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = t, userType = tmp.iloc[0].type, features = [moving.MovingObject(num = featureNum+i, timeInterval = moving.TimeInterval(int(tmp.frame.min()), int(tmp.frame.max())), positions = featureTrajectories[i]) for i in range(4)])
1403 #newObj.featureNumbers = [f.getNum() for f in newObj.getFeatures()]
1404 objects.append(newObj) 1385 objects.append(newObj)
1405 featureNum += 4 1386 featureNum += 4
1406 return objects 1387 return objects
1407 1388
1408 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): 1389 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):