Mercurial Hosting > traffic-intelligence
changeset 1253:ef68d4ba7dae
added loading ego vehicle in kitti 2D format and method to plot outline
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Mon, 25 Mar 2024 17:05:20 -0400 |
| parents | fe35473acee3 |
| children | a477ad82ab66 |
| files | trafficintelligence/cvutils.py trafficintelligence/moving.py trafficintelligence/storage.py |
| diffstat | 3 files changed, 55 insertions(+), 14 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/cvutils.py Fri Mar 22 14:33:25 2024 -0400 +++ b/trafficintelligence/cvutils.py Mon Mar 25 17:05:20 2024 -0400 @@ -30,6 +30,7 @@ trackerExe = 'feature-based-tracking' #importaggdraw # agg on top of PIL (antialiased drawing) +colors = 'rgbcymk' cvRed = {'default': (0,0,255), 'colorblind': (0,114,178)} cvGreen = {'default': (0,255,0), @@ -534,7 +535,10 @@ 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)) + if len(m.shape) == 2: + homoM = ones((m.shape[0], m.shape[1]+1)) + else: + homoM = ones((1, len(m)+1)) homoM[:,:-1] = m return homoM
--- a/trafficintelligence/moving.py Fri Mar 22 14:33:25 2024 -0400 +++ b/trafficintelligence/moving.py Mon Mar 25 17:05:20 2024 -0400 @@ -1414,6 +1414,7 @@ 5: 'bus', #6: 'train', 7: 'truck'} +kitti2Types = {'Car': 1, 'Cyclist': 4, 'Pedestrian': 2, 'Van': 1} userType2Num = utils.inverseEnumeration(userTypeNames) @@ -1440,6 +1441,8 @@ self.userType = userType self.setNObjects(nObjects) # a feature has None for nObjects self.features = features + if features is not None and len(features) > 1: + self.featureNumbers = [f.getNum() for f in features] # compute bounding polygon from trajectory @staticmethod @@ -1819,15 +1822,21 @@ for f in self.getFeatures(): f.setStationary() - def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs): + def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withOutline = False, withIds = False, **kwargs): if withIds: objNum = self.getNum() else: objNum = None - if withFeatures and self.hasFeatures(): - for f in self.getFeatures(): - f.positions.plot('r', True, timeStep, **kwargs) - self.positions.plot('bx-', True, timeStep, objNum, **kwargs) + if (withFeatures or withOutline) and self.hasFeatures(): + if withOutline: + for t in self.getTimeInterval(): + if t%timeStep == 0: + self.plotOutlineAtInstant(t, cvutils.colors[self.getNum()%len(cvutils.colors)]+'-') + self.positions.plot(cvutils.colors[self.getNum()%len(cvutils.colors)]+'-', withOrigin, timeStep, objNum, **kwargs) + else: + for f in self.getFeatures(): + f.positions.plot('r', True, timeStep, **kwargs) + self.positions.plot('bx-', True, timeStep, objNum, **kwargs) else: self.positions.plot(options, withOrigin, timeStep, objNum, **kwargs)
--- a/trafficintelligence/storage.py Fri Mar 22 14:33:25 2024 -0400 +++ b/trafficintelligence/storage.py Mon Mar 25 17:05:20 2024 -0400 @@ -1395,8 +1395,7 @@ featureTrajectories[i].addPositionXY(xCoords[i], yCoords[i]) # check https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c if interval.length()>1: - newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType, features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]) - objects.append(newObj) + objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType, features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])) featureNum += 4 # ego vehicle @@ -1434,15 +1433,17 @@ 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+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)]) - objects.append(newObj) + objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car', features = [moving.MovingObject(num = featureNum+i, timeInterval = copy(interval), positions = featureTrajectories[i], velocities = featureTrajectories[i].differentiate(True)) for i in range(4)])) return objects -def loadTrajectoriesFromKITTI2D(filename): +def loadTrajectoriesFromKITTI2D(filename, kittiCalibration = None, oxts = None): '''Loads x,y coordinate series e.g. obtained by projecting from image to ground world plane using a homography - Format: frame_id,track_id,usertype,x,y,score ''' + Format: frame_id,track_id,usertype,x,y,score + + if oxts is not None, it is obtained using utils.load_oxts_packets_and_poses(['./training/oxts/0001.txt']) from pykitti + to generate the movement of the ego vehicle''' header = ['frame','trackingid','usertype','x','y','score'] data = read_csv(filename, delimiter=' ', names = header) @@ -1459,9 +1460,36 @@ #print(tmp.info()) if interval.length()>1: t = moving.Trajectory([tmp.x.to_list(), tmp.y.to_list()]) - newObj = moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType) - objects.append(newObj) + objects.append(moving.MovingObject(num = objNum, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = userType)) + # ego vehicle + if kittiCalibration is not None and oxts is not None: + invR0 = linalg.inv(reshape(kittiCalibration['R_rect'], (3, 3))) + transCam2Velo = transpose(kittiCalibration['Tr_cam_velo']) + t = moving.Trajectory() + featureTrajectories = [moving.Trajectory() for i in range(4)] + interval = moving.TimeInterval(0, len(oxts)-1)#int(data.frame.min()), int(data.frame.max())) + #from pykitti.utils import roty + from trafficintelligence.cvutils import cartesian2Homogeneous + for _, Tr_imu_to_world in oxts: + #R = roty(pi/2) + #corners3d = dot(R, [0,0,0]) + #print(corners3d) + #refCorners = transpose(dot(invR0, corners3d)) + #print(invR0, refCorners) # 000 + #homRefCorners = cartesian2Homogeneous(array([refCorners])) + veloCorners = transCam2Velo[3, :] # dot(homRefCorners, transCam2Velo) + #print(transCam2Velo, veloCorners) # last column of transCam2Velo + homVeloCorners = cartesian2Homogeneous(veloCorners) + imuCorners = dot(kittiCalibration['Tr_velo_imu'], homVeloCorners.T).T # 8x3 + #print(kittiCalibration['Tr_velo_imu'], imuCorners) + homImuCorners = cartesian2Homogeneous(imuCorners) + worldCorners = dot(Tr_imu_to_world, homImuCorners.T).T # 8x3 + #print(Tr_imu_to_world, worldCorners) + xCoords = worldCorners[:4,0] + yCoords = worldCorners[:4,1] + t.addPositionXY(xCoords.mean(), yCoords.mean()) + objects.append(moving.MovingObject(num = max([o.getNum() for o in objects])+1, timeInterval = interval, positions = t, velocities = t.differentiate(True), userType = 'Car')) return objects def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0):