def __init__(self, basedir, transform=None, gen_func=None, args=None, viz=False): self.data = [ pykitti.tracking(basedir, sequence) for sequence in sequences ] calib_locations = [ basedir + "calib/" + sequence + '.txt' for sequence in sequences ] self.calibs = [ read_calib_file(calib_location) for calib_location in calib_locations ] self.R0 = [calibs_['R_rect'] for calibs_ in self.calibs] self.R0 = [np.reshape(R0_, [3, 3]) for R0_ in self.R0] self.V2C = [calibs_['Tr_velo_cam'] for calibs_ in self.calibs] self.V2C = [np.reshape(V2C_, [3, 4]) for V2C_ in self.V2C] self.C2V = [inverse_rigid_trans(V2C_) for V2C_ in self.V2C] self.tracklets_list = populate_train_list(basedir) self.transform = transform self.gen_func = gen_func self.num_points = args.num_points self.remove_ground = args.remove_ground self.viz = viz
def test_plot_bevs(self): config_path = 'cfg_mini.yml' config = load_config(config_path) input_config = InputConfig(config['INPUT_CONFIG']) train_config = TrainConfig(config['TRAIN_CONFIG']) basepath = "/Users/erikbohnsack/data/training" tracking = pykitti.tracking(base_path=basepath, sequence="0000") points = tracking.get_velo(0) bev_map = np.zeros(input_config.bev_shape) print("BEV map size: {}".format(bev_map.shape)) grid_limits = np.array([[input_config.x_min, input_config.x_max], [input_config.y_min, input_config.y_max], [input_config.z_min, input_config.z_max]]) grid_sizes = np.array([ input_config.x_grid_size, input_config.y_grid_size, input_config.z_grid_size ]) start_jit = time.time() kitti_dataset.format_to_BEV_jit(bev_map, points, grid_limits, grid_sizes) end_jit = time.time() plot_BEV(bev_map) print("Elapsed time: {}".format(end_jit - start_jit))
def populate_train_list(basedir): tracklets_list = [] for sequence in sequences: data = pykitti.tracking(basedir, sequence) data_dict = {} for i in range(len(data)): objects = data.get_objects(i) for obj in objects: if (obj.type == 'DontCare'): continue if (obj.track_id in data_dict.keys()): data_dict[obj.track_id].append(i) else: data_dict[obj.track_id] = [i] for key in data_dict.keys(): for i in range(len(data_dict[key]) - 1): if (data_dict[key][i + 1] - data_dict[key][i] == 1): tracklets_list.append([ data_dict[key][i], data_dict[key][i + 1], key, sequence ]) return tracklets_list
def __init__(self, basedir, drive): # TODO: finish configurations for kitti tracking dataset self.data = pykitti.tracking(basedir, drive) self.original_shape = (1242, 375) self.operate_folder_name = 'image_02' self.image_folder_name = 'data' self.image_ext = 'png' self.dets_name = 'mrcnn_dts.txt' self.mask_folder_name = 'mrcnn_masks' self.depthmaps_name = "depthmap/disparities_pp.npy" self.segment_folder_name = 'masks_seg' self.tracking_name = 'tracking.txt' self.dets_save_name = 'dets_3d' self.dets_viz_save_name = 'dets_3d_viz'
def read_data(kitti_tracking_dir, annotation_files): """ function read data, lidar scan and images for each file in list of annotation_files assumes that annotation files are in form returned by get_list_kitti_annotation_files function assumes that every annotation file has a corresponding data in kitti_tracking_dir assumes dataset used in kitti tracking dataset :param kitti_tracking_dir: directory that has kitti tracking data: lidar scans, images annotation_files: list of annotation files to read the corresponding data of :return dataset: list of dictionary items for every annotation file dictionary items consists of: folder_type: string to indicate with data is for training or testing seq_number: sequence number of this data frame_id: id of frame of data ground_truth: annotation of data, labels image: image of seq_number and frame_id velo_data: lidar scan of seq_number and frame_id """ dataset = [] for train_val,seq_number,frame_id,ground_truth in annotation_files: #To-do: make it generic, reading from both from training / testing tracking dataset folders basedir = os.path.join(kitti_tracking_dir,'training') if not (os.path.isdir(basedir)): continue loaded = pykitti.tracking(basedir,seq_number) if not loaded.velo_files: print("cannot be loaded",basedir,seq_number) continue image = loaded.get_cam2(int(frame_id) ) image = np.array(image) velo_data = loaded.get_velo(int(frame_id)) if velo_data.size == 0: print("data could be get",seq_number,frame_id) continue dataset.append({ 'folder_type': train_val, 'seq_number': seq_number, 'frame_id': frame_id, 'ground_truth': ground_truth, 'velo_data': velo_data, 'image': image }) return dataset
def create_temporal_bevs_np_jit(CONF): conf = InputConfig(CONF) if platform.system() == 'Darwin': basepath = "/Users/erikbohnsack/data/training" else: basepath = "/home/mlt/data/training" velodyne_path = os.path.join(basepath, 'velodyne') allfiles = os.listdir(velodyne_path) sequences = [ fname for fname in allfiles if not fname.endswith('.DS_Store') ] grid_limits = np.array([[conf.x_min, conf.x_max], [conf.y_min, conf.y_max], [conf.z_min, conf.z_max]]) grid_sizes = np.array( [conf.x_grid_size, conf.y_grid_size, conf.z_grid_size]) for sequence in sequences: tracking = pykitti.tracking(base_path=basepath, sequence=sequence) nof_frames = len(tracking.velo_files) bev_maps = [] for frame in tqdm(range(0, nof_frames)): points = tracking.get_velo(frame) bev_map = np.zeros(conf.bev_shape) kitti_dataset.format_to_BEV_jit(bev_map, points, grid_limits, grid_sizes) print("Frame: {}, bev map shape {}".format(frame, bev_map.shape)) bev_maps.append(bev_map) bev_path = os.path.join(os.path.join(basepath, 'bevs'), sequence) if not os.path.exists(bev_path): os.mkdir(bev_path) for i in range(conf.num_conseq_frames - 1, nof_frames - conf.num_conseq_frames + 1): conc_bev_map = np.concatenate( (bev_maps[i - conf.num_conseq_frames + 1:i + 1]), 2) print("Conc bev map shape {}".format(conc_bev_map.shape)) assert np.shape( conc_bev_map)[2] == conf.num_conseq_frames * conf.bev_shape[2] # For saving/loading pytorch tensors: # torch.save(tensor, 'file.pt') and torch.load('file.pt') filename = os.path.join(bev_path, str(i).zfill(6) + '.pt') torch.save(torch.from_numpy(conc_bev_map), filename)
def create_temporal_bevs(CONF): conf = InputConfig(CONF) if platform.system() == 'Darwin': basepath = "/Users/erikbohnsack/data/training" else: basepath = "/home/mlt/data/training" velodyne_path = os.path.join(basepath, 'velodyne') allfiles = os.listdir(velodyne_path) sequences = [ fname for fname in allfiles if not fname.endswith('.DS_Store') ] for sequence in sequences: tracking = pykitti.tracking(base_path=basepath, sequence=sequence) nof_frames = len(tracking.velo_files) bev_maps = [] for frame in tqdm(range(0, nof_frames)): points = tracking.get_velo(frame) bev_map = torch.zeros(conf.bev_shape) kitti_dataset.format_to_BEV(bev_map, points, conf) bev_maps.append(bev_map) bev_path = os.path.join(os.path.join(basepath, 'bevs'), sequence) if not os.path.exists(bev_path): os.mkdir(bev_path) for i in range(conf.num_conseq_frames - 1, nof_frames - conf.num_conseq_frames + 1): conc_bev_map = torch.cat( (bev_maps[i - conf.num_conseq_frames + 1:i + 1]), 2) assert np.shape( conc_bev_map)[2] == conf.num_conseq_frames * conf.bev_shape[2] # For saving/loading pytorch tensors: # torch.save(tensor, 'file.pt') and torch.load('file.pt') filename = os.path.join(bev_path, str(i).zfill(6) + '.pt') torch.save(conc_bev_map, filename)
def main(): ############################## # Options ############################## # sequence_id = '0013' # sequence_id = '0014' sequence_id = '0015' sequence_id = '0020' # raw_dir = os.path.expanduser('~/Kitti/raw') tracking_dir = os.path.expanduser('~/Kitti/tracking/training') tracking_data = pykitti.tracking(tracking_dir, sequence_id) max_fps = 30.0 vtk_window_size = (1280, 720) show_pose = True point_cloud_source = 'lidar' buffer_size = 5 oxts_path = tracking_data.base_path + '/oxts/{}.txt'.format(sequence_id) oxts_data = pykitti.utils.load_oxts_packets_and_poses([oxts_path]) poses = np.asarray([oxt.T_w_imu for oxt in oxts_data]) calib_path = tracking_data.base_path + '/calib/{}.txt'.format(sequence_id) calib_data = tracking_utils.load_calib(calib_path) camera_viewpoint = 'front' camera_viewpoint = 'elevated' load_images = True # load_images = False ############################## label_path = tracking_data.base_path + '/label_02/{}.txt'.format( sequence_id) gt_tracklets = tracking_utils.get_tracklets(label_path) min_loop_time = 1.0 / max_fps vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'KITTI Tracking Demo', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() if point_cloud_source != 'lidar': raise ValueError( 'Invalid point cloud source {}'.format(point_cloud_source)) # cam_p = raw_data.calib.P_rect_20 cam_p = calib_data['P2'].reshape(3, 4) r_rect = calib_data['R_rect'].reshape(3, 3) # Read calibrations tf_velo_calib_imu_calib = calib_data['Tr_imu_velo'].reshape(3, 4) tf_velo_calib_imu_calib = np.vstack( [tf_velo_calib_imu_calib, np.asarray([0, 0, 0, 1])]) # tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu tf_imu_calib_velo_calib = transform_utils.invert_tf( tf_velo_calib_imu_calib) tf_cam0_calib_velo_calib = r_rect @ calib_data['Tr_velo_cam'].reshape(3, 4) tf_cam0_calib_velo_calib = np.vstack( [tf_cam0_calib_velo_calib, np.asarray([0, 0, 0, 1])]) # tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo tf_velo_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_velo_calib) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) # Create vtk actors vtk_pc_list = [] for i in range(buffer_size): # Point cloud actor wrapper vtk_pc = VtkPointCloudGlyph() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) # all_vtk_actors.append(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_pc_list.append(vtk_pc) curr_actor_idx = -1 vtk_boxes = VtkBoxes() vtk_text_labels = VtkTextLabels() vtk_renderer.AddActor(vtk_boxes.vtk_actor) vtk_renderer.AddActor(vtk_text_labels.vtk_actor) for frame_idx in range(len(poses)): loop_start_time = time.time() curr_actor_idx = (curr_actor_idx + 1) % buffer_size vtk_pc = vtk_pc_list[curr_actor_idx] for i in range(buffer_size): if i == curr_actor_idx: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(3) else: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(0.5) print('{} / {}'.format(frame_idx, len(tracking_data.velo_files) - 1)) # Load next frame data load_start_time = time.time() if point_cloud_source == 'lidar': velo_points, velo_points_i = get_velo_points( tracking_data, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') cam0_curr_pc_all_padded = tf_cam0_calib_velo_calib @ velo_curr_points_padded.T # cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': if load_images: rgb_image = np.asarray(tracking_data.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] points_in_img = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p) points_in_img_int = np.round(points_in_img).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img_int, bgr_image.shape) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] points_in_img_int_valid = points_in_img_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: cam0_curr_pc_padded = cam0_curr_pc_all_padded point_colours = np.repeat( (velo_points_i * 255).astype(np.uint8), 3).reshape(-1, 3) # point_colours = np.full(cam0_curr_pc_padded[0:3].T.shape, [255, 255, 255]) else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get calibration transformations tf_velo_calib_imu_calib = transform_utils.invert_tf( tf_imu_calib_velo_calib) tf_cam0_calib_imu_calib = tf_cam0_calib_velo_calib @ tf_velo_calib_imu_calib tf_imu_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_imu_calib) # Get poses imu_ref_pose = poses[frame_idx] tf_imu_ref_imu_curr = imu_ref_pose velo_curr_pc_padded = tf_velo_calib_cam0_calib @ cam0_curr_pc_padded imu_curr_pc_padded = tf_imu_calib_velo_calib @ velo_curr_pc_padded imu_ref_pc_padded = tf_imu_ref_imu_curr @ imu_curr_pc_padded # TODO: Show points in correct frame # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_curr_pc_padded[0:3].T, point_colours) # vtk_pc.set_points(imu_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # if show_pose: # vtk_pc_pose = VtkPointCloudGlyph() # vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) # vtk_pc_pose.set_points(np.reshape(imu_ref_pose[0:3, 3], [-1, 3])) # vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) # Tracklets frame_mask = [tracklet.frame == frame_idx for tracklet in gt_tracklets] tracklets_for_frame = gt_tracklets[frame_mask] obj_labels = [ tracking_utils.tracklet_to_obj_label(tracklet) for tracklet in tracklets_for_frame ] vtk_boxes.set_objects(obj_labels, colour_scheme=vtk_utils.COLOUR_SCHEME_KITTI) positions_3d = [obj_label.t for obj_label in obj_labels] text_labels = [str(tracklet.id) for tracklet in tracklets_for_frame] vtk_text_labels.set_text_labels(positions_3d, text_labels) # Move camera if camera_viewpoint == 'front': imu_curr_cam0_position = tf_imu_calib_cam0_calib @ [ 0.0, 0.0, 0.0, 1.0 ] elif camera_viewpoint == 'elevated': imu_curr_cam0_position = tf_imu_calib_cam0_calib.dot( [0.0, -5.0, -10.0, 1.0]) else: raise ValueError('Invalid camera_pos', camera_viewpoint) # imu_ref_cam0_position = tf_imu_ref_imu_curr.dot(imu_curr_cam0_position) # imu_curr_focal_point = tf_imu_calib_cam0_calib.dot([0.0, 0.0, 20.0, 1.0]) # imu_ref_focal_point = tf_imu_ref_imu_curr.dot(imu_curr_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, 0, 1) current_cam.SetPosition(0.0, -8.0, -20.0) # current_cam.SetPosition(imu_ref_cam0_position[0:3]) current_cam.SetFocalPoint(0.0, 0.0, 20.0) # current_cam.SetFocalPoint(*imu_ref_focal_point[0:3]) # current_cam.Zoom(0.5) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) # Pause to keep frame rate under max loop_run_time = time.time() - loop_start_time print('loop\t\t', loop_run_time) if loop_run_time < min_loop_time: time.sleep(min_loop_time - loop_run_time) print('---') print('Done') # Keep window open vtk_interactor.Start()
def read_data(self): """ function read data, lidar scan and images for each file in list of annotation_files assumes that annotation files are in form returned by get_annoatation_files function assumes that every annotation file has a corresponding data in kitti_tracking_dir assumes dataset used in kitti tracking dataset :param kitti_tracking_dir: directory that has kitti tracking data: lidar scans, images annotation_files: list of annotation files to read the corresponding data of :return dataset: list of dictionary items for every annotation file dictionary items consists of: folder_type: string to indicate with data is for training or testing seq_number: sequence number of this data frame_id: id of frame of data ground_truth: annotation of data, labels image: image of seq_number and frame_id velo_data: lidar scan of seq_number and frame_id """ # read zhang annotation files self.get_lidar_annotation_files() for annotation_file in tqdm(self.lidar_annotation_files): train_val = annotation_file['train_val'] seq_number = annotation_file["sequence_number"] frame_id = annotation_file["frame_id"] lidar_ground_truth = annotation_file["lidar_ground_truth"] #To-do: make it generic, reading from both from training / testing tracking dataset folders # basedir = os.path.join(kitti_tracking_dir,'training') basedir = self.kitti_tracking_dir / 'training' if not basedir.exists(): print("kitti tracking directory does not exist {}".format( str(basedir))) break loaded = pykitti.tracking(str(basedir), seq_number) if not loaded.velo_files: print("cannot be loaded", str(basedir), seq_number) continue image = loaded.get_cam2(int(frame_id)) image = np.array(image) velo_data = loaded.get_velo(int(frame_id)) tr_velo_matrix = self.get_translation_rotation_Velo_matrix( seq_number) R_rect_00_matrix = self.get_rectified_cam0_coord(seq_number) P_rect_matrix = self.get_projection_rect(seq_number, mode='02') if velo_data.size == 0: print("data could be get", seq_number, frame_id) continue self.dataset.append({ 'folder_type': train_val, 'seq_number': seq_number, 'frame_id': frame_id, 'lidar_ground_truth': lidar_ground_truth, 'velo_data': velo_data, 'image': image, "tr_velo_matrix": tr_velo_matrix, "R_rect_00_matrix": R_rect_00_matrix, "P_rect_matrix": P_rect_matrix, }) return self.dataset
import pykitti __author__ = "Lee Clement" __email__ = "*****@*****.**" # Change this to the directory where you store KITTI data basedir = "/mnt/sakuradata2/datasets/kitti/tracking" # Specify the dataset to load sequence = "0000" # Optionally, specify the frame range to load frame_range = range(0, 20, 5) # Load the data dataset = pykitti.tracking(basedir, sequence, frame_range) # Load image data dataset.load_rgb(format='cv2') # Loads images as uint8 with BGR ordering dataset.load_oxts() # Do some stereo processing stereo = cv2.StereoBM_create(numDisparities=64, blockSize=15) disp_rgb = stereo.compute( cv2.cvtColor(dataset.rgb[0].left, cv2.COLOR_BGR2GRAY), cv2.cvtColor(dataset.rgb[0].right, cv2.COLOR_BGR2GRAY)) # Display some data f, ax = plt.subplots(2, 1) #, figsize=(15, 5)) ax[0].imshow(cv2.cvtColor(dataset.rgb[0].left, cv2.COLOR_BGR2RGB))
# Chose which visualization library to use: "mayavi" or "matplotlib" VISLIB = "mayavi" # VISLIB = "matplotlib" # Raw Data directory information basedir = '/home/dodo_brain/kitti_data/' date = '2011_09_26' drive = '0005' # Optionally, specify the frame range to load # since we are only visualizing one frame, we will restrict what we load # Set to None to use all the data frame_range = range(150, 151, 1) # Load the data dataset = pykitti.tracking(basedir, date, drive, frame_range) # Load Lidar Data dataset.load_velo() # Each scan is a Nx4 array of [x,y,z,reflectance] # Plot only the ith frame (out of what has been loaded) i = 0 velo = dataset.velo[i] if VISLIB == "mayavi": # Plot using mayavi -Much faster and smoother than matplotlib import mayavi.mlab fig = mayavi.mlab.figure(bgcolor=(0, 0, 0), size=(640, 360)) mayavi.mlab.points3d( velo[:, 0], # x