diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3e50481 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +**__pycache__** +**build** +**egg-info** +**dist** +argo2kitti.py diff --git a/README.md b/README.md index 1f179bb..8d8b1a8 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,8 @@ # argoverse-kitti-adapter A toolbox to translate [Argoverse dataset (CVPR2019)](https://www.argoverse.org/data.html) into [KITTI dataset (CVPR2012)](http://www.cvlibs.net/datasets/kitti/) format for perception/tracking tasks. - - Author: Yiyang Zhou - - Contact: yiyang.zhou@berkeley.edu + - Author: Yiyang Zhou, Di Feng + - Contact: yiyang.zhou@berkeley.edu, di.feng@berkeley.edu ## Introduction This toolbox is to translate Argoverse dataset from CVPR2019 into the KITTI dataset format. The major changes are: @@ -34,7 +34,7 @@ argodataset - To use the toolbox, please follow the instructions in the [argoverse github repository](https://github.com/argoai/argoverse-api/tree/16dec1ba51479a24b14d935e7873b26bfd1a7464) to install the corrsponding python API. ### 3. Clone the Argoverse-kitti-adapter Repo -'''git clone https://github.com/yzhou377/argoverse-kitti-adapter.git''' +```git clone https://github.com/yzhou377/argoverse-kitti-adapter.git``` - Once the data and the API are well equipped, please open the 'apater.py' file for changing your root_dir (the directory to your argoverse-tracking folder). The toolbox will automatically construct a new folder (train_kitti) for you. The new file structure is shown as below: ``` @@ -43,13 +43,14 @@ argodataset └── train <-------------------------------------data_dir └──5c251c22-11b2-3278-835c-0cf3cdee3f44 └──... - └── train_kitti <-------------------------------goal_dir - └──velodyne - └──iamge_2 - └──calib - └──label_2 - └──velodyne_reduced <-----------------------empty folder - └── ... + └── argoverse-kitti <---------------------------goal_dir + └── training + └──velodyne + └──iamge_2 + └──calib + └──label_2 + └── statistics <----------------------------dataset statistics + └── Imagesets <-----------------------------dataset split ``` ### 4. Change Hyperparameters @@ -57,18 +58,18 @@ argodataset ### 5. Run the Adapter - After changing the configruation file, please run the adapter.py file using the following commands -"""python adapter.py""" +```python adapter.py``` ## Note 1. Frequency and Sychronization - In KITTI, the camera and the LIDAR are synchronized at 10Hz. However, in Argoverse, the ring cameras are running at 30Hz, while the LIDAR is running at 10Hz. To fully realize the KITTI dataset format, we match each LIDAR scan with the corresponding camera at the closest timestamp. As a result, the sensor combo in the modified KITTI version of Argoverse is still running at 10Hz. -2. Multi-camera -- In KITTI dataset, each image is matched up with one LIDAR scan, one label file, and one calibration document. However, in Argoverse, seven images share one LIDAR scan, and one log only has one single label/calibration combo. Using only the ring cameras, the LIDAR file is copied 7 times to match with each image, and corresponding label/calibration files are generated as well. -3. Labelling File Clips -- KITTI only labels the object in the view of the front camera, while Argoverse, given its panoramic nature, labels all the obstacles around the object. Thus, for each associated labelling file, if the object is not seen in this specific image, then it is not labelled. Furthermore, objects that are too small (beyond 50m) were not labelled. One can cetrainly change this threshold in the ['apater.py'](https://github.com/yzhou377/argoverse-kitti-adapter/blob/master/adapter.py). [Here](https://github.com/yzhou377/argoverse-kitti-adapter/blob/master/supplementals/KITTI_README) attaches the KITTI label README file . For the Argoverse label file, please go check the [Argoverse github](https://github.com/argoai/argoverse-api/tree/16dec1ba51479a24b14d935e7873b26bfd1a7464) + +2. Labelling File Clips +- KITTI only labels the object in the view of the front camera, while Argoverse, given its panoramic nature, labels all the obstacles around the object. Thus, for each associated labelling file, if the object is not seen in this specific image, then it is not labelled. Furthermore, objects that are too small (beyond 70m) were not labelled. One can cetrainly change this threshold in the [`apater.py`](https://github.com/frankfengdi/argoverse-kitti-adapter/blob/master/adapter.py). [Here](https://github.com/frankfengdi/argoverse-kitti-adapter/blob/master/supplementals/KITTI_README) attaches the KITTI label README file . For the Argoverse label file, please go check the [Argoverse github](https://github.com/argoai/argoverse-api/tree/16dec1ba51479a24b14d935e7873b26bfd1a7464) 4. Calibration File - To match the KITTI calibration file, the tool is designed to combine the 'R0_rect' matrix together with the 'P2' matrix to form intrinsic matrix 'K' of the camera. In the new label file, 'R0_rect' is set to be an identity matrix, while 'P2' contains all the intrinsics. ## Reference - [1] M. Chang et al., Argoverse: 3D Tracking and Forecasting with Rich Maps, CVPR2019, Long Beach, U.S.A - [2] A. Geiger et al., Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite, CVPR2012, Rhode Island, U.S.A +- [3] Y. Wang et al., Train in Germany, Test in The USA: Making 3D Object Detectors Generalize, CVPR2020, Seatle, U.S.A diff --git a/adapter.py b/adapter.py index d52cdf3..c550866 100644 --- a/adapter.py +++ b/adapter.py @@ -1,34 +1,11 @@ -# Adapter -"""The code to translate Argoverse dataset to KITTI dataset format""" +"""The code to translate Argoverse dataset to KITTI dataset format +Author: Yiyang Zhou +Email: yiyang.zhou@berkeley.edu -# Argoverse-to-KITTI Adapter +Extension: Di Feng +Email: di.feng@berkeley.edu -# Author: Yiyang Zhou -# Email: yiyang.zhou@berkeley.edu - -print('\nLoading files...') - -import argoverse -from argoverse.data_loading.argoverse_tracking_loader import ArgoverseTrackingLoader -import os -from shutil import copyfile -from argoverse.utils import calibration -import json -import numpy as np -from argoverse.utils.calibration import CameraConfig -from argoverse.utils.cv2_plotting_utils import draw_clipped_line_segment -from argoverse.utils.se3 import SE3 -from argoverse.utils.transform import quat2rotmat -import math -import os -from typing import Union -import numpy as np -import pyntcloud -import progressbar -from time import sleep - -""" Your original file directory is: argodataset └── argoverse-tracking <----------------------------root_dir @@ -43,177 +20,545 @@ └──8a15674a-ae5c-38e2-bc4b-f4156d384072 └──... +Acknowledgement: +We thank Xiangyu Chen (xc429@cornell.edu) for providing useful help to convert argoverse dataset. """ +print('\nLoading files...') +import os -####CONFIGURATION################################################# -# Root directory -root_dir= '/media/msc/8TB/car/argodataset/argoverse-tracking/' - -# Maximum thresholding distance for labelled objects -# (Object beyond this distance will not be labelled) -max_d=50 - -#################################################################### -_PathLike = Union[str, "os.PathLike[str]"] -def load_ply(ply_fpath: _PathLike) -> np.ndarray: - """Load a point cloud file from a filepath. - Args: - ply_fpath: Path to a PLY file - Returns: - arr: Array of shape (N, 3) - """ +import json +import numpy as np +import math +from typing import Union +from itertools import chain +from time import sleep - data = pyntcloud.PyntCloud.from_file(os.fspath(ply_fpath)) - x = np.array(data.points.x)[:, np.newaxis] - y = np.array(data.points.y)[:, np.newaxis] - z = np.array(data.points.z)[:, np.newaxis] +import matplotlib.pyplot as plt +from shutil import copyfile +import pyntcloud +import progressbar +from scipy.spatial.transform import Rotation as R - return np.concatenate((x, y, z), axis=1) +import argoverse +from argoverse.data_loading.argoverse_tracking_loader import ArgoverseTrackingLoader +from argoverse.utils.camera_stats import RING_CAMERA_LIST, STEREO_CAMERA_LIST, get_image_dims_for_camera, STEREO_IMG_WIDTH, STEREO_IMG_HEIGHT, RING_IMG_HEIGHT, RING_IMG_WIDTH +from argoverse.utils.calibration import CameraConfig +from argoverse.utils.cv2_plotting_utils import draw_clipped_line_segment +from argoverse.utils.se3 import SE3 +from argoverse.utils.transform import quat2rotmat, quat_argo2scipy, quat_argo2scipy_vectorized +from argoverse.utils import calibration -# Setup the root directory +_PathLike = Union[str, "os.PathLike[str]"] -data_dir= root_dir+'train_test/' -goal_dir= root_dir+'train_test_kitti/' -if not os.path.exists(goal_dir): +####CONFIGURATION################################################# +root_dir = '/media/vision/HDD Storage/data/argoverse/argoverse-tracking/' # Root directory +# Set up the data dir and target dir +data_dir_list = ['train1/', 'train2/', 'train3/', 'train4/', 'val/'] +goal_dir = root_dir + 'argoverse-lidar-augmented-full-range-kitti/' # 'argoverse-lidar-augmented-kitti', 'argoverse-kitti', 'argoverse-lidar-augmented-full-range-kitti/' +goal_subdir = goal_dir + 'training/' +imageset_dir = goal_dir+'/ImageSets' # to store train val mapping + +if not os.path.exists(goal_dir): os.mkdir(goal_dir) - os.mkdir(goal_dir+'velodyne') - os.mkdir(goal_dir+'image_2') - os.mkdir(goal_dir+'calib') - os.mkdir(goal_dir+'label_2') - os.mkdir(goal_dir+'velodyne_reduced') - -# Check the number of logs(one continuous trajectory) -argoverse_loader= ArgoverseTrackingLoader(data_dir) -print('\nTotal number of logs:',len(argoverse_loader)) -argoverse_loader.print_all() -print('\n') - -cams=['ring_front_center', +if not os.path.exists(goal_dir+'statistics'): + os.mkdir(goal_dir+'statistics') # store dataset statistics +if not os.path.exists(imageset_dir): + os.mkdir(imageset_dir) +if not os.path.exists(goal_subdir): + os.mkdir(goal_subdir) + os.mkdir(goal_subdir+'velodyne') + os.mkdir(goal_subdir+'image_2') + os.mkdir(goal_subdir+'calib') + os.mkdir(goal_subdir+'label_2') + os.mkdir(goal_subdir+'ego_vehicle_pose') # rotation + translation matrix, city coordinate + os.mkdir(goal_subdir+'ego_vehicle_ground_height') # city coordinate + + +max_d = 70 # Maximum thresholding distance for labelled objects (Object beyond this distance will not be labelled) +full_range_lidar = True # When set True, store object labels in full range + +# Camera reference setup +cams_all = ['ring_front_center', 'ring_front_left', 'ring_front_right', 'ring_rear_left', 'ring_rear_right', 'ring_side_left', - 'ring_side_right'] - - - -# count total number of files -total_number=0 -for q in argoverse_loader.log_list: - path, dirs, files = next(os.walk(data_dir+q+'/lidar')) - total_number= total_number+len(files) - -total_number= total_number*7 - -bar = progressbar.ProgressBar(maxval=total_number, \ - widgets=[progressbar.Bar('=', '[', ']'), ' ', progressbar.Percentage()]) + 'ring_side_right'] # All available cameras +cam_id = 0 # Choose only one of camera as reference. TODO: save labels for all cameras +cam = cams_all[cam_id] + + +sample_rate = 1 # Sample rate to avoid sequential data (original 10Hz) + + +remove_ground = False # Remove ground lidar points when generating /velodyne + +# Map info +create_map = False # When set True, create drivable road maps and ground maps as rasterized maps with 1x1 meters +map_x_limit = [-40, 40] # lateral distance +map_y_limit = [0, 70] # longitudinal distance +raster_size = 1.0 # argoverse map resolution (meter) + +create_map_semantics = True # When set True, append lidar map features to "velodyne" + +if create_map or create_map_semantics or remove_ground: + from argoverse.map_representation.map_api import ArgoverseMap + argoverse_map = ArgoverseMap() + +# Official categorization from the paper "Train in Germany, Test in The USA: Making 3D Object Detectors Generalize", from Xiangyu Chen et al. CVPR2020 +# Information provided by Xiangyu Chen by courtesy xc429@cornell.edu +CLASS_MAP = { + "VEHICLE": "Car", + "PEDESTRIAN": "Pedestrian", + "LARGE_VEHICLE": "Truck", + "BICYCLIST": "Cyclist", + "BUS": "Truck", + "TRAILER": "Truck", + "MOTORCYCLIST": "Cyclist", + "EMERGENCY_VEHICLE": "Van", + "SCHOOL_BUS": "Truck" +} + + +# Save data conversion configuration +config = '=========== Argo2KITTI conversion configuration ========== \n' + \ + 'sample_rate = {}'.format(sample_rate) + '\n' + \ + 'max_d = {}'.format(max_d) + '\n' + \ + 'full_range_lidar = {}'.format(full_range_lidar) + '\n' + \ + 'selected_camera = ' + cams_all[cam_id] + '\n' + \ + 'remove_ground = {}'.format(remove_ground) + '\n' + \ + 'create_map = {}'.format(create_map) + '\n' + \ + 'create_map_semantics = {}'.format(create_map_semantics) + '\n' + \ + '===========================================================' +print(config) +with open(os.path.join(goal_dir, 'config.txt'),'a') as f: + f.write(config) +#################################################################### -print('Total number of files: {}. Translation starts...'.format(total_number)) -print('Progress:') -bar.start() -i= 0 - -for log_id in argoverse_loader.log_list: - argoverse_data= argoverse_loader.get(log_id) - for cam in cams: - # Recreate the calibration file content - calibration_data=calibration.load_calib(data_dir+log_id+'/vehicle_calibration_info.json')[cam] - L3='P2: ' - for j in calibration_data.K.reshape(1,12)[0]: - L3= L3+ str(j)+ ' ' - L3=L3[:-1] - - L6= 'Tr_velo_to_cam: ' - for k in calibration_data.extrinsic.reshape(1,16)[0][0:12]: - L6= L6+ str(k)+ ' ' - L6=L6[:-1] - - - L1='P0: 0 0 0 0 0 0 0 0 0 0 0 0' - L2='P1: 0 0 0 0 0 0 0 0 0 0 0 0' - L4='P3: 0 0 0 0 0 0 0 0 0 0 0 0' - L5='R0_rect: 1 0 0 0 1 0 0 0 1' - L7='Tr_imu_to_velo: 0 0 0 0 0 0 0 0 0 0 0 0' - - file_content="""{} -{} -{} -{} -{} -{} -{} - """.format(L1,L2,L3,L4,L5,L6,L7) - l=0 - - - # Loop through the each lidar frame (10Hz) to copy and reconfigure all images, lidars, calibration files, and label files. - for timestamp in argoverse_data.lidar_timestamp_list: - - # Save lidar file into .bin format under the new directory - lidar_file_path= data_dir + log_id + '/lidar/PC_' + str(timestamp) + '.ply' - target_lidar_file_path= goal_dir + 'velodyne/'+ str(i).zfill(6) + '.bin' - - lidar_data=load_ply(lidar_file_path) - lidar_data_augmented=np.concatenate((lidar_data,np.zeros([lidar_data.shape[0],1])),axis=1) - lidar_data_augmented=lidar_data_augmented.astype('float32') - lidar_data_augmented.tofile(target_lidar_file_path) - - # Save the image file into .png format under the new directory - cam_file_path=argoverse_data.image_list_sync[cam][l] - target_cam_file_path= goal_dir +'image_2/' + str(i).zfill(6) + '.png' - copyfile(cam_file_path,target_cam_file_path) - - file=open(goal_dir+'calib/' + str(i).zfill(6) + '.txt','w+') - file.write(file_content) - file.close() - - label_object_list= argoverse_data.get_label_object(l) - file=open(goal_dir + 'label_2/' + str(i).zfill(6) + '.txt','w+') - l+=1 - - for detected_object in label_object_list: - classes= detected_object.label_class - occulusion= round(detected_object.occlusion/25) - height= detected_object.height - length= detected_object.length - width= detected_object.width - truncated= 0 - - center= detected_object.translation # in ego frame - - corners_ego_frame=detected_object.as_3d_bbox() # all eight points in ego frame - corners_cam_frame= calibration_data.project_ego_to_cam(corners_ego_frame) # all eight points in the camera frame - image_corners= calibration_data.project_ego_to_image(corners_ego_frame) - image_bbox= [min(image_corners[:,0]), min(image_corners[:,1]),max(image_corners[:,0]),max(image_corners[:,1])] - # the four coordinates we need for KITTI - image_bbox=[round(x) for x in image_bbox] - - center_cam_frame= calibration_data.project_ego_to_cam(np.array([center])) - - if 0 0 + valid = (0 <= image_bbox[1] < RING_IMG_HEIGHT or 0 < image_bbox[3] <= RING_IMG_HEIGHT) and ( + 0 <= image_bbox[0] < RING_IMG_WIDTH or 0 < image_bbox[2] <= RING_IMG_WIDTH) and np.min(corners_cam_frame[:, 2], axis=0) > 0 and detected_object.translation[0] > 0 + + + if full_range_lidar: + label_keep = (-max_d<=center_cam_frame[0][2]<=max_d) and (-max_d<=center_cam_frame[0][0]<=max_d) + else: + label_keep = 0.txt are in rect camera coord. + 2d box xy are in image2 coord + Points in .bin are in Velodyne coord. + y_image2 = P^2_rect * x_rect + y_image2 = P^2_rect * R0_rect * Tr_velo_to_cam * x_velo + x_ref = Tr_velo_to_cam * x_velo + x_rect = R0_rect * x_ref + P^2_rect = [f^2_u, 0, c^2_u, -f^2_u b^2_x; + 0, f^2_v, c^2_v, -f^2_v b^2_y; + 0, 0, 1, 0] + = K * [1|t] + image2 coord: + ----> x-axis (u) + | + | + v y-axis (v) + velodyne coord: + front x, left y, up z + rect/ref camera coord: + right x, down y, front z + Ref (KITTI paper): http://www.cvlibs.net/publications/Geiger2013IJRR.pdf + TODO(rqi): do matrix multiplication only once for each projection. + ''' + def __init__(self, calib_filepath, from_video=False): + if from_video: + calibs = self.read_calib_from_video(calib_filepath) + else: + calibs = self.read_calib_file(calib_filepath) + # Projection matrix from rect camera coord to image2 coord + self.P = calibs['P2'] + self.P = np.reshape(self.P, [3,4]) + # Rigid transform from Velodyne coord to reference camera coord + self.V2C = calibs['Tr_velo_to_cam'] + self.V2C = np.reshape(self.V2C, [3,4]) + self.C2V = inverse_rigid_trans(self.V2C) + # Rotation from reference camera coord to rect camera coord + self.R0 = calibs['R0_rect'] + self.R0 = np.reshape(self.R0,[3,3]) + + # Camera intrinsics and extrinsics + self.c_u = self.P[0,2] + self.c_v = self.P[1,2] + self.f_u = self.P[0,0] + self.f_v = self.P[1,1] + self.b_x = self.P[0,3]/(-self.f_u) # relative + self.b_y = self.P[1,3]/(-self.f_v) + + def read_calib_file(self, filepath): + ''' Read in a calibration file and parse into a dictionary. + Ref: https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py + ''' + data = {} + with open(filepath, 'r') as f: + for line in f.readlines(): + line = line.rstrip() + if len(line)==0: continue + key, value = line.split(':', 1) + # The only non-float values in these files are dates, which + # we don't care about anyway + try: + data[key] = np.array([float(x) for x in value.split()]) + except ValueError: + pass + + return data + + def read_calib_from_video(self, calib_root_dir): + ''' Read calibration for camera 2 from video calib files. + there are calib_cam_to_cam and calib_velo_to_cam under the calib_root_dir + ''' + data = {} + cam2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_cam_to_cam.txt')) + velo2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_velo_to_cam.txt')) + Tr_velo_to_cam = np.zeros((3,4)) + Tr_velo_to_cam[0:3,0:3] = np.reshape(velo2cam['R'], [3,3]) + Tr_velo_to_cam[:,3] = velo2cam['T'] + data['Tr_velo_to_cam'] = np.reshape(Tr_velo_to_cam, [12]) + data['R0_rect'] = cam2cam['R_rect_00'] + data['P2'] = cam2cam['P_rect_02'] + return data + + def cart2hom(self, pts_3d): + ''' Input: nx3 points in Cartesian + Oupput: nx4 points in Homogeneous by pending 1 + ''' + n = pts_3d.shape[0] + pts_3d_hom = np.hstack((pts_3d, np.ones((n,1)))) + return pts_3d_hom + + # =========================== + # ------- 3d to 3d ---------- + # =========================== + def project_velo_to_ref(self, pts_3d_velo): + pts_3d_velo = self.cart2hom(pts_3d_velo) # nx4 + return np.dot(pts_3d_velo, np.transpose(self.V2C)) + + def project_ref_to_velo(self, pts_3d_ref): + pts_3d_ref = self.cart2hom(pts_3d_ref) # nx4 + return np.dot(pts_3d_ref, np.transpose(self.C2V)) + + def project_rect_to_ref(self, pts_3d_rect): + ''' Input and Output are nx3 points ''' + return np.transpose(np.dot(np.linalg.inv(self.R0), np.transpose(pts_3d_rect))) + + def project_ref_to_rect(self, pts_3d_ref): + ''' Input and Output are nx3 points ''' + return np.transpose(np.dot(self.R0, np.transpose(pts_3d_ref))) + + def project_rect_to_velo(self, pts_3d_rect): + ''' Input: nx3 points in rect camera coord. + Output: nx3 points in velodyne coord. + ''' + pts_3d_ref = self.project_rect_to_ref(pts_3d_rect) + return self.project_ref_to_velo(pts_3d_ref) + + def project_velo_to_rect(self, pts_3d_velo): + pts_3d_ref = self.project_velo_to_ref(pts_3d_velo) + return self.project_ref_to_rect(pts_3d_ref) + + # =========================== + # ------- 3d to 2d ---------- + # =========================== + def project_rect_to_image(self, pts_3d_rect): + ''' Input: nx3 points in rect camera coord. + Output: nx2 points in image2 coord. + ''' + pts_3d_rect = self.cart2hom(pts_3d_rect) + pts_2d = np.dot(pts_3d_rect, np.transpose(self.P)) # nx3 + pts_2d[:,0] /= pts_2d[:,2] + pts_2d[:,1] /= pts_2d[:,2] + return pts_2d[:,0:2] + + def project_velo_to_image(self, pts_3d_velo): + ''' Input: nx3 points in velodyne coord. + Output: nx2 points in image2 coord. + ''' + pts_3d_rect = self.project_velo_to_rect(pts_3d_velo) + return self.project_rect_to_image(pts_3d_rect) + + # =========================== + # ------- 2d to 3d ---------- + # =========================== + def project_image_to_rect(self, uv_depth): + ''' Input: nx3 first two channels are uv, 3rd channel + is depth in rect camera coord. + Output: nx3 points in rect camera coord. + ''' + n = uv_depth.shape[0] + x = ((uv_depth[:,0]-self.c_u)*uv_depth[:,2])/self.f_u + self.b_x + y = ((uv_depth[:,1]-self.c_v)*uv_depth[:,2])/self.f_v + self.b_y + pts_3d_rect = np.zeros((n,3)) + pts_3d_rect[:,0] = x + pts_3d_rect[:,1] = y + pts_3d_rect[:,2] = uv_depth[:,2] + return pts_3d_rect + + def project_image_to_velo(self, uv_depth): + pts_3d_rect = self.project_image_to_rect(uv_depth) + return self.project_rect_to_velo(pts_3d_rect) + + +def rotx(t): + ''' 3D Rotation about the x-axis. ''' + c = np.cos(t) + s = np.sin(t) + return np.array([[1, 0, 0], + [0, c, -s], + [0, s, c]]) + + +def roty(t): + ''' Rotation about the y-axis. ''' + c = np.cos(t) + s = np.sin(t) + return np.array([[c, 0, s], + [0, 1, 0], + [-s, 0, c]]) + + +def rotz(t): + ''' Rotation about the z-axis. ''' + c = np.cos(t) + s = np.sin(t) + return np.array([[c, -s, 0], + [s, c, 0], + [0, 0, 1]]) + + +def transform_from_rot_trans(R, t): + ''' Transforation matrix from rotation matrix and translation vector. ''' + R = R.reshape(3, 3) + t = t.reshape(3, 1) + return np.vstack((np.hstack([R, t]), [0, 0, 0, 1])) + + +def inverse_rigid_trans(Tr): + ''' Inverse a rigid body transform matrix (3x4 as [R|t]) + [R'|-R't; 0|1] + ''' + inv_Tr = np.zeros_like(Tr) # 3x4 + inv_Tr[0:3,0:3] = np.transpose(Tr[0:3,0:3]) + inv_Tr[0:3,3] = np.dot(-np.transpose(Tr[0:3,0:3]), Tr[0:3,3]) + return inv_Tr + +def read_label(label_filename): + lines = [line.rstrip() for line in open(label_filename)] + objects = [Object3d(line) for line in lines] + return objects + +def load_image(img_filename): + return cv2.imread(img_filename) + +def load_velo_scan(velo_filename): + scan = np.fromfile(velo_filename, dtype=np.float32) + scan = scan.reshape((-1, 4)) + return scan + +def project_to_image(pts_3d, P): + ''' Project 3d points to image plane. + Usage: pts_2d = projectToImage(pts_3d, P) + input: pts_3d: nx3 matrix + P: 3x4 projection matrix + output: pts_2d: nx2 matrix + P(3x4) dot pts_3d_extended(4xn) = projected_pts_2d(3xn) + => normalize projected_pts_2d(2xn) + <=> pts_3d_extended(nx4) dot P'(4x3) = projected_pts_2d(nx3) + => normalize projected_pts_2d(nx2) + ''' + n = pts_3d.shape[0] + pts_3d_extend = np.hstack((pts_3d, np.ones((n,1)))) + print(('pts_3d_extend shape: ', pts_3d_extend.shape)) + pts_2d = np.dot(pts_3d_extend, np.transpose(P)) # nx3 + pts_2d[:,0] /= pts_2d[:,2] + pts_2d[:,1] /= pts_2d[:,2] + return pts_2d[:,0:2] + + +def compute_box_3d(obj, P): + ''' Takes an object and a projection matrix (P) and projects the 3d + bounding box into the image plane. + Returns: + corners_2d: (8,2) array in left image coord. + corners_3d: (8,3) array in in rect camera coord. + ''' + # compute rotational matrix around yaw axis + R = roty(obj.ry) + + # 3d bounding box dimensions + l = obj.l; + w = obj.w; + h = obj.h; + + # 3d bounding box corners + x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]; + y_corners = [0,0,0,0,-h,-h,-h,-h]; + z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]; + + # rotate and translate 3d bounding box + corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners])) + #print corners_3d.shape + corners_3d[0,:] = corners_3d[0,:] + obj.t[0]; + corners_3d[1,:] = corners_3d[1,:] + obj.t[1]; + corners_3d[2,:] = corners_3d[2,:] + obj.t[2]; + #print 'cornsers_3d: ', corners_3d + # only draw 3d bounding box for objs in front of the camera + if np.any(corners_3d[2,:]<0.1): + corners_2d = None + return corners_2d, np.transpose(corners_3d) + + # project the 3d bounding box into the image plane + corners_2d = project_to_image(np.transpose(corners_3d), P); + #print 'corners_2d: ', corners_2d + return corners_2d, np.transpose(corners_3d) + + +def compute_orientation_3d(obj, P): + ''' Takes an object and a projection matrix (P) and projects the 3d + object orientation vector into the image plane. + Returns: + orientation_2d: (2,2) array in left image coord. + orientation_3d: (2,3) array in in rect camera coord. + ''' + + # compute rotational matrix around yaw axis + R = roty(obj.ry) + + # orientation in object coordinate system + orientation_3d = np.array([[0.0, obj.l],[0,0],[0,0]]) + + # rotate and translate in camera coordinate system, project in image + orientation_3d = np.dot(R, orientation_3d) + orientation_3d[0,:] = orientation_3d[0,:] + obj.t[0] + orientation_3d[1,:] = orientation_3d[1,:] + obj.t[1] + orientation_3d[2,:] = orientation_3d[2,:] + obj.t[2] + + # vector behind image plane? + if np.any(orientation_3d[2,:]<0.1): + orientation_2d = None + return orientation_2d, np.transpose(orientation_3d) + + # project orientation into the image plane + orientation_2d = project_to_image(np.transpose(orientation_3d), P); + return orientation_2d, np.transpose(orientation_3d) + +def draw_projected_box3d(image, qs, color=(255,255,255), thickness=2): + ''' Draw 3d bounding box in image + qs: (8,3) array of vertices for the 3d box in following order: + 1 -------- 0 + /| /| + 2 -------- 3 . + | | | | + . 5 -------- 4 + |/ |/ + 6 -------- 7 + ''' + qs = qs.astype(np.int32) + for k in range(0,4): + # Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html + i,j=k,(k+1)%4 + # use LINE_AA for opencv3 + cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA) + + i,j=k+4,(k+1)%4 + 4 + cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA) + + i,j=k,k+4 + cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA) + return image \ No newline at end of file diff --git a/kitti_visulization/plain_vis.py b/kitti_visulization/plain_vis.py new file mode 100644 index 0000000..ce5ec73 --- /dev/null +++ b/kitti_visulization/plain_vis.py @@ -0,0 +1,62 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.patches as patches +import yaml +import pandas as pd +from kitti_util import * +from pylab import * +from matplotlib.lines import Line2D +import matplotlib.gridspec as gridspec + +goal_dir = '/media/vision/HDD Storage/data/argoverse/argoverse-tracking/argoverse-kitti/training' +imageset_dir = os.path.join(goal_dir, '..', 'ImageSets') +lidar_dir = os.path.join(goal_dir, 'velodyne') +ground_dir = os.path.join(goal_dir, 'velodyne_semantics') +if not os.path.exists('image_plane'): os.mkdir('image_plane') + +train_id_list = [x.strip() for x in open(os.path.join(imageset_dir, 'train.txt')).readlines()] +for train_id in train_id_list[:20]: + print(train_id) + lidar_data = np.fromfile(os.path.join(lidar_dir, train_id + '.bin'), dtype=np.float32).reshape(-1,4) # x,y,z,intensity + semantic_data = np.fromfile(os.path.join(ground_dir, train_id + '.bin'), dtype=np.float32).reshape(-1,3) # ground_height, lidar_ground_bool,drivable_area_bool + + plt.figure(figsize=(15,6)) + gs = gridspec.GridSpec(5, 15) + ax = plt.subplot(gs[:4, :5]) + plt.scatter(lidar_data[:,0], lidar_data[:,1], marker='.', c='blue', alpha=0.7, s=0.1) + plt.title('Full LiDAR points on BEV') + plt.xlim([-80,80]) + plt.ylim([-80,80]) + plt.tight_layout() + + ax = plt.subplot(gs[:4, 5:10]) + plt.scatter(lidar_data[semantic_data[:,2]==1,0], lidar_data[semantic_data[:,2]==1,1], marker='.', c='red', alpha=0.7, s=0.1, label='drivable area') + plt.scatter(lidar_data[semantic_data[:,2]==0,0], lidar_data[semantic_data[:,2]==0,1], marker='.', c='blue', alpha=0.7, s=0.1, label='non drivable area') + plt.title('Drivable LiDAR points on BEV') + plt.legend() + plt.xlim([-80,80]) + plt.ylim([-80,80]) + plt.tight_layout() + + ax = plt.subplot(gs[:4, 10:15]) + plt.scatter(lidar_data[semantic_data[:,1]==1,0], lidar_data[semantic_data[:,1]==1,1], marker='.', c='green', alpha=0.7, s=0.1, label='ground') + plt.scatter(lidar_data[semantic_data[:,1]==0,0], lidar_data[semantic_data[:,1]==0,1], marker='.', c='blue', alpha=0.7, s=0.1, label='non ground') + plt.title('Ground LiDAR points on BEV') + plt.legend() + plt.xlim([-80,80]) + plt.ylim([-80,80]) + plt.tight_layout() + + ax = plt.subplot(gs[4,:15]) + plt.scatter(lidar_data[semantic_data[:,2]==1,1], lidar_data[semantic_data[:,2]==1,2], marker='.', c='blue', alpha=0.7, s=0.1, label='original pcl') + plt.scatter(lidar_data[semantic_data[:,2]==1,1], semantic_data[semantic_data[:,2]==1,0], marker='.', c='m', alpha=0.7, s=0.1, label='ground pcl') + plt.title('Orignal pcl and corr. ground') + plt.legend() + plt.xlim([-40,40]) + plt.ylim([-5, 5]) + plt.tight_layout() + + plt.savefig('image_plane/'+train_id+'.png', dpi=150) + plt.clf() \ No newline at end of file diff --git a/kitti_visulization/plot.py b/kitti_visulization/plot.py new file mode 100644 index 0000000..f0f4ac6 --- /dev/null +++ b/kitti_visulization/plot.py @@ -0,0 +1,376 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.patches as patches +import yaml +import pandas as pd +from kitti_util import * +from pylab import * +from matplotlib.lines import Line2D +import pickle +import pandas as pd + + +# ============================================================================== +# POINT_CLOUD_2_BIRDSEYE +# ============================================================================== +def point_cloud_2_top(points, + res=0.2, + zres=5, + side_range=(-20., 20-0.05), # left-most to right-most + fwd_range=(0., 40.-0.05), # back-most to forward-most + height_range=(-2., 2), # bottom-most to upper-most + ): + """ Creates an birds eye view representation of the point cloud data for MV3D. + Args: + points: (numpy array) + N rows of points data + Each point should be specified by at least 3 elements x,y,z + res: (float) + Desired resolution in metres to use. Each output pixel will + represent an square region res x res in size. + zres: (float) + Desired resolution on Z-axis in metres to use. + side_range: (tuple of two floats) + (-left, right) in metres + left and right limits of rectangle to look at. + fwd_range: (tuple of two floats) + (-behind, front) in metres + back and front limits of rectangle to look at. + height_range: (tuple of two floats) + (min, max) heights (in metres) relative to the origin. + All height values will be clipped to this min and max value, + such that anything below min will be truncated to min, and + the same for values above max. + Returns: + numpy array encoding height features , density and intensity. + """ + # EXTRACT THE POINTS FOR EACH AXIS + x_points = points[:, 0] + y_points = points[:, 1] + z_points = points[:, 2] + reflectance = points[:,3] + + # INITIALIZE EMPTY ARRAY - of the dimensions we want + x_max = int((side_range[1] - side_range[0]) / res) + y_max = int((fwd_range[1] - fwd_range[0]) / res) + z_max = int((height_range[1] - height_range[0]) / zres) + top = np.zeros([y_max+1, x_max+1, z_max+1], dtype=np.float32) + + # FILTER - To return only indices of points within desired cube + # Three filters for: Front-to-back, side-to-side, and height ranges + # Note left side is positive y axis in LIDAR coordinates + f_filt = np.logical_and( + (x_points > fwd_range[0]), (x_points < fwd_range[1])) + s_filt = np.logical_and( + (y_points > -side_range[1]), (y_points < -side_range[0])) + filt = np.logical_and(f_filt, s_filt) + + for i, height in enumerate(np.arange(height_range[0], height_range[1], zres)): + + z_filt = np.logical_and((z_points >= height), + (z_points < height + zres)) + zfilter = np.logical_and(filt, z_filt) + indices = np.argwhere(zfilter).flatten() + + # KEEPERS + xi_points = x_points[indices] + yi_points = y_points[indices] + zi_points = z_points[indices] + ref_i = reflectance[indices] + + # CONVERT TO PIXEL POSITION VALUES - Based on resolution + x_img = (-yi_points / res).astype(np.int32) # x axis is -y in LIDAR + y_img = (-xi_points / res).astype(np.int32) # y axis is -x in LIDAR + + # SHIFT PIXELS TO HAVE MINIMUM BE (0,0) + # floor & ceil used to prevent anything being rounded to below 0 after + # shift + x_img -= int(np.floor(side_range[0] / res)) + y_img += int(np.floor(fwd_range[1] / res)) + + # CLIP HEIGHT VALUES - to between min and max heights + pixel_values = zi_points - height_range[0] + # pixel_values = zi_points + + # FILL PIXEL VALUES IN IMAGE ARRAY + top[y_img, x_img, i] = pixel_values + + # max_intensity = np.max(prs[idx]) + top[y_img, x_img, z_max] = ref_i + + top = (top / np.max(top) * 255).astype(np.uint8) + return top + +def transform_to_img(xmin, xmax, ymin, ymax, + res=0.2, + side_range=(-20., 20-0.05), # left-most to right-most + fwd_range=(0., 40.-0.05), # back-most to forward-most + ): + + xmin_img = -ymax/res - side_range[0]/res + xmax_img = -ymin/res - side_range[0]/res + ymin_img = -xmax/res + fwd_range[1]/res + ymax_img = -xmin/res + fwd_range[1]/res + + return xmin_img, xmax_img, ymin_img, ymax_img + + +def draw_point_cloud(ax, points, axes=[0, 1, 2], point_size=0.1, xlim3d=None, ylim3d=None, zlim3d=None): + """ + Convenient method for drawing various point cloud projections as a part of frame statistics. + """ + axes_limits = [ + [-20, 80], # X axis range + [-40, 40], # Y axis range + [-3, 3] # Z axis range + ] + axes_str = ['X', 'Y', 'Z'] + ax.grid(False) + + ax.scatter(*np.transpose(points[:, axes]), s=point_size, c=points[:, 3], cmap='gray') + ax.set_xlabel('{} axis'.format(axes_str[axes[0]])) + ax.set_ylabel('{} axis'.format(axes_str[axes[1]])) + if len(axes) > 2: + ax.set_xlim3d(*axes_limits[axes[0]]) + ax.set_ylim3d(*axes_limits[axes[1]]) + ax.set_zlim3d(*axes_limits[axes[2]]) + ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + ax.set_zlabel('{} axis'.format(axes_str[axes[2]])) + else: + ax.set_xlim(*axes_limits[axes[0]]) + ax.set_ylim(*axes_limits[axes[1]]) + # User specified limits + if xlim3d!=None: + ax.set_xlim3d(xlim3d) + if ylim3d!=None: + ax.set_ylim3d(ylim3d) + if zlim3d!=None: + ax.set_zlim3d(zlim3d) + +def compute_3d_box_cam2(h, w, l, x, y, z, yaw): + """ + Return : 3xn in cam2 coordinate + """ + R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]]) + x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2] + y_corners = [0,0,0,0,-h,-h,-h,-h] + z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2] + corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners])) + corners_3d_cam2 += np.vstack([x, y, z]) + return corners_3d_cam2 + +def draw_box(ax, vertices, axes=[0, 1, 2], color='black'): + """ + Draws a bounding 3D box in a pyplot axis. + + Parameters + ---------- + pyplot_axis : Pyplot axis to draw in. + vertices : Array 8 box vertices containing x, y, z coordinates. + axes : Axes to use. Defaults to `[0, 1, 2]`, e.g. x, y and z axes. + color : Drawing color. Defaults to `black`. + """ + vertices = vertices[axes, :] + connections = [ + [0, 1], [1, 2], [2, 3], [3, 0], # Lower plane parallel to Z=0 plane + [4, 5], [5, 6], [6, 7], [7, 4], # Upper plane parallel to Z=0 plane + [0, 4], [1, 5], [2, 6], [3, 7] # Connections between upper and lower planes + ] + for connection in connections: + ax.plot(*vertices[:, connection], c=color, lw=0.5) + + +def read_detection(path, score=False): + if score: + df = pd.read_csv(path, header=None, sep=' ', usecols=range(16)) + df.columns = ['type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top', + 'bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y', 'score'] + else: + df = pd.read_csv(path, header=None, sep=' ', usecols=range(15)) + df.columns = ['type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top', + 'bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y'] + + df = df[df['type']!='Truck'] + df = df[df['type']!='Van'] + + df.reset_index(drop=True, inplace=True) + return df + + +def transform_detection_kitti_format(root_dir, model, epoch): + """ + transform detection from .pkl file to standard kitti file + """ + path = root_dir +'dfeng/lidarMTL/output' + root_dir + 'dfeng/lidarMTL/tools/cfgs/kitti_models/' \ + + model + '/default/eval/epoch_' + str(epoch) + '/val20/default' + result = pd.read_pickle(path+'/result.pkl') + + if not os.path.exists(model): os.mkdir(model) + if not os.path.exists(model+'/'+str(epoch)): os.mkdir(model+'/'+str(epoch)) + target_dir = model+'/'+str(epoch) + '/label_2' + if not os.path.exists(target_dir): os.mkdir(target_dir) + + for detected_frame in result: + frame_id = detected_frame['frame_id'] + name = detected_frame['name'] + truncated = detected_frame['truncated'] + occluded = detected_frame['occluded'] + alpha = detected_frame['alpha'] + bbox = detected_frame['bbox'] + dimensions = detected_frame['dimensions'] # lhw -> hwl + location = detected_frame['location'] + rotation_y = detected_frame['rotation_y'] + score = detected_frame['score'] + + ''' + #Values Name Description + ---------------------------------------------------------------------------- + 1 type Describes the type of object: 'Car', 'Van', 'Truck', + 'Pedestrian', 'Person_sitting', 'Cyclist', 'Tram', + 'Misc' or 'DontCare' + 1 truncated Float from 0 (non-truncated) to 1 (truncated), where + truncated refers to the object leaving image boundaries + 1 occluded Integer (0,1,2,3) indicating occlusion state: + 0 = fully visible, 1 = partly occluded + 2 = largely occluded, 3 = unknown + 1 alpha Observation angle of object, ranging [-pi..pi] + 4 bbox 2D bounding box of object in the image (0-based index): + contains left, top, right, bottom pixel coordinates + 3 dimensions 3D object dimensions: height, width, length (in meters) + 3 location 3D object location x,y,z in camera coordinates (in meters) + 1 rotation_y Rotation ry around Y-axis in camera coordinates [-pi..pi] + 1 score Only for results: Float, indicating confidence in + detection, needed for p/r curves, higher is better. + ''' + file = open(target_dir + '/' + frame_id + '.txt','w+') + for n, t, o, a, b, d, l, ry, s in zip(name, truncated, occluded, alpha, bbox, dimensions, location, rotation_y, score): + line = n + ' {} {} {} {} {} {} {} {} {} {} {} {} {} {} {} \n'.format( + float(t), + float(o), + float(a), + float(b[0]), + float(b[1]), + float(b[2]), + float(b[3]), + float(d[1]), #height + float(d[2]), #width + float(d[0]), #length + float(l[0]), + float(l[1]), + float(l[2]), + float(ry), + float(s)) + file.write(line) + file.close() + +def draw_bbox(ax, objects, calib, gt=False, color='cyan'): + for o in range(len(objects)): + corners_3d_cam2 = compute_3d_box_cam2(*objects.loc[o, ['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']]) + corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T) + x1,x2,x3,x4 = corners_3d_velo[0:4,0] + y1,y2,y3,y4 = corners_3d_velo[0:4,1] + ''' + xmax = np.max(corners_3d_velo[:, 0]) + xmin = np.min(corners_3d_velo[:, 0]) + ymax = np.max(corners_3d_velo[:, 1]) + ymin = np.min(corners_3d_velo[:, 1]) + ''' + x1, x2, y1, y2 = transform_to_img(x1, x2, y1, y2, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05)) + x3, x4, y3, y4 = transform_to_img(x3, x4, y3, y4, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05)) + ps=[] + polygon = np.zeros([5,2], dtype = np.float32) + polygon[0,0] = x1 + polygon[1,0] = x2 + polygon[2,0] = x3 + polygon[3,0] = x4 + polygon[4,0] = x1 + + polygon[0,1] = y1 + polygon[1,1] = y2 + polygon[2,1] = y3 + polygon[3,1] = y4 + polygon[4,1] = y1 + + line1 = [(x1,y1), (x2,y2)] + line2 = [(x2,y2), (x3,y3)] + line3 = [(x3,y3), (x4,y4)] + line4 = [(x4,y4), (x1,y1)] + (line1_xs, line1_ys) = zip(*line1) + (line2_xs, line2_ys) = zip(*line2) + (line3_xs, line3_ys) = zip(*line3) + (line4_xs, line4_ys) = zip(*line4) + ax.add_line(Line2D(line1_xs, line1_ys, linewidth=2, color=color)) + ax.add_line(Line2D(line2_xs, line2_ys, linewidth=2, color=color)) + ax.add_line(Line2D(line3_xs, line3_ys, linewidth=2, color=color)) + ax.add_line(Line2D(line4_xs, line4_ys, linewidth=2, color=color)) + + if gt: + line5 = [(x1,y1), (x3,y3)] + line6 = [(x2,y2), (x4,y4)] + (line5_xs, line5_ys) = zip(*line5) + (line6_xs, line6_ys) = zip(*line6) + ax.add_line(Line2D(line5_xs, line5_ys, linewidth=2, color=color)) + ax.add_line(Line2D(line6_xs, line6_ys, linewidth=2, color=color)) + + if 'score' in objects.columns: + classes = objects.loc[o, ['type']] + score = float(objects.loc[o, ['score']])*100 + ax.text(0.5*(x1+x3), 0.5*(y1+y3), str(int(score)), color='red') + + return ax + +def plot_single_frame(img_id, data_dir, det_model='pv_rcnn_backbone_argo_v1_D', epoch=80): + print('plot frame ', img_id) + calib_path = data_dir+'/training/calib/%06d.txt'%img_id + label_path = data_dir+'/training/label_2/%06d.txt'%img_id + points_path = data_dir+'/training/velodyne/%06d.bin'%img_id + + if os.path.exists(calib_path) and len(np.genfromtxt(label_path,dtype='str'))>0: + calib = Calibration(calib_path) + points = np.fromfile(points_path, dtype=np.float32).reshape(-1, 4) + df = read_detection(label_path) + + fig = plt.figure(figsize=(10, 10)) + ax = plt.subplot(111) + + top = point_cloud_2_top(points, zres=1, side_range=(-40., 40-0.05), fwd_range=(0., 80.-0.05), height_range=(-1.5, 4)) + top = np.array(top, dtype = np.float32) + top[top>0] = 1 + top = np.mean(top, axis=2) + #top[top>0] = 1 + + ax.imshow(top, aspect='equal', cmap=plt.cm.gray) + + # draw ground truth + ax = draw_bbox(ax, df, calib, gt=True, color='lime') + + # draw detections + if det_model is not None: + det = read_detection(det_model +'/'+str(epoch) + '/label_2/%06d.txt'%img_id, score=True) + ax = draw_bbox(ax, det, calib, color='blue') + + plt.axis('off') + + image_dir = 'image' if det_model is None else det_model +'/'+str(epoch) + '/image' + if not os.path.exists(image_dir): os.mkdir(image_dir) + + plt.savefig(image_dir + '/' + '%06d.png'%img_id) + plt.clf() + + +if __name__ == "__main__": + ########### CONFIGURATION ############## + root_dir = '/media/vision/HDD Storage/' + data_dir = root_dir + '/data/argoverse/argoverse-kitti' + model = 'PartA2_stage1_argo_multi' # prediction + epoch = 80 # epoch to evaluate + ######################################## + + transform_detection_kitti_format(root_dir, model, epoch) + + #for img_id in range(14000,18000,250): + # plot_single_frame(img_id, data_dir, det_model=model, epoch=epoch)