def test_rig_recover_inplace(self): trajectories = deepcopy(self._trajectories_cams) rigs = deepcopy(self._rigs) kapture.rigs_recover_inplace(trajectories, rigs) self.assertTrue( equal_trajectories(trajectories, self._trajectories_rigs)) self.assertTrue(equal_rigs(rigs, self._rigs))
def test_rig_recover_inplace_consistency(self): # compare inplace and not inplace trajectories_inplace = deepcopy(self._trajectories_cams) rigs_inplace = deepcopy(self._rigs) kapture.rigs_recover_inplace(trajectories_inplace, rigs_inplace) trajectories_not_inplace = kapture.rigs_recover(self._trajectories_cams, self._rigs) self.assertTrue(equal_trajectories(trajectories_inplace, trajectories_not_inplace))
def _make_rigs(replace_pose_rig, trajectories) -> kapture.Rigs: logger.info('Making up a rig ...') rigs = kapture.Rigs() pose_babord = kapture.PoseTransform(t=[0, 0, 0], r=quaternion.from_rotation_vector( [0, -np.pi / 2, 0])) pose_tribord = kapture.PoseTransform(t=[0, 0, 0], r=quaternion.from_rotation_vector( [0, np.pi / 2, 0])) rigs['silda_rig', '0'] = pose_babord rigs['silda_rig', '1'] = pose_tribord if replace_pose_rig: logger.info('replacing camera poses with rig poses.') kapture.rigs_recover_inplace(trajectories, rigs) return rigs
def import_robotcar_seasons( robotcar_path: str, kapture_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, skip_reconstruction: bool = False, rig_collapse: bool = False, use_colmap_intrinsics: bool = False, import_v1: bool = False) -> None: """ Read the RobotCar Seasons data, creates several kaptures with training and query data. :param robotcar_path: path to the robotcar top directory :param kapture_path: path to the kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param skip_reconstruction: if True, will skip the reconstruction part from the training data :param rig_collapse: if True, will collapse the rig """ kapture_path = path.join(kapture_path, "base") os.makedirs(kapture_path, exist_ok=True) cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics')) rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics')) logger.info("Importing test data") # Test data image_pattern = re.compile( r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg') queries_path = path.join(robotcar_path, '3D-models', 'individual', 'queries_per_location') kapture_imported_query = {} for root, dirs, files in os.walk(queries_path): for query_file in files: records_camera = kapture.RecordsCamera() # Get list of query images with open(path.join(queries_path, query_file)) as f: for line in f: matches = image_pattern.match(line) image_path = line.strip() if not matches: logger.warning(f"Error matching line in {image_path}") continue matches = matches.groupdict() timestamp = int(matches['timestamp']) camera = str(matches['camera']) condition = str(matches['condition']) records_camera[timestamp, camera] = image_path (query_name, _) = query_file.split('.') kapture_test = kapture.Kapture(sensors=cameras, rigs=rigs, records_camera=records_camera) kapture_imported_query[int( query_name.split('_')[-1])] = kapture_test # Reference map data logger.info("Importing reference map") colmap_reconstructions_path = path.join(robotcar_path, '3D-models', 'individual', 'colmap_reconstructions') kapture_imported_mapping = {} for root, dirs, files in os.walk(colmap_reconstructions_path): for colmap_reconstruction in dirs: (loc_id, _) = colmap_reconstruction.split('_') kapture_reconstruction_dir = path.join(kapture_path, f"{int(loc_id):02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_erase=force_overwrite_existing) logger.info(f'Converting reconstruction {loc_id} to kapture ...') kapture_reconstruction_data = import_robotcar_colmap_location( robotcar_path, path.join(colmap_reconstructions_path, colmap_reconstruction), kapture_reconstruction_dir, rigs, skip_reconstruction) # replace intrinsics with the ones found in the text files if not use_colmap_intrinsics: kapture_reconstruction_data.sensors = cameras kapture_imported_mapping[int(loc_id)] = kapture_reconstruction_data if not import_v1: queries_per_location = { image_name: (ts, cam_id, loc_id) for loc_id, kdata_test in kapture_imported_query.items() for ts, cam_id, image_name in kapture.flatten(kdata_test.records_camera) } kapture_imported_training = {} # stores kapture for each submap # read robotcar_v2_train.txt v2_train_data = read_robotcar_v2_train(robotcar_path) for image_name, pose in v2_train_data.items(): ts, cam_id, loc_id = queries_per_location[image_name] assert cam_id == 'rear' # create kapture object for submap if it doesn't exist if loc_id not in kapture_imported_training: kapture_loc_id = kapture.Kapture(sensors=cameras, rigs=rigs) kapture_loc_id.records_camera = kapture.RecordsCamera() kapture_loc_id.trajectories = kapture.Trajectories() kapture_imported_training[loc_id] = kapture_loc_id kapture_imported_training[loc_id].records_camera[ ts, cam_id] = image_name kapture_imported_training[loc_id].trajectories[ts, cam_id] = pose matches = image_pattern.match(image_name) if not matches: logger.warning(f"Error matching line in {image_name}") continue matches = matches.groupdict() condition = str(matches['condition']) timestamp = str(matches['timestamp']) camera = str(matches['camera']) # added left and right images in records_camera left_image_name = condition + '/' + 'left' + '/' + timestamp + '.jpg' right_image_name = condition + '/' + 'right' + '/' + timestamp + '.jpg' kapture_imported_training[loc_id].records_camera[ ts, 'left'] = left_image_name kapture_imported_training[loc_id].records_camera[ ts, 'right'] = right_image_name # remove entries from query del kapture_imported_query[loc_id].records_camera[ts][cam_id] del kapture_imported_query[loc_id].records_camera[ts]['left'] del kapture_imported_query[loc_id].records_camera[ts]['right'] del kapture_imported_query[loc_id].records_camera[ts] # all remaining query images are kept; reading robotcar_v2_test.txt is not necessary # apply rig collapse if rig_collapse: logger.info('replacing camera poses with rig poses.') for kdata_mapping in kapture_imported_mapping.values(): kapture.rigs_recover_inplace(kdata_mapping.trajectories, rigs, 'rear') for kdata_training in kapture_imported_training.values(): kapture.rigs_recover_inplace(kdata_training.trajectories, rigs, 'rear') # IO operations robotcar_image_path = path.join(robotcar_path, "images") for loc_id, kdata_query in kapture_imported_query.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing test data: {loc_id_str}') kapture_test_dir = path.join(kapture_path, loc_id_str, "query") delete_existing_kapture_files(kapture_test_dir, force_erase=force_overwrite_existing) if not kdata_query.records_camera: # all images were removed continue kapture_to_dir(kapture_test_dir, kdata_query) query_images = [ f for _, _, f in kapture.flatten(kdata_query.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir, query_images, images_import_method) for loc_id, kdata_mapping in kapture_imported_mapping.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing mapping data: {loc_id_str}') kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_erase=force_overwrite_existing) kapture_to_dir(kapture_reconstruction_dir, kdata_mapping) mapping_images = [ f for _, _, f in kapture.flatten(kdata_mapping.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_reconstruction_dir, mapping_images, images_import_method) for loc_id, kdata_training in kapture_imported_training.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing training data: {loc_id_str}') kapture_training_dir = path.join(kapture_path, f"{loc_id:02d}", "training") delete_existing_kapture_files(kapture_training_dir, force_erase=force_overwrite_existing) kapture_to_dir(kapture_training_dir, kdata_training) mapping_images = [ f for _, _, f in kapture.flatten(kdata_training.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_training_dir, mapping_images, images_import_method)
def import_robotcar_seasons( robotcar_path: str, # noqa: C901: function a bit long but not too complex kapture_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, import_feature_db: bool = False, skip_reconstruction: bool = False, rig_collapse: bool = False, use_colmap_intrinsics: bool = False, import_v1: bool = False) -> None: """ Read the RobotCar Seasons data, creates several kaptures with training and query data. :param robotcar_path: path to the robotcar top directory :param kapture_path: path to the kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param import_feature_db: if True, will import the features from the database :param skip_reconstruction: if True, will skip the reconstruction part from the training data :param rig_collapse: if True, will collapse the rig :param use_colmap_intrinsics: if True, will use the colmap intrinsics :param import_v1: if True, will use the version 1 of the format """ os.makedirs(kapture_path, exist_ok=True) cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics')) rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics')) logger.info("Importing test data") # Test data image_pattern = re.compile( r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg') queries_path = path.join(robotcar_path, '3D-models', 'individual', 'queries_per_location') kapture_imported_query = {} for root, dirs, files in os.walk(queries_path): for query_file in files: records_camera = kapture.RecordsCamera() # Get list of query images with open(path.join(queries_path, query_file)) as f: for line in f: matches = image_pattern.match(line) image_path = line.strip() if not matches: logger.warning(f"Error matching line in {image_path}") continue matches = matches.groupdict() timestamp = int(matches['timestamp']) camera = str(matches['camera']) # condition = str(matches['condition']) : not used ? records_camera[timestamp, camera] = image_path (query_name, _) = query_file.split('.') kapture_test = kapture.Kapture(sensors=cameras, rigs=rigs, records_camera=records_camera) kapture_imported_query[int( query_name.split('_')[-1])] = kapture_test # Training data logger.info("Importing training data") colmap_reconstructions_path = path.join(robotcar_path, '3D-models', 'individual', 'colmap_reconstructions') kapture_imported_training = {} for root, dirs, files in os.walk(colmap_reconstructions_path): for colmap_reconstruction in dirs: (loc_id, _) = colmap_reconstruction.split('_') kapture_reconstruction_dir = path.join(kapture_path, f"{int(loc_id):02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_overwrite_existing) logger.info(f'Converting reconstruction {loc_id} to kapture ...') kapture_reconstruction_data = import_robotcar_colmap_location( robotcar_path, path.join(colmap_reconstructions_path, colmap_reconstruction), kapture_reconstruction_dir, rigs, skip_reconstruction) # replace intrinsics with the ones found in the text files if not use_colmap_intrinsics: kapture_reconstruction_data.sensors = cameras kapture_imported_training[int( loc_id)] = kapture_reconstruction_data if not import_v1: _import_robotcar_v2_train(robotcar_path, kapture_imported_query, kapture_imported_training, image_pattern) # apply rig collapse if rig_collapse: logger.info('replacing camera poses with rig poses.') for kapture_mapping in kapture_imported_training.values(): kapture.rigs_recover_inplace(kapture_mapping.trajectories, rigs, ['rear']) # IO operations robotcar_image_path = path.join(robotcar_path, "images") for loc_id, kapture_query in kapture_imported_query.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing test data: {loc_id_str}') kapture_test_dir = path.join(kapture_path, loc_id_str, "query") delete_existing_kapture_files(kapture_test_dir, force_overwrite_existing) if not kapture_query.records_camera: # all images were removed continue kapture_to_dir(kapture_test_dir, kapture_query) query_images = [ f for _, _, f in kapture.flatten(kapture_query.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir, query_images, images_import_method) for loc_id, kapture_mapping in kapture_imported_training.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing mapping data: {loc_id_str}') kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}", "mapping") kapture_to_dir(kapture_reconstruction_dir, kapture_mapping) mapping_images = [ f for _, _, f in kapture.flatten(kapture_mapping.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_reconstruction_dir, mapping_images, images_import_method) if import_feature_db: _import_colmap_overcast_reference(robotcar_path, kapture_path, force_overwrite_existing)
def import_silda( silda_dirpath: str, destination_kapture_dirpath: str, fallback_cam_model: str = 'FOV', do_split_cams: bool = False, corpus: Optional[str] = None, replace_pose_rig: bool = False, force_overwrite_existing: bool = False, images_import_strategy: TransferAction = TransferAction.link_absolute ) -> None: """ Imports data from silda dataset. :param silda_dirpath: path to the silda top directory :param destination_kapture_dirpath: input path to kapture directory. :param fallback_cam_model: camera model to fallback when necessary :param do_split_cams: If true, re-organises and renames the image files to split apart cameras. :param corpus: the list of corpus to be imported, among 'mapping', 'query'. :param replace_pose_rig: if True, replaces poses of individual cameras with poses of the rig. :param force_overwrite_existing: if true, Silently overwrite kapture files if already exists. :param images_import_strategy: how to copy image files. """ # sanity check silda_dirpath = path_secure(path.abspath(silda_dirpath)) destination_kapture_dirpath = path_secure( path.abspath(destination_kapture_dirpath)) if TransferAction.root_link == images_import_strategy and do_split_cams: raise ValueError( 'impossible to only link images directory and applying split cam.') hide_progress_bars = logger.getEffectiveLevel() >= logging.INFO # prepare output directory kapture.io.structure.delete_existing_kapture_files( destination_kapture_dirpath, force_overwrite_existing) os.makedirs(destination_kapture_dirpath, exist_ok=True) # images ########################################################################################################### logger.info('Processing images ...') # silda-images # ... # ├── 1445_0.png # ├── 1445_1.png # ... silda_images_root_path = path.join(silda_dirpath, 'silda-images') # list all png files (its PNG in silda) using a generator. if corpus is not None: assert corpus in SILDA_CORPUS_SPLIT_FILENAMES # if corpus specified, filter by those which directory name match corpus. logger.debug(f'only importing {corpus} part.') coprus_filepath = path.join(silda_dirpath, SILDA_CORPUS_SPLIT_FILENAMES[corpus]) with open(coprus_filepath, 'rt') as corpus_file: corpus_filenames = corpus_file.readlines() image_filenames_original = sorted(filename.strip() for filename in corpus_filenames) else: image_filenames_original = sorted( filename for dirpath, sd, fs in os.walk(silda_images_root_path) for filename in fs if filename.endswith('.png')) image_filenames_kapture = [] snapshots = kapture.RecordsCamera() image_name_to_ids = {} # '1445_0.png' -> (1445, 0) for image_filename_original in tqdm(image_filenames_original, disable=hide_progress_bars): # retrieve info from image filename shot_info = SILDA_IMAGE_NAME_PATTERN.match(image_filename_original) assert shot_info is not None shot_info = shot_info.groupdict() shot_info['timestamp'] = int( shot_info['timestamp'] ) # To avoid warnings about type of the value # eg. file_info = {'filename': '1445_0.png', 'timestamp': 1445, 'cam_id': '0'} # create a path of the image into NLE dir if do_split_cams: # re-organise images with subfolders per corpus/camera/timestamp.png kapture_image_filename = path.join( shot_info['cam_id'], '{:04d}.png'.format(shot_info['timestamp'])) else: # keep the original file hierarchy kapture_image_filename = image_filename_original image_filenames_kapture.append(kapture_image_filename) snapshots[shot_info['timestamp'], shot_info['cam_id']] = kapture_image_filename image_name_to_ids[shot_info['filename']] = (shot_info['timestamp'], shot_info['cam_id']) assert len(image_filenames_kapture) == len(image_filenames_original) # intrinsics ####################################################################################################### logger.info('Processing sensors ...') cameras = kapture.Sensors() # use hard coded intrinsics # evaluated using colmap # 1 OPENCV_FISHEYE 1024 1024 393.299 394.815 512 512 -0.223483 0.117325 -0.0326138 0.00361082 # fx, fy, cx, cy, omega # 1 FOV 1024 1024 300 300 512 512 0.899632 cam_id_list = sorted( set(cam_id for _, cam_id, _ in kapture.flatten(snapshots))) for cam_id in cam_id_list: # pick a image for that cam id random_image_intrinsic = next( f'{timestamp}_{cam_id}.intrinsics' # keep only filename (thats what silda expect) for timestamp, cid, filename in kapture.flatten(snapshots) if cid == cam_id) logger.debug( f'camera {cam_id} intrinsics : picking at random: ("{random_image_intrinsic}")' ) intrinsic_filepath = path.join(silda_dirpath, 'camera-intrinsics', random_image_intrinsic) logger.debug(f'loading file: "{intrinsic_filepath}"') silda_proj_params = np.loadtxt(intrinsic_filepath) # only retrieve principal point from intrinsics, # because the rest correspond to a fisheye model not available in colmap. principal_point = (silda_proj_params[0:2] * SILDA_IMAGE_SIZE).flatten().tolist() projection = fallback_cam_model if 'OPENCV_FISHEYE' == projection: focal_length = [393.299, 394.815] fisheye_coefficients = [ -0.223483, 0.117325, -0.0326138, 0.00361082 ] # // fx, fy, cx, cy, k1, k2, k3, k4 proj_params = focal_length + principal_point + fisheye_coefficients elif 'FOV' == projection: # use hard coded intrinsics from Torsten reconstruction, ie. : # 217.294036, 217.214703, 512.000000, 507.897400, -0.769113 focal_length = [217.294036, 217.214703] # principal_point = [512.000000, 507.897400] omega = [-0.769113] # fx, fy, cx, cy, omega proj_params = focal_length + principal_point + omega else: raise ValueError( 'Only accepts OPENCV_FISHEYE, or FOV as projection model.') camera = kapture.Camera(projection, SILDA_IMAGE_SIZE.tolist() + proj_params) cameras[cam_id] = camera # extrinsics ####################################################################################################### logger.info('Processing trajectories ...') trajectories = kapture.Trajectories() with open(path.join(silda_dirpath, 'silda-train-poses.txt')) as file: lines = file.readlines() lines = (line.rstrip().split() for line in lines) extrinsics = { line[0]: np.array(line[1:8], dtype=np.float) for line in lines } for silda_image_name, pose_params in tqdm(extrinsics.items(), disable=hide_progress_bars): # Silda poses are 7-dim vectors with the rotation quaternion, # and the translation vector. The order needs to be: # qw,qx,qy,qz,tx,ty,tz # The parameters should be described in terms of camera to world transformations if silda_image_name not in image_name_to_ids: # if this is not referenced: means its part of the corpus to be ignored. continue pose = kapture.PoseTransform(pose_params[0:4], pose_params[4:7]).inverse() timestamp, cam_id = image_name_to_ids[silda_image_name] trajectories[timestamp, cam_id] = pose # rigs logger.info('Making up a rig ...') rigs = kapture.Rigs() pose_babord = kapture.PoseTransform(t=[0, 0, 0], r=quaternion.from_rotation_vector( [0, -np.pi / 2, 0])) pose_tribord = kapture.PoseTransform(t=[0, 0, 0], r=quaternion.from_rotation_vector( [0, np.pi / 2, 0])) rigs['silda_rig', '0'] = pose_babord rigs['silda_rig', '1'] = pose_tribord if replace_pose_rig: logger.info('replacing camera poses with rig poses.') kapture.rigs_recover_inplace(trajectories, rigs) # pack it all together kapture_data = kapture.Kapture(sensors=cameras, records_camera=snapshots, trajectories=trajectories, rigs=rigs) logger.info('saving to Kapture ...') kapture.io.csv.kapture_to_dir(destination_kapture_dirpath, kapture_data) # finally import images if images_import_strategy != TransferAction.skip: # importing image files logger.info(f'importing {len(image_filenames_original)} images ...') assert len(image_filenames_original) == len(image_filenames_kapture) image_filepaths_original = [ path.join(silda_images_root_path, image_filename_kapture) for image_filename_kapture in image_filenames_original ] image_filepaths_kapture = [ get_image_fullpath(destination_kapture_dirpath, image_filename_kapture) for image_filename_kapture in image_filenames_kapture ] transfer_files_from_dir(image_filepaths_original, image_filepaths_kapture, images_import_strategy) logger.info('done.')