Mercurial Hosting > traffic-intelligence
diff trafficintelligence/storage.py @ 1205:3905b393ade0
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 |
line wrap: on
line diff
--- a/trafficintelligence/storage.py Wed Mar 22 22:01:12 2023 -0400 +++ b/trafficintelligence/storage.py Wed Mar 22 23:29:09 2023 -0400 @@ -7,10 +7,10 @@ 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, ones +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 from pandas import read_csv, merge -from trafficintelligence import utils, moving, events, indicators +from trafficintelligence import utils, moving, events, indicators, cvutils from trafficintelligence.base import VideoFilenameAddable @@ -1261,8 +1261,7 @@ """ Inverse a rigid body transform matrix (3x4 as [R|t]) [R'|-R't; 0|1] """ - import numpy as np - inv_Tr = np.zeros_like(Tr) # 3x4 + inv_Tr = zeros_like(Tr) # 3x4 inv_Tr[0:3, 0:3] = transpose(Tr[0:3, 0:3]) inv_Tr[0:3, 3] = dot(-transpose(Tr[0:3, 0:3]), Tr[0:3, 3]) return inv_Tr @@ -1271,7 +1270,7 @@ '''Loads KITTI calibration data''' calib = {} with open(filename, 'r') as f: - for l in f.readlines(): + for l in f: l = l.rstrip() if len(l) == 0: continue @@ -1285,10 +1284,8 @@ continue #calib['Tr_velo_to_cam'] = calib['Tr_velo_cam'] #calib['Tr_imu_to_velo'] = calib['Tr_imu_velo'] - temps = reshape(calib['Tr_velo_cam'], (3, 4)) - calib['Tr_cam_velo'] = inverse_rigid_trans(temps) - temps = reshape(calib['Tr_imu_velo'], (3, 4)) - calib['Tr_velo_imu'] = inverse_rigid_trans(temps) + calib['Tr_cam_velo'] = inverse_rigid_trans(reshape(calib['Tr_velo_cam'], (3, 4))) + calib['Tr_velo_imu'] = inverse_rigid_trans(reshape(calib['Tr_imu_velo'], (3, 4))) P = reshape(calib['P2'], (3, 4)) calib['c_u'] = P[0, 2] @@ -1302,23 +1299,6 @@ # from https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py -# def get_cam_in_ex(calib): -# """ -# Qingwu -# :param calib: calib from kitti_tracking -# :return: cam_parameters: Camera intrinsics and extrinsics -# """ -# cam_parameters = {} -# P = np.reshape(calib["P2"], [3, 4]) -# cam_parameters["c_u"] = P[0, 2] -# cam_parameters["c_v"] = P[1, 2] -# cam_parameters["f_u"] = P[0, 0] -# cam_parameters["f_v"] = P[1, 1] -# cam_parameters["b_x"] = P[0, 3] / (-cam_parameters["f_u"]) # relative -# cam_parameters["b_y"] = P[1, 3] / (-cam_parameters["f_v"]) -# return cam_parameters - - def loadTrajectoriesFromKITTI(filename, kittiCalibration, oxts): '''Reads data from KITTI ground truth or output from an object detection and tracking method @@ -1329,6 +1309,7 @@ from pykitti.utils import roty invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) + transCam2Velo = transpose(kittiCalibration['Tr_cam_velo']) header = ['frame', # 0, 1, ..., n 'trackingid', # -1, 0 , 1, ..., k @@ -1357,7 +1338,7 @@ t = moving.Trajectory()#([[float(numbers[6])],[float(numbers[7])]]) featureTrajectories = [moving.Trajectory() for i in range(4)] for i, r in tmp.iterrows(): - _, Tr_imu2w = oxts[r.frame] + _, Tr_imu_to_world = oxts[r.frame] # transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)), # 'R_rect': kittiCalibration['R_rect'], # 'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'], @@ -1380,17 +1361,18 @@ corners3d[2, :] = corners3d[2, :] + r.z#obj.t[2] # corners3d = transpose(corners3d) # box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) - 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) + 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) + homRefCorners = cvutils.cartesian2Homogeneous(refCorners) + veloCorners = dot(homRefCorners, transCam2Velo) +# self.project_ref_to_velo(pts_3d_ref) + # 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 + homVeloCorners = cvutils.cartesian2Homogeneous(veloCorners) imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 - homImuCorners = ones((imuCorners.shape[0], imuCorners.shape[1]+1)) - homImuCorners[:,:-1] = imuCorners # 8x4 with ones in last column - worldCorners = dot(Tr_imu2w, homImuCorners.T).T # 8x3 + homImuCorners = cvutils.cartesian2Homogeneous(imuCorners) + 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] @@ -1399,8 +1381,7 @@ 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, 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)]) - #newObj.featureNumbers = [f.getNum() for f in newObj.getFeatures()] + 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)]) objects.append(newObj) featureNum += 4 return objects