def get_paths(traj_indx, database_path_base = '/datasets/kitti', split_txt = None, mode = 'train'): ''' Return the training info for one trajectory Assuming: (1) the kitti data set is organized as /path_to_kitti/rawdata /path_to_kitti/train /path_to_kitti/val where rawdata folder contains the raw kitti data, train and val folders contains the GT depth (2) the depth frames for one traj. is always nimg - 10 (ignoring the first and last 5 frames) (3) we will use image 02 data (the left camera ?) Inputs: - traj_indx : the index (in the globbed array) of the trajectory - databse_path_base : the path to the database - split_txt : the split txt file path, including the name of trajectories for trianing/testing/validation - mode: 'train' or 'val' or 'test' Outputs: - n_traj : the # of trajectories in the set defined in split_txt - pykitti_dataset : the dataset object defined in pykitti. It is used to get the input images and camera poses - dmap_paths: the list of paths for the GT dmaps, same length as dataset - poses: list of camera poses, corresponding to dataset and dmap_paths ''' assert split_txt is not None, 'split_txt file is needed' scene_names = _read_split_file(split_txt) n_traj = len( scene_names ) assert traj_indx < n_traj, 'traj_indx should smaller than the scene length' basedir = database_path_base + '/rawdata' sceneName = scene_names[traj_indx] name_contents = sceneName.split('_') date = name_contents[0] + '_' + name_contents[1] + '_' + name_contents[2] drive = name_contents[4] p_data_full = pykitti.raw(basedir, date, drive) nimg = len(p_data_full) # # assume: the depth frames for one traj. is always nimg - 10 (ignoring the first and last 5 frames) p_data = pykitti.raw(basedir, date, drive, frames= range(5, nimg-5)) nimg = len(p_data) dmap_paths = [] poses = [] for i_img in range(nimg): imgname = p_data.cam2_files[i_img].split('/')[-1] poses.append( p_data.oxts[i_img].T_w_imu) dmap_file = '%s/%s/%s/proj_depth/groundtruth/image_02/%s'%( database_path_base, mode, sceneName, imgname) dmap_paths.append( dmap_file ) intrin_path = 'NOT NEEDED' return n_traj, p_data, dmap_paths, poses, intrin_path
def __init__(self, cfg, base_dir, seq, frames=None): if seq == "03" or seq not in self.raw_path.keys(): raise ValueError("%s sequence not supported" % seq) self.cfg = cfg self.imu_time_sync_shift = 1 # we discovered IMU seems to be delayed, so we shifted it forward if frames: range_start = self.raw_range[seq][0] + frames.start range_stop = range_start + frames.stop else: range_start = self.raw_range[seq][0] range_stop = self.raw_range[seq][1] + 1 self.data_raw_kitti = pykitti.raw(base_dir, self.raw_path[seq][0], self.raw_path[seq][1], frames=range(range_start, range_stop)) self.data_imu_kitti = pykitti.raw(base_dir, self.raw_path[seq][0], self.raw_path[seq][1], frames=range(range_start + self.imu_time_sync_shift, range_stop + self.imu_time_sync_shift)) self.data_odom_kitti = pykitti.odometry(base_dir, seq, frames=frames) self.data_lidar_image = LidarDataLoader(self.cfg, base_dir, seq, frames=frames) assert (len(self.data_raw_kitti.oxts) == len(self.data_odom_kitti.poses) and len(self.data_imu_kitti.oxts) == len(self.data_odom_kitti.poses) and len(self.data_odom_kitti.poses) == self.data_lidar_image.num_frames) self.imu_measurements = [] self.imu_measurements_reversed = [] self.__load_imu()
def get_paths_as_dict(depth_dir, rgb_dir, split_txt=None, mode='train', cam_id=None, output_size=None, window_size=None): assert split_txt is not None, 'split_txt file is needed' videos = dict() scene_names = _read_split_file(split_txt) for idx, sname in enumerate(scene_names): name_contents = sname.split('_') date = name_contents[0] + '_' + name_contents[1] + '_' + name_contents[2] drive = name_contents[4] p_data = pykitti.raw(rgb_dir, date, drive) nimg = len(p_data) # # assume: the depth frames for one traj. is always nimg - 10 (ignoring the first and last 5 frames) p_data = pykitti.raw(rgb_dir, date, drive, frames=range(5, nimg - 5)) nimg = len(p_data) if type(cam_id) is int: cam_ids = [cam_id] elif cam_id is None: cam_ids = [2] else: cam_ids = cam_id for idc in cam_ids: cam_intrinsics = _read_IntM_from_pdata(p_data, out_size=output_size, cam_id=idc) K = cam_intrinsics['intrinsic_M'] img_paths = [] gt_dmap_paths = [] sdmap_paths = [] poses = [] M_imu2cam = getattr(p_data.calib, 'T_cam%d_imu' % idc) all_imgs = getattr(p_data, 'cam%d_files' % idc) for i_img in range(nimg): img_path = all_imgs[i_img] imgname = img_path.split('/')[-1] gt_file = '%s/data_depth_annotated/%s/%s/proj_depth/groundtruth/image_%.2d/%s' \ % (depth_dir, mode, sname, idc, imgname) if os.path.exists(gt_file): img_paths.append(img_path) gt_dmap_paths.append(gt_file) input_file = '%s/data_depth_velodyne/%s/%s/proj_depth/velodyne_raw/image_%.2d/%s' \ % (depth_dir, mode, sname, idc, imgname) sdmap_paths.append(input_file) pose_imu = p_data.oxts[i_img].T_w_imu extM = np.matmul(M_imu2cam, np.linalg.inv(pose_imu)) poses.append(extM) video_name = sname + '_cam%d' % idc videos[video_name] = dict(intrinsic_matrix=K, cam_poses=poses, img_paths=img_paths, gt_paths=gt_dmap_paths, sdmap_paths=sdmap_paths) return videos
def get_cam_calib(self): intrinsics, extrinsics = dict(), dict() for name in self.path_images.keys(): date, drive, cam_id = tuple(name.split('x')) cam_id = int(cam_id) p_data = pykitti.raw(self.path_to_rgb, date, drive) K = _read_IntM_from_pdata(p_data, (370, 1240), cam_id=cam_id)['intrinsic_M'] intrinsics[name] = K img_name2id = dict() for idx in range(len(p_data)): if cam_id == 2: img_name = p_data.cam2_files[idx].split('/')[-1].split( '.')[0] elif cam_id == 3: img_name = p_data.cam3_files[idx].split('/')[-1].split( '.')[0] img_name2id[img_name] = idx intrinsics[name], extrinsics[name] = list(), list() M_imu2cam = p_data.calib.T_cam2_imu if cam_id == 2 else p_data.calib.T_cam3_imu for path_img in self.path_images[name]: img_name = path_img.split('/')[-1].split('_')[-3] idx = img_name2id[img_name] pose_imu = p_data.oxts[idx].T_w_imu extM = np.matmul(M_imu2cam, np.linalg.inv(pose_imu)) extrinsics[name].append(extM) return intrinsics, extrinsics
def __init__(self, config: dict): # Set Ground Truth and Velocity if config['name'] == 'kitti_raw': self.resize_factor = config['resize_factor'] self.dataset = pykitti.raw(config['basedir'], config['date'], config['drive']) self.image_getter_left = iter(self.dataset.cam0) self.velocity_getter = ([ oxt.packet.vl, oxt.packet.vu, oxt.packet.vf ] for oxt in self.dataset.oxts) self.true_pose_getter = ([ oxt.T_w_imu[0][3], oxt.T_w_imu[1][3], oxt.T_w_imu[2][3] ] for oxt in self.dataset.oxts) self.k = self.dataset.k = self.resize_factor * self.dataset.calib.K_cam0 self.k[2][2] = 1 self.inv_k = np.linalg.inv(self.k) self.focal = self.k[0][0] self.pp = (self.k[0][2], self.k[1][2]) if config['name'] == 'kitti_odometry': self.resize_factor = config['resize_factor'] self.dataset = pykitti.odometry(config['basedir'], config['sequence']) self.image_getter_left = iter(self.dataset.cam0) self.image_getter_right = iter(self.dataset.cam1) self.true_pose_getter = ([pose[0][3], pose[1][3], pose[2][3]] for pose in self.dataset.poses) self.k = self.dataset.k = self.resize_factor * self.dataset.calib.K_cam0 self.k[2][2] = 1 self.inv_k = np.linalg.inv(self.k) self.focal = self.k[0][0] self.pp = (self.k[0][2], self.k[1][2])
def get_combined_global_trajectory_and_weights(dates, drives, fastThreshold=150): ''' Combine multiple drives using the OXTS GPS lat/lon/alt measurements. Yaw is sloppily assuming flat orientation (i.e. small roll and pitch) for now. ''' lat = [] lon = [] alt = [] yaw = [] w = [] orb = cv2.ORB_create(nfeatures=10000, fastThreshold=fastThreshold) for date, drive in zip(dates, drives): data = pykitti.raw(basedir, date, drive, imformat='cv2') for img, p in zip(data.cam0, data.oxts): lat.append(p[0].lat) lon.append(p[0].lon) # Assumes a small enough region alt.append(p[0].alt) # find the keypoints with ORB kp = orb.detect(img, None) w.append(len(kp)) yaw.append(p[0].yaw) x, y = lat_lon_to_xy(np.pi * np.array(lat) / 180.0, np.pi * np.array(lon) / 180.0) alt = np.array(alt) alt = alt - alt[0] yaw = np.array(yaw) return x, y, alt, yaw, np.array(w)
def load_dataset(date, drive, calibrated=False, frame_range=None): """ Loads the dataset with `date` and `drive`. Parameters ---------- date : Dataset creation date. drive : Dataset drive. calibrated : Flag indicating if we need to parse calibration data. Defaults to `False`. frame_range : Range of frames. Defaults to `None`. Returns ------- Loaded dataset of type `raw`. """ dataset = pykitti.raw(basedir, date, drive) # Load the data if calibrated: dataset.load_calib() # Calibration data are accessible as named tuples np.set_printoptions(precision=4, suppress=True) print('\nDrive: ' + str(dataset.drive)) print('\nFrame range: ' + str(dataset.frames)) if calibrated: print('\nIMU-to-Velodyne transformation:\n' + str(dataset.calib.T_velo_imu)) print('\nGray stereo pair baseline [m]: ' + str(dataset.calib.b_gray)) print('\nRGB stereo pair baseline [m]: ' + str(dataset.calib.b_rgb)) return dataset
def run(self): # load dataset try: dirs = sorted(os.listdir(self._basedir + '/extracted')) self._datasets = {} if self._verbose: print('loading datasets: ', end='') for dr in dirs: mtch = re.match("([0-9_]+)_drive_([0-9]+)_sync", dr) if mtch is None: continue date, drive = mtch.groups() if (self._subdate != None and date != self._subdate) or (self._subdrive != None and drive != self._subdrive): continue if self._verbose: print('.', end='') sys.stdout.flush() self._datasets[date + "_" + drive] = pykitti.raw(self._basedir, date, drive) if self._verbose: print() # fetching blobs lim = None while True: loc, shape = self._iqueue.get() im = self.load_image(loc, shape) if im == None: im = lim # FIXME: reuse last image in case of error self._oqueue.put(im) lim = im #print('image processed {} todo'.format(self._iqueue.qsize())) except KeyboardInterrupt: self.terminate() return
def run(args): torch.cuda.empty_cache() device = torch.device('cuda:0' if torch.cuda.is_available() else 'cpu') model = hub.load('TheCodez/pytorch-GoogLeNet-FCN', 'googlenet_fcn', pretrained='cityscapes') model = model.to(device) model.eval() for date in args.dates: for drive in args.drives: data = pykitti.raw(args.dir, date, drive) for i, image in enumerate(data.cam2): name = os.path.basename(data.cam2_files[i])[:-4] file_name = '{}_{}_{}.npy'.format(date, drive, name) pc_velo = data.get_velo(i) print("Processing: ", file_name) pred = autolabel.semantic_segmentation(model, image, device) pc_velo = autolabel.get_points_in_fov_90(pc_velo) pc_labels = autolabel.transfer_labels(pc_velo, pred, data.calib.T_cam0_velo, data.calib.K_cam0) record = autolabel.spherical_projection(pc_labels) os.makedirs(args.output_dir, exist_ok=True) np.save(os.path.join(args.output_dir, file_name), record) print("Saved: ", file_name) print("Processing finished")
def main(last_frame, step_frame, nb_corres_points, learning_rate): """Train the universal correspondence network . Args: last_frame: last image to be loaded. step_frame: load every step_frame images nb_corres_points: Number of velodyne correspondence points to load learning_rate: learning rate of the optimiser """ logger = logging.getLogger(__name__) data = pykitti.raw('data', '2011_09_26', '0002', frames=range(0, last_frame, step_frame)) # Load the velodyne correspondence points correspondences = [] for i, velo_points in enumerate(data.velo): velo_points = velo_points[:nb_corres_points] # Project the points in the images of the left and right cameras cam2 = velo_points.dot(np.transpose(data.calib.T_cam2_velo)) cam3 = velo_points.dot(np.transpose(data.calib.T_cam3_velo)) # Only gather the x and y position on the images correspondences.append(np.concatenate((cam2[:, :2], cam3[:, :2]), axis=1)) ucn = UniversalCorrepondenceNetwork([375, 1242, 3], nb_corres_points) learner = CorrespondenceLearner(ucn, 0.001, 10) loss = learner.train_network(np.array(list(data.cam2)), np.array(list(data.cam3)), np.array(correspondences), learning_rate) logger.info('Loss: {loss}'.format(loss=loss))
def __init__(self, basedir, date, drive): self.dataset = pykitti.raw(basedir, date, drive) self.frames = len(self.dataset.timestamps) tracklet_path = '{}/{}/{}_drive_{}_sync/tracklet_labels.xml'.format(basedir, date, date, drive) self.tracklets, self.tracklet_rects, self.tracklet_types, self.track_id, self.track_center \ = self.load_tracklets_for_frames(tracklet_path) self.track_velocity, self.flowlets = self.load_car_velocity() self.track_bb = self.load_car_bb()
def load_raw_data(drive): basedir = cfg.basedir + drive date = cfg.date dataset = pykitti.raw(basedir, date, drive) return [x for x in dataset.velo]
def __init__(self, base_dir, date, drive, frames): self.base_dir = base_dir self.date = date self.drive = drive self.first_frame = frames[0] self.last_frame_plus_1 = frames[1] self.kitti = pykitti.raw(base_dir, date, drive)
def __init__(self, date, drive) -> None: super().__init__() self.date = date self.drive = drive self.dataset = pykitti.raw(dataset_base_path, date, drive) self.calib = self.dataset.calib
def __init__(self, base_dir=constants.KITTI_BASE_DIR, date=constants.DATE, drive=constants.DRIVE, cam_num=constants.CAM_NUM): self._dataset = pykitti.raw(base_dir, date, drive) self.__get_cam = self._dataset.__getattribute__('get_cam' + str(cam_num))
def __init__(self, basedir, date, drive, frame_range): # Change this to the directory where you store KITTI data self.basedir = basedir # Specify the dataset to load self.date = date self.drive = drive self.frame_range = frame_range self.raw_data = pykitti.raw(basedir, date, drive, frames=frame_range)
def get_raw_data(drive_id, raw_dir): # Load raw data drive_date = drive_id[0:10] drive_num = drive_id[17:21] raw_data = pykitti.raw(raw_dir, drive_date, drive_num) return raw_data
def load_kitti(self, data_folder: str, date: str, drive: str, **kwargs): self.data_kitti = pykitti.raw(data_folder, date, drive, **kwargs) logging.info("Loading KITTI Dataset from dir: %r; date: %r, drive: %r", self.data_folder, self.date, self.drive) self.load_projections(self.cam) self.fix_imu() self.img_n = IMG_WIDTH self.img_m = IMG_HEIGHT self.fix_missing_velo()
def __init__(self, date: str, drive_id: str): self.data_path = pp.get_path(date, drive_id) self.default_img_dir = "image_00" self.default_img_path = os.path.join(self.data_path, self.default_img_dir, "data") print("[KittiDataLoader] image path:", self.default_img_path) self.default_img_list = glob.glob(self.default_img_path + "/*.png") assert self.default_img_list, "no images in {}".format(self.default_img_path) self.default_img_list.sort() self.kitti = pykitti.raw(pp.get_data_root(), date, drive_id)
def load_raw_forward_data(drive, one_out_of=None, y_threshold=None): basedir = cfg.basedir + drive date = cfg.date dataset = pykitti.raw(basedir, date, drive) return [ _get_points_with_threshold(frame, y_threshold, one_out_of) for frame in dataset.velo ]
def make_kitti_video(): basedir = '/root/milestone3/src/pixor/scripts/PIXOR/srcs/data/' date = '2011_09_28' drive = '0016' dataset = pykitti.raw(basedir, date, drive) videoname = "detection_{}_{}.avi".format(date, drive) save_path = os.path.join(basedir, date, "{}_drive_{}_sync".format(date, drive), videoname) run(dataset, save_path)
def make_kitti_video(): basedir = '/home/aw2/Development/PIXOR' date = '2011_09_26' drive = '0035' dataset = pykitti.raw(basedir, date, drive) videoname = "detection_{}_{}.avi".format(date, drive) save_path = os.path.join(basedir, date, "{}_drive_{}_sync".format(date, drive), videoname) run(dataset, save_path)
def make_kitti_video(): basedir = '/mnt/ssd2/od/KITTI/raw' date = '2011_09_26' drive = '0035' dataset = pykitti.raw(basedir, date, drive) videoname = "detection_{}_{}.avi".format(date, drive) save_path = os.path.join(basedir, date, "{}_drive_{}_sync".format(date, drive), videoname) run(dataset, save_path)
def make_kitti_video(date, drive, basedir=None): # basedir = '/Users/charlie/Documents/Coursework/Group_Project/PIXOR/Kitti/' if basedir == None: basedir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'data/') dataset = pykitti.raw(basedir, date, drive) videoname = "detection_{}_{}.avi".format(date, drive) save_path = os.path.join(basedir, date, "{}_drive_{}_sync".format(date, drive), videoname) run(dataset, save_path)
def sequence(self, i): # if 0 <= i < 11: # sequence = pykitti.odometry(self.ododir, '{:02d}'.format(i)) # sequence.poses = numpy.array(sequence.poses) # else: drive = kittidrives.drives[i] sequence = pykitti.raw(self.rawdir, drive['date'], drive['drive']) T_imu_cam0 = util.invert_ht(sequence.calib.T_cam0_imu) sequence.poses = numpy.array( [numpy.matmul(oxts.T_w_imu, T_imu_cam0) \ for oxts in sequence.oxts]) return sequence
def run_vo_kitti(basedir, date, drive, frames, outfile=None): # Load KITTI data dataset = pykitti.raw(basedir, date, drive, frames=frames, imformat='cv2') first_oxts = dataset.oxts[0] T_cam0_imu = SE3.from_matrix(dataset.calib.T_cam0_imu) T_cam0_imu.normalize() T_0_w = T_cam0_imu.dot(SE3.from_matrix(first_oxts.T_w_imu).inv()) T_0_w.normalize() # Create the camera test_im = np.array(next(dataset.cam0)) fu = dataset.calib.K_cam0[0, 0] fv = dataset.calib.K_cam0[1, 1] cu = dataset.calib.K_cam0[0, 2] cv = dataset.calib.K_cam0[1, 2] b = dataset.calib.b_gray h, w = test_im.shape camera = StereoCamera(cu, cv, fu, fv, b, w, h) # Ground truth T_w_c_gt = [ SE3.from_matrix(o.T_w_imu).dot(T_cam0_imu.inv()) for o in dataset.oxts ] # Pipeline vo = DenseStereoPipeline(camera, first_pose=T_0_w) # Skip the highest resolution level vo.pyrlevel_sequence.pop() vo.pyr_cameras.pop() start = time.perf_counter() for c_idx, impair in enumerate(dataset.gray): vo.track(np.array(impair[0]), np.array(impair[1])) # vo.track(impair[0], impair[1], guess=T_w_c_gt[c_idx].inv()) end = time.perf_counter() print('Image {}/{} | {:.3f} s'.format(c_idx, len(dataset), end - start)) start = end # Compute errors T_w_c_est = [T.inv() for T in vo.T_c_w] tm = TrajectoryMetrics(T_w_c_gt, T_w_c_est) # Save to file if outfile is not None: print('Saving to {}'.format(outfile)) tm.savemat(outfile) # Clean up del vo return tm
def _read_lidar(self, frame): if frame not in self.kitti_data.frame_range: self.kitti_data = pykitti.raw( self.basedir, self.date, self.drive, range(frame, frame + 1)) #, range(start_frame, start_frame + total_frames)) self.kitti_data.load_calib() assert frame in self.kitti_data.frame_range self.kitti_data.load_velo( ) # Each scan is a Nx4 array of [x,y,z,reflectance] assert len(self.kitti_data.velo) == 1 self.lidars[frame] = self.kitti_data.velo[0] return
def __init__(self, data_path, date, drive, calibrated=True): path = data_path dataset = pykitti.raw(data_path, date, drive) print(np.shape(dataset.calib)) # dataset = pykitti.raw(basedir, date, drive) # Load the data # if calibrated: # dataset.load_calib() # Calibration data are accessible as named tuples np.set_printoptions(precision=4, suppress=True) print('\nDrive: ' + str(dataset.drive)) print('\nFrame range: ' + str(dataset.frames))
def __init__(self, basedir, date, drive): self.data = pykitti.raw(basedir, date, 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_dets.txt' self.mask_folder_name = 'masks_obj' 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 get_kitti_dataset(): # Change this to the directory where you store KITTI data curr_dir_path = os.getcwd() basedir = curr_dir_path + '/kitti_data' # Specify the dataset to load date = '2011_09_26' drive = '0001' # Load the data. Optionally, specify the frame range to load. # dataset = pykitti.raw(basedir, date, drive) dataset = pykitti.raw(basedir, date, drive) return dataset
if not len(sys.argv) == 4: print 'Usage: python kitti2lcm.py [base_dir] [date] [drive]' sys.exit(1) base_dir = sys.argv[1] date = sys.argv[2] drive = sys.argv[3] data_dir = os.path.join(base_dir, date, date + '_drive_' + drive + '_sync') N = len(os.listdir(os.path.join(data_dir, 'image_00/data'))) frame_range = range(0, N) frame_objects = [[] for i in xrange(N)] dataset = pykitti.raw(base_dir, date, drive, frame_range) dataset.load_calib() dataset.load_timestamps() dataset.load_oxts() #dataset.load_rgb(format='cv2') #dataset.load_velo() load_tracklets() print 'Loaded: drive %s' % (date + '_' + drive) lc = lcm.LCM() for j in xrange(10): for i in xrange(N): publish_calib(i) publish_imu(i)
import matplotlib.pyplot as plt import pykitti __author__ = "Lee Clement" __email__ = "*****@*****.**" # Change this to the directory where you store KITTI data basedir = '/Users/leeclement/Desktop/KITTI/raw' # Specify the dataset to load date = '2011_09_30' drive = '0034' # Load the data. Optionally, specify the frame range to load. dataset = pykitti.raw(basedir, date, drive, frames=range(0, 20, 5)) # dataset.calib: Calibration data are accessible as a named tuple # dataset.timestamps: Timestamps are parsed into a list of datetime objects # dataset.oxts: List of OXTS packets and 6-dof poses as named tuples # dataset.camN: Returns a generator that loads individual images from camera N # dataset.get_camN(idx): Returns the image from camera N at idx # dataset.gray: Returns a generator that loads monochrome stereo pairs (cam0, cam1) # dataset.get_gray(idx): Returns the monochrome stereo pair at idx # dataset.rgb: Returns a generator that loads RGB stereo pairs (cam2, cam3) # dataset.get_rgb(idx): Returns the RGB stereo pair at idx # dataset.velo: Returns a generator that loads velodyne scans as [x,y,z,reflectance] # dataset.get_velo(idx): Returns the velodyne scan at idx # Grab some data first_gray = dataset.get_gray(0)