# HG changeset patch # User Nicolas Saunier # Date 1679542149 14400 # Node ID 3905b393ade0de603111722bc9fbee7efad18299 # Parent a12d126346ff37907ec0aa94391be4e9be54c4a9 kitti loading code seems to be working diff -r a12d126346ff -r 3905b393ade0 trafficintelligence/cvutils.py --- a/trafficintelligence/cvutils.py Wed Mar 22 22:01:12 2023 -0400 +++ b/trafficintelligence/cvutils.py Wed Mar 22 23:29:09 2023 -0400 @@ -535,6 +535,12 @@ [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) return cv2.remap(img, map1, map2, interpolation=interpolation) +def cartesian2Homogeneous(m): + 'Transforms n x m matrix to n x (m+1) by adding last column of 1s' + homoM = ones((m.shape[0], m.shape[1]+1)) + homoM[:,:-1] = m + return homoM + def homographyProject(points, homography, output3D = False): '''Returns the coordinates of the points (2xN array) projected through homography''' if points.shape[0] != 2: diff -r a12d126346ff -r 3905b393ade0 trafficintelligence/moving.py --- a/trafficintelligence/moving.py Wed Mar 22 22:01:12 2023 -0400 +++ b/trafficintelligence/moving.py Wed Mar 22 23:29:09 2023 -0400 @@ -357,12 +357,12 @@ return (p1-p2).norm2() @staticmethod - def plotAll(points, options = '', closePolygon = False, **kwargs): + def plotAll(points, closePolygon = False, options = '', **kwargs): xCoords = [p.x for p in points] yCoords = [p.y for p in points] if closePolygon: - xCoords.append[0] - yCoords.append[0] + xCoords.append(xCoords[0]) + yCoords.append(yCoords[0]) plot(xCoords, yCoords, options, **kwargs) def similarOrientation(self, refDirection, cosineThreshold): @@ -1659,7 +1659,7 @@ def plotOutlineAtInstant(self, t, options = '', **kwargs): if self.hasFeatures(): points = [f.getPositionAtInstant(t) for f in self.getFeatures()] - Point.plotAll(points, options, True, kwargs) + Point.plotAll(points, True, options, **kwargs) def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) diff -r a12d126346ff -r 3905b393ade0 trafficintelligence/storage.py --- 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