repo/traffic-intelligence: trafficintelligence/storage.py comparison

comparison trafficintelligence/storage.py @ 1201:28bf2420c412

work in progress to import KITTI data

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 28 Feb 2023 17:16:49 -0500
parents	ccab20f85710
children	059b7282aa09

comparison

equal deleted inserted replaced

-:6a6a4d5958f7
+:28bf2420c412
 from pathlib import Path
 import shutil
 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
+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 pandas import read_csv, merge
 from trafficintelligence import utils, moving, events, indicators
 from trafficintelligence.base import VideoFilenameAddable
 if (obj.size[1] != float(numbers[9])):
 print('changed width obj {}'.format(obj.getNum()))
 inputfile.close()
 return objects
+# from https://github.com/kuixu/kitti_object_vis
+def inverse_rigid_trans(Tr):
+""" 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[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
+def loadKITTICalibration(filename):
+'''Loads KITTI calibration data'''
+calib = {}
+with open(filename, 'r') as f:
+for l in f.readlines():
+l = l.rstrip()
+if len(l) == 0:
+continue
+key, value = l.split(' ', 1)
+if ":" in key:
+key = key[:-1]
+try:
+calib[key] = array([float(x) for x in value.split()])
+except ValueError as e:
+print(e)
+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)
+P = reshape(calib['P2'], (3, 4))
+calib['c_u'] = P[0, 2]
+calib['c_v'] = P[1, 2]
+calib['f_u'] = P[0, 0]
+calib['f_v'] = P[1, 1]
+calib['b_x'] = P[0, 3] / (-calib['f_u'])  # relative
+calib['b_y'] = P[1, 3] / (-calib['f_v'])
+return calib
+# 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
+kittiCalibration is obtained from loading training/testing calibration file for each sequence 00XX.txt
+oxts is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti
+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)))
+header = ['frame', # 0, 1, ..., n
+'trackingid', # -1, 0 , 1, ..., k
+'type',  # 'Car', 'Pedestrian', ...
+'truncation', # truncated pixel ratio [0..1]
+'occlusion', # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown
+'alpha', # object observation angle [-pi..pi]
+# extract 2d bounding box in 0-based coordinates
+'xmin', # left
+'ymin', # top
+'xmax', # right
+'ymax', # bottom
+# extract 3d bounding box information
+'h', # box height
+'w', # box width
+'l', # box length (in meters)
+'x', 'y', 'z', # location (x,y,z) in camera coord
+'ry'] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
+data = read_csv(filename, delimiter = ' ', names = header)
+data = data[data.trackingid > -1]
+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)
+for i, r in tmp.iterrows():
+_, Tr_imu2w = oxts[r.frame]
+transfer_matrix = {'P2': reshape(kittiCalibration['P2'], (3, 4)),
+'R_rect': kittiCalibration['R_rect'],
+'Tr_cam_to_velo': kittiCalibration['Tr_cam_velo'],
+# transfer_matrix['Tr_velo_to_cam'] = kittiCalibration['Tr_velo_to_cam']
+'Tr_velo_to_imu': kittiCalibration['Tr_velo_imu'],
+# transfer_matrix['Tr_imu_to_velo'] = kittiCalibration['Tr_imu_to_velo']
+'Tr_imu_to_world': Tr_imu2w}
+# 3D object
+# compute rotational matrix around yaw axis
+R = roty(r.ry)
+# 3d bounding box corners
+x_corners = [r.l / 2, r.l / 2, -r.l / 2, -r.l / 2, r.l / 2, r.l / 2, -r.l / 2, -r.l / 2]
+y_corners = [0, 0, 0, 0, -r.h, -r.h, -r.h, -r.h]
+z_corners = [r.w / 2, -r.w / 2, -r.w / 2, r.w / 2, r.w / 2, -r.w / 2, -r.w / 2, r.w / 2]
+# rotate and translate 3d bounding box
+corners3d = dot(R, vstack([x_corners, y_corners, z_corners]))
+corners3d[0, :] = corners3d[0, :] + r.x#obj.t[0]
+corners3d[1, :] = corners3d[1, :] + r.y#obj.t[1]
+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)))
+#             boxes3d_world_single, flag_imu, flag_world = calib_function.project_velo_to_world(bboxes_3d_velo=boxes3d_velo_single, Tr_matrix=transfer_matrix)
+# check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
 def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):
 '''Reads data from the trajectory data provided by NGSIM project
 and converts to our current format.'''
 if append:
 for sectionName in config.sections():
 configDict[sectionName] = SceneParameters(config, sectionName)
 return configDict
-if __name__ == "__main__":
+if __name__ == '__main__':
 import doctest
 import unittest
 suite = doctest.DocFileSuite('tests/storage.txt')
 unittest.TextTestRunner().run(suite)
 #     #doctest.testmod()

Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/storage.py @ 1201:28bf2420c412