def colmap_build_map_from_loaded_data(kapture_data: kapture.Kapture, kapture_path: str, colmap_path: str, colmap_binary: str, pairsfile_path: Optional[str], use_colmap_matches_importer: bool, point_triangulator_options: List[str], skip_list: List[str], force: bool) -> None: """ Build a colmap model using custom features with the kapture data. :param kapture_data: kapture data to use :param kapture_path: path to the kapture to use :param colmap_path: path to the colmap build :param colmap_binary: path to the colmap executable :param pairsfile_path: Optional[str], :param use_colmap_matches_importer: bool, :param point_triangulator_options: options for the point triangulator :param skip_list: list of steps to skip :param force: Silently overwrite kapture files if already exists. """ os.makedirs(colmap_path, exist_ok=True) if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches): raise ValueError('records_camera, sensors, keypoints, matches are mandatory') if not kapture_data.trajectories: logger.info('there are no trajectories, running mapper instead of point_triangulator') # COLMAP does not fully support rigs. if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Set fixed name for COLMAP database colmap_db_path = path.join(colmap_path, 'colmap.db') reconstruction_path = path.join(colmap_path, "reconstruction") priors_txt_path = path.join(colmap_path, "priors_for_reconstruction") if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) safe_remove_any_path(reconstruction_path, force) safe_remove_any_path(priors_txt_path, force) os.makedirs(reconstruction_path, exist_ok=True) if 'colmap_db' not in skip_list: logger.info('Using precomputed keypoints and matches') logger.info('Step 1: Export kapture format to colmap') colmap_db = COLMAPDatabase.connect(colmap_db_path) if kapture_data.descriptors is not None: kapture_data.descriptors.clear() database_extra.kapture_to_colmap(kapture_data, kapture_path, colmap_db, export_two_view_geometry=not use_colmap_matches_importer) # close db before running colmap processes in order to avoid locks colmap_db.close() if use_colmap_matches_importer: logger.info('Step 2: Run geometric verification') logger.debug('running colmap matches_importer...') colmap_lib.run_matches_importer_from_kapture( colmap_binary, colmap_use_cpu=True, colmap_gpu_index=None, colmap_db_path=colmap_db_path, kapture_data=kapture_data, force=force ) else: logger.info('Step 2: Run geometric verification - skipped') if kapture_data.trajectories is not None: # Generate priors for reconstruction os.makedirs(priors_txt_path, exist_ok=True) if 'priors_for_reconstruction' not in skip_list: logger.info('Step 3: Exporting priors for reconstruction.') colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.generate_priors_for_reconstruction(kapture_data, colmap_db, priors_txt_path) colmap_db.close() # Point triangulator reconstruction_path = path.join(colmap_path, "reconstruction") os.makedirs(reconstruction_path, exist_ok=True) if 'triangulation' not in skip_list: logger.info("Step 4: Triangulation") colmap_lib.run_point_triangulator( colmap_binary, colmap_db_path, get_image_fullpath(kapture_path), priors_txt_path, reconstruction_path, point_triangulator_options ) else: # mapper reconstruction_path = path.join(colmap_path, "reconstruction") os.makedirs(reconstruction_path, exist_ok=True) if 'triangulation' not in skip_list: logger.info("Step 4: Triangulation") colmap_lib.run_mapper( colmap_binary, colmap_db_path, get_image_fullpath(kapture_path), None, reconstruction_path, point_triangulator_options ) # use reconstruction 0 as main first_reconstruction = os.path.join(reconstruction_path, '0') files = os.listdir(first_reconstruction) for f in files: shutil.move(os.path.join(first_reconstruction, f), os.path.join(reconstruction_path, f)) shutil.rmtree(first_reconstruction) # run model_converter if 'model_converter' not in skip_list: logger.info("Step 5: Export reconstruction results to txt") colmap_lib.run_model_converter( colmap_binary, reconstruction_path, reconstruction_path )
def run_colmap_gv_from_loaded_data(kapture_none_matches: kapture.Kapture, kapture_colmap_matches: kapture.Kapture, kapture_none_matches_dirpath: str, kapture_colmap_matches_dirpath: str, tar_handlers_none_matches: Optional[TarCollection], tar_handlers_colmap_matches: Optional[TarCollection], colmap_binary: str, keypoints_type: Optional[str], skip_list: List[str], force: bool): logger.info('run_colmap_gv...') if not (kapture_none_matches.records_camera and kapture_none_matches.sensors and kapture_none_matches.keypoints and kapture_none_matches.matches): raise ValueError('records_camera, sensors, keypoints, matches are mandatory') # COLMAP does not fully support rigs. if kapture_none_matches.rigs is not None and kapture_none_matches.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_none_matches.trajectories, kapture_none_matches.rigs) # Set fixed name for COLMAP database colmap_db_path = os.path.join(kapture_colmap_matches_dirpath, 'colmap.db') if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kapture_none_matches.matches) assert keypoints_type is not None assert keypoints_type in kapture_none_matches.keypoints assert keypoints_type in kapture_none_matches.matches if 'matches_importer' not in skip_list: logger.debug('compute matches difference.') if kapture_colmap_matches.matches is not None and keypoints_type in kapture_colmap_matches.matches: colmap_matches = kapture_colmap_matches.matches[keypoints_type] else: colmap_matches = kapture.Matches() matches_to_verify = kapture.Matches(kapture_none_matches.matches[keypoints_type].difference(colmap_matches)) kapture_data_to_export = kapture.Kapture(sensors=kapture_none_matches.sensors, trajectories=kapture_none_matches.trajectories, records_camera=kapture_none_matches.records_camera, keypoints={ keypoints_type: kapture_none_matches.keypoints[keypoints_type] }, matches={ keypoints_type: matches_to_verify }) # creates a new database with matches logger.debug('export matches difference to db.') colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.kapture_to_colmap(kapture_data_to_export, kapture_none_matches_dirpath, tar_handlers_none_matches, colmap_db, keypoints_type, None, export_two_view_geometry=False) # close db before running colmap processes in order to avoid locks colmap_db.close() logger.debug('run matches_importer command.') colmap_lib.run_matches_importer_from_kapture_matches( colmap_binary, colmap_use_cpu=True, colmap_gpu_index=None, colmap_db_path=colmap_db_path, kapture_matches=matches_to_verify, force=force ) if 'import' not in skip_list: logger.debug('import verified matches.') os.umask(0o002) colmap_db = COLMAPDatabase.connect(colmap_db_path) kapture_data = kapture.Kapture() kapture_data.records_camera, _ = get_images_and_trajectories_from_database(colmap_db) kapture_data.matches = { keypoints_type: get_matches_from_database(colmap_db, kapture_data.records_camera, kapture_colmap_matches_dirpath, tar_handlers_colmap_matches, keypoints_type, no_geometric_filtering=False) } colmap_db.close() if kapture_colmap_matches.matches is None: kapture_colmap_matches.matches = {} if keypoints_type not in kapture_colmap_matches.matches: kapture_colmap_matches.matches[keypoints_type] = kapture.Matches() kapture_colmap_matches.matches[keypoints_type].update(kapture_data.matches[keypoints_type]) if 'delete_db' not in skip_list: logger.debug('delete intermediate colmap db.') os.remove(colmap_db_path)
def colmap_localize_from_loaded_data(kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: Optional[TarCollection], colmap_path: str, input_database_path: str, input_reconstruction_path: str, colmap_binary: str, keypoints_type: Optional[str], use_colmap_matches_importer: bool, image_registrator_options: List[str], skip_list: List[str], force: bool) -> None: """ Localize images on a colmap model with the kapture data. :param kapture_data: kapture data to use :param kapture_path: path to the kapture to use :param tar_handler: collection of preloaded tar archives :param colmap_path: path to the colmap build :param input_database_path: path to the map colmap.db :param input_database_path: path to the map colmap.db :param input_reconstruction_path: path to the map reconstruction folder :param colmap_binary: path to the colmap binary executable :param keypoints_type: type of keypoints, name of the keypoints subfolder :param use_colmap_matches_importer: bool, :param image_registrator_options: options for the image registrator :param skip_list: list of steps to skip :param force: Silently overwrite kapture files if already exists. """ os.makedirs(colmap_path, exist_ok=True) if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches): raise ValueError('records_camera, sensors, keypoints, matches are mandatory') if kapture_data.trajectories: logger.warning("Input data contains trajectories: they will be ignored") kapture_data.trajectories.clear() else: kapture_data.trajectories = kapture.Trajectories() # COLMAP does not fully support rigs. if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Prepare output # Set fixed name for COLMAP database colmap_db_path = path.join(colmap_path, 'colmap.db') image_list_path = path.join(colmap_path, 'images.list') reconstruction_path = path.join(colmap_path, "reconstruction") if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) safe_remove_file(image_list_path, force) safe_remove_any_path(reconstruction_path, force) os.makedirs(reconstruction_path, exist_ok=True) # Copy colmap db to output if not os.path.exists(colmap_db_path): shutil.copy(input_database_path, colmap_db_path) # find correspondences between the colmap db and the kapture data images_all = {image_path: (ts, cam_id) for ts, shot in kapture_data.records_camera.items() for cam_id, image_path in shot.items()} colmap_db = COLMAPDatabase.connect(colmap_db_path) colmap_image_ids = database_extra.get_colmap_image_ids_from_db(colmap_db) colmap_images = database_extra.get_images_from_database(colmap_db) colmap_db.close() # dict ( kapture_camera -> colmap_camera_id ) colmap_camera_ids = {images_all[image_path][1]: colmap_cam_id for image_path, colmap_cam_id in colmap_images if image_path in images_all} images_to_add = {image_path: value for image_path, value in images_all.items() if image_path not in colmap_image_ids} flatten_images_to_add = [(ts, kapture_cam_id, image_path) for image_path, (ts, kapture_cam_id) in images_to_add.items()] if 'import_to_db' not in skip_list: logger.info("Step 1: Add precomputed keypoints and matches to colmap db") if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches cameras_to_add = kapture.Sensors() for _, (_, kapture_cam_id) in images_to_add.items(): if kapture_cam_id not in colmap_camera_ids: kapture_cam = kapture_data.sensors[kapture_cam_id] cameras_to_add[kapture_cam_id] = kapture_cam colmap_db = COLMAPDatabase.connect(colmap_db_path) colmap_added_camera_ids = database_extra.add_cameras_to_database(cameras_to_add, colmap_db) colmap_camera_ids.update(colmap_added_camera_ids) colmap_added_image_ids = database_extra.add_images_to_database_from_flatten( colmap_db, flatten_images_to_add, kapture_data.trajectories, colmap_camera_ids) colmap_image_ids.update(colmap_added_image_ids) colmap_image_ids_reversed = {v: k for k, v in colmap_image_ids.items()} # colmap_id : name # add new features colmap_keypoints = database_extra.get_keypoints_set_from_database(colmap_db, colmap_image_ids_reversed) keypoints_all = kapture_data.keypoints[keypoints_type] keypoints_to_add = {name for name in keypoints_all if name not in colmap_keypoints} keypoints_to_add = kapture.Keypoints(keypoints_all.type_name, keypoints_all.dtype, keypoints_all.dsize, keypoints_to_add) database_extra.add_keypoints_to_database(colmap_db, keypoints_to_add, keypoints_type, kapture_path, tar_handlers, colmap_image_ids) # add new matches colmap_matches = kapture.Matches(database_extra.get_matches_set_from_database(colmap_db, colmap_image_ids_reversed)) colmap_matches.normalize() matches_all = kapture_data.matches[keypoints_type] matches_to_add = kapture.Matches({pair for pair in matches_all if pair not in colmap_matches}) # print(list(matches_to_add)) database_extra.add_matches_to_database(colmap_db, matches_to_add, keypoints_type, kapture_path, tar_handlers, colmap_image_ids, export_two_view_geometry=not use_colmap_matches_importer) colmap_db.close() if use_colmap_matches_importer: logger.info('Step 2: Run geometric verification') logger.debug('running colmap matches_importer...') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kapture_data.matches) assert keypoints_type is not None assert keypoints_type in kapture_data.matches # compute two view geometry colmap_lib.run_matches_importer_from_kapture_matches( colmap_binary, colmap_use_cpu=True, colmap_gpu_index=None, colmap_db_path=colmap_db_path, kapture_matches=kapture_data.matches[keypoints_type], force=force) else: logger.info('Step 2: Run geometric verification - skipped') if 'image_registrator' not in skip_list: logger.info("Step 3: Run image_registrator") # run image_registrator colmap_lib.run_image_registrator( colmap_binary, colmap_db_path, input_reconstruction_path, reconstruction_path, image_registrator_options ) # run model_converter if 'model_converter' not in skip_list: logger.info("Step 4: Export reconstruction results to txt") colmap_lib.run_model_converter( colmap_binary, reconstruction_path, reconstruction_path )
def pyransaclib_localize_from_loaded_data( kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: TarCollection, kapture_query_data: kapture.Kapture, output_path: str, pairsfile_path: str, inlier_threshold: float, number_lo_steps: int, min_num_iterations: int, max_num_iterations: int, refine_poses: bool, keypoints_type: Optional[str], duplicate_strategy: DuplicateCorrespondencesStrategy, rerank_strategy: RerankCorrespondencesStrategy, write_detailed_report: bool, force: bool) -> None: """ Localize images using pyransaclib. :param kapture_data: loaded kapture data (incl. points3d) :param kapture_path: path to the kapture to use :param tar_handlers: collection of pre-opened tar archives :param kapture_data: loaded kapture data (mapping and query images) :param output_path: path to the write the localization results :param pairsfile_path: pairs to use :param inlier_threshold: RANSAC inlier threshold in pixel :param number_lo_steps: number of local optimization iterations in LO-MSAC. Use 0 to use MSAC :param min_num_iterations: minimum number of ransac loops :param max_num_iterations: maximum number of ransac loops :param refine_poses: refine poses with pycolmap :param keypoints_type: types of keypoints (and observations) to use :param force: Silently overwrite kapture files if already exists. """ assert has_pyransaclib if refine_poses: assert has_pycolmap if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.points3d and kapture_data.observations): raise ValueError('records_camera, sensors, keypoints, matches, ' 'points3d, observations are mandatory for map+query') if not (kapture_query_data.records_camera and kapture_query_data.sensors): raise ValueError('records_camera, sensors are mandatory for query') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection( kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() if kapture_query_data.trajectories is not None: logger.warning( "Input query data contains trajectories: they will be ignored") kapture_query_data.trajectories.clear() os.umask(0o002) os.makedirs(output_path, exist_ok=True) delete_existing_kapture_files(output_path, force_erase=force) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = kapture.io.csv.table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kapture_data.matches[keypoints_type].normalize() keypoints_filepaths = keypoints_to_filepaths( kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path, tar_handlers) obs_for_keypoints_type = { point_id: per_keypoints_type_subdict[keypoints_type] for point_id, per_keypoints_type_subdict in kapture_data.observations.items() if keypoints_type in per_keypoints_type_subdict } point_id_from_obs = { (img_name, kp_id): point_id for point_id in obs_for_keypoints_type.keys() for img_name, kp_id in obs_for_keypoints_type[point_id] } query_images = [(timestamp, sensor_id, image_name) for timestamp, sensor_id, image_name in kapture.flatten( kapture_query_data.records_camera)] # kapture for localized images + pose trajectories = kapture.Trajectories() progress_bar = tqdm(total=len(query_images), disable=logging.getLogger().level >= logging.CRITICAL) for timestamp, sensor_id, image_name in query_images: if image_name not in pairs: continue keypoints_filepath = keypoints_filepaths[image_name] kapture_keypoints_query = image_keypoints_from_file( filepath=keypoints_filepath, dsize=kapture_data.keypoints[keypoints_type].dsize, dtype=kapture_data.keypoints[keypoints_type].dtype) query_cam = kapture_query_data.sensors[sensor_id] assert isinstance(query_cam, kapture.Camera) num_keypoints = kapture_keypoints_query.shape[0] kapture_keypoints_query, K, distortion = get_camera_matrix_from_kapture( kapture_keypoints_query, query_cam) kapture_keypoints_query = kapture_keypoints_query.reshape( (num_keypoints, 2)) cv2_keypoints_query = np.copy(kapture_keypoints_query) if np.count_nonzero(distortion) > 0: epsilon = np.finfo(np.float64).eps stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 500, epsilon) cv2_keypoints_query = cv2.undistortPointsIter( cv2_keypoints_query, K, distortion, R=None, P=K, criteria=stop_criteria) cv2_keypoints_query = cv2_keypoints_query.reshape((num_keypoints, 2)) # center keypoints for i in range(cv2_keypoints_query.shape[0]): cv2_keypoints_query[i, 0] = cv2_keypoints_query[i, 0] - K[0, 2] cv2_keypoints_query[i, 1] = cv2_keypoints_query[i, 1] - K[1, 2] kpts_query = kapture_keypoints_query if ( refine_poses or write_detailed_report) else None points2D, points2D_undistorted, points3D, stats = get_correspondences( kapture_data, keypoints_type, kapture_path, tar_handlers, image_name, pairs[image_name], point_id_from_obs, kpts_query, cv2_keypoints_query, duplicate_strategy, rerank_strategy) # compute absolute pose # inlier_threshold - RANSAC inlier threshold in pixels # answer - dictionary containing the RANSAC output ret = pyransaclib.ransaclib_localization(image_name, K[0, 0], K[1, 1], points2D_undistorted, points3D, inlier_threshold, number_lo_steps, min_num_iterations, max_num_iterations) # add pose to output kapture if ret['success'] and ret['num_inliers'] > 0: pose = kapture.PoseTransform(ret['qvec'], ret['tvec']) if refine_poses: inlier_mask = np.zeros((len(points2D), ), dtype=bool) inlier_mask[ret['inliers']] = True inlier_mask = inlier_mask.tolist() col_cam_id, width, height, params, _ = get_colmap_camera( query_cam) cfg = { 'model': CAMERA_MODEL_NAME_ID[col_cam_id][0], 'width': int(width), 'height': int(height), 'params': params } ret_refine = pycolmap.pose_refinement(pose.t_raw, pose.r_raw, points2D, points3D, inlier_mask, cfg) if ret_refine['success']: pose = kapture.PoseTransform(ret_refine['qvec'], ret_refine['tvec']) logger.debug( f'{image_name} refinement success, new pose: {pose}') if write_detailed_report: reprojection_error = compute_reprojection_error( pose, ret['num_inliers'], ret['inliers'], points2D, points3D, K, distortion) cache = { "num_correspondences": len(points3D), "num_inliers": ret['num_inliers'], "inliers": ret['inliers'], "reprojection_error": reprojection_error, "stats": stats } cache_path = os.path.join( output_path, f'pyransaclib_cache/{image_name}.json') save_to_json(cache, cache_path) trajectories[timestamp, sensor_id] = pose progress_bar.update(1) progress_bar.close() kapture_data_localized = kapture.Kapture( sensors=kapture_query_data.sensors, trajectories=trajectories, records_camera=kapture_query_data.records_camera, rigs=kapture_query_data.rigs) kapture.io.csv.kapture_to_dir(output_path, kapture_data_localized)
def test_import(self): image_transfer_action = TransferAction.skip with tempfile.TemporaryDirectory() as tmpdirname: configuration = 'all' light_range = [1, 4] loop_range = [1] camera_range = list(range(0, 6)) occlusion_range = [2, 3] as_rig = False import_virtual_gallery(self.virtual_gallery_1_0_0_folder, configuration, light_range, loop_range, camera_range, occlusion_range, as_rig, image_transfer_action, tmpdirname, force_overwrite_existing=True) expected_kdata = kapture_from_dir(path.join(self.virtual_gallery_folder, 'kapture/all')) imported_virtual_gallery_data = kapture_from_dir(tmpdirname) self.assertTrue(equal_kapture(imported_virtual_gallery_data, expected_kdata)) with tempfile.TemporaryDirectory() as tmpdirname: configuration = 'all' light_range = [4] loop_range = [1] camera_range = [2] occlusion_range = [3] as_rig = False import_virtual_gallery(self.virtual_gallery_1_0_0_folder, configuration, light_range, loop_range, camera_range, occlusion_range, as_rig, image_transfer_action, tmpdirname, force_overwrite_existing=True) expected_kdata = kapture_from_dir(path.join(self.virtual_gallery_folder, 'kapture/reduced')) imported_virtual_gallery_data = kapture_from_dir(tmpdirname) self.assertTrue(equal_kapture(imported_virtual_gallery_data, expected_kdata)) with tempfile.TemporaryDirectory() as tmpdirname: configuration = 'training' light_range = [1, 4] loop_range = [1] camera_range = list(range(0, 6)) occlusion_range = [2, 3] as_rig = False import_virtual_gallery(self.virtual_gallery_1_0_0_folder, configuration, light_range, loop_range, camera_range, occlusion_range, as_rig, image_transfer_action, tmpdirname, force_overwrite_existing=True) expected_kdata = kapture_from_dir(path.join(self.virtual_gallery_folder, 'kapture/training')) imported_virtual_gallery_data = kapture_from_dir(tmpdirname) self.assertTrue(equal_kapture(imported_virtual_gallery_data, expected_kdata)) with tempfile.TemporaryDirectory() as tmpdirname: configuration = 'training' light_range = [1, 4] loop_range = [1] camera_range = list(range(0, 6)) occlusion_range = [2, 3] as_rig = True import_virtual_gallery(self.virtual_gallery_1_0_0_folder, configuration, light_range, loop_range, camera_range, occlusion_range, as_rig, image_transfer_action, tmpdirname, force_overwrite_existing=True) expected_kdata = kapture_from_dir(path.join(self.virtual_gallery_folder, 'kapture/training')) imported_virtual_gallery_data = kapture_from_dir(tmpdirname) self.assertFalse(equal_kapture(imported_virtual_gallery_data, expected_kdata)) rigs_remove_inplace(imported_virtual_gallery_data.trajectories, imported_virtual_gallery_data.rigs) self.assertTrue(equal_kapture(imported_virtual_gallery_data, expected_kdata)) with tempfile.TemporaryDirectory() as tmpdirname: configuration = 'testing' light_range = [1, 4] loop_range = [1] camera_range = list(range(0, 6)) occlusion_range = [2, 3] as_rig = False import_virtual_gallery(self.virtual_gallery_1_0_0_folder, configuration, light_range, loop_range, camera_range, occlusion_range, as_rig, image_transfer_action, tmpdirname, force_overwrite_existing=True) expected_kdata = kapture_from_dir(path.join(self.virtual_gallery_folder, 'kapture/testing')) imported_virtual_gallery_data = kapture_from_dir(tmpdirname) self.assertTrue(equal_kapture(imported_virtual_gallery_data, expected_kdata))
def colmap_build_sift_map(kapture_path: str, colmap_path: str, colmap_binary: str, colmap_use_cpu: bool, colmap_gpu_index: str, vocab_tree_path: str, point_triangulator_options: List[str], skip_list: List[str], force: bool) -> None: """ Build a colmap model using default SIFT features with the kapture data. :param kapture_path: path to the kapture to use :param colmap_path: path to the colmap build :param colmap_binary: path to the colmap executable :param colmap_use_cpu: to use cpu only (and ignore gpu) or to use also gpu :param colmap_gpu_index: gpu index for sift extractor and mapper :param vocab_tree_path: path to the colmap vocabulary tree file :param point_triangulator_options: options for the point triangulator :param skip_list: list of steps to skip :param force: Silently overwrite kapture files if already exists. """ os.makedirs(colmap_path, exist_ok=True) # Load input files first to make sure it is OK logger.info('loading kapture files...') kapture_data = kapture.io.csv.kapture_from_dir(kapture_path) if not (kapture_data.records_camera and kapture_data.sensors): raise ValueError('records_camera, sensors are mandatory') if not kapture_data.trajectories: logger.info('there are no trajectories, running mapper instead of point_triangulator') if not os.path.isfile(vocab_tree_path): raise ValueError(f'Vocabulary Tree file does not exist: {vocab_tree_path}') # COLMAP does not fully support rigs. if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Set fixed name for COLMAP database colmap_db_path = path.join(colmap_path, 'colmap.db') image_list_path = path.join(colmap_path, 'images.list') reconstruction_path = path.join(colmap_path, "reconstruction") if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) safe_remove_file(image_list_path, force) safe_remove_any_path(reconstruction_path, force) os.makedirs(reconstruction_path, exist_ok=True) if 'feature_extract' not in skip_list: logger.info("Step 1: Feature extraction using colmap") with open(image_list_path, 'w') as fid: for timestamp, sensor_id in sorted(kapture_data.records_camera.key_pairs()): fid.write(kapture_data.records_camera[timestamp][sensor_id] + "\n") colmap_lib.run_feature_extractor( colmap_binary, colmap_use_cpu, colmap_gpu_index, colmap_db_path, get_image_fullpath(kapture_path), image_list_path ) # Update cameras in COLMAP: # - use only one camera for all images taken with the same camera (update all camera IDs) # - import camera intrinsics # - import camera pose if 'update_db_cameras' not in skip_list: logger.info("Step 2: Populate COLMAP DB with cameras and poses") colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.update_DB_cameras_and_poses(colmap_db, kapture_data) # close db before running colmap processes in order to avoid locks colmap_db.close() # Extract matches with COLMAP if 'matches' not in skip_list: logger.info("Step 3: Compute matches with colmap") colmap_lib.run_vocab_tree_matcher( colmap_binary, colmap_use_cpu, colmap_gpu_index, colmap_db_path, vocab_tree_path) if kapture_data.trajectories is not None: # Generate priors for reconstruction txt_path = path.join(colmap_path, "priors_for_reconstruction") os.makedirs(txt_path, exist_ok=True) if 'priors_for_reconstruction' not in skip_list: logger.info('Step 4: Exporting priors for reconstruction.') colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.generate_priors_for_reconstruction(kapture_data, colmap_db, txt_path) colmap_db.close() # Point triangulator reconstruction_path = path.join(colmap_path, "reconstruction") os.makedirs(reconstruction_path, exist_ok=True) if 'triangulation' not in skip_list: logger.info("Step 5: Triangulation") colmap_lib.run_point_triangulator( colmap_binary, colmap_db_path, get_image_fullpath(kapture_path), txt_path, reconstruction_path, point_triangulator_options ) else: # mapper reconstruction_path = path.join(colmap_path, "reconstruction") os.makedirs(reconstruction_path, exist_ok=True) if 'triangulation' not in skip_list: logger.info("Step 5: Triangulation") colmap_lib.run_mapper( colmap_binary, colmap_db_path, get_image_fullpath(kapture_path), None, reconstruction_path, point_triangulator_options ) # use reconstruction 0 as main first_reconstruction = os.path.join(reconstruction_path, '0') files = os.listdir(first_reconstruction) for f in files: shutil.move(os.path.join(first_reconstruction, f), os.path.join(reconstruction_path, f)) shutil.rmtree(first_reconstruction) # run model_converter if 'model_converter' not in skip_list: logger.info("Step 6: Export reconstruction results to txt") colmap_lib.run_model_converter( colmap_binary, reconstruction_path, reconstruction_path )
def pycolmap_localize_from_loaded_data( kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: TarCollection, kapture_query_data: kapture.Kapture, output_path: str, pairsfile_path: str, max_error: float, min_inlier_ratio: float, min_num_iterations: int, max_num_iterations: int, confidence: float, keypoints_type: Optional[str], duplicate_strategy: DuplicateCorrespondencesStrategy, rerank_strategy: RerankCorrespondencesStrategy, write_detailed_report: bool, force: bool) -> None: """ Localize images using pycolmap. :param kapture_data: loaded kapture data (incl. points3d) :param kapture_path: path to the kapture to use :param tar_handlers: collection of pre-opened tar archives :param kapture_data: loaded kapture data (mapping and query images) :param output_path: path to the write the localization results :param pairsfile_path: pairs to use :param max_error: RANSAC inlier threshold in pixel :param min_inlier_ratio: abs_pose_options.ransac_options.min_inlier_ratio :param min_num_iterations: abs_pose_options.ransac_options.min_num_trials :param max_num_iterations: abs_pose_options.ransac_options.max_num_trials :param confidence: abs_pose_options.ransac_options.confidence :param keypoints_type: types of keypoints (and observations) to use :param duplicate_strategy: strategy to handle duplicate correspondences (either kpt_id and/or pt3d_id) :param rerank_strategy: strategy to reorder pairs before handling duplicate correspondences :param write_detailed_report: if True, write a json file with inliers, reprojection error for each query :param force: Silently overwrite kapture files if already exists """ assert has_pycolmap if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.points3d and kapture_data.observations): raise ValueError('records_camera, sensors, keypoints, matches, ' 'points3d, observations are mandatory for map+query') if not (kapture_query_data.records_camera and kapture_query_data.sensors): raise ValueError('records_camera, sensors are mandatory for query') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection( kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() if kapture_query_data.trajectories is not None: logger.warning( "Input query data contains trajectories: they will be ignored") kapture_query_data.trajectories.clear() os.umask(0o002) os.makedirs(output_path, exist_ok=True) delete_existing_kapture_files(output_path, force_erase=force) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = kapture.io.csv.table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kapture_data.matches[keypoints_type].normalize() keypoints_filepaths = keypoints_to_filepaths( kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path, tar_handlers) obs_for_keypoints_type = { point_id: per_keypoints_type_subdict[keypoints_type] for point_id, per_keypoints_type_subdict in kapture_data.observations.items() if keypoints_type in per_keypoints_type_subdict } point_id_from_obs = { (img_name, kp_id): point_id for point_id in obs_for_keypoints_type.keys() for img_name, kp_id in obs_for_keypoints_type[point_id] } query_images = [(timestamp, sensor_id, image_name) for timestamp, sensor_id, image_name in kapture.flatten( kapture_query_data.records_camera)] # kapture for localized images + pose trajectories = kapture.Trajectories() for timestamp, sensor_id, image_name in tqdm( query_images, disable=logging.getLogger().level >= logging.CRITICAL): if image_name not in pairs: continue # N number of correspondences # points2D - Nx2 array with pixel coordinates # points3D - Nx3 array with world coordinates points2D = [] points3D = [] keypoints_filepath = keypoints_filepaths[image_name] kapture_keypoints_query = image_keypoints_from_file( filepath=keypoints_filepath, dsize=kapture_data.keypoints[keypoints_type].dsize, dtype=kapture_data.keypoints[keypoints_type].dtype) query_cam = kapture_query_data.sensors[sensor_id] assert isinstance(query_cam, kapture.Camera) col_cam_id, width, height, params, _ = get_colmap_camera(query_cam) cfg = { 'model': CAMERA_MODEL_NAME_ID[col_cam_id][0], 'width': int(width), 'height': int(height), 'params': params } points2D, _, points3D, stats = get_correspondences( kapture_data, keypoints_type, kapture_path, tar_handlers, image_name, pairs[image_name], point_id_from_obs, kapture_keypoints_query, None, duplicate_strategy, rerank_strategy) # compute absolute pose # inlier_threshold - RANSAC inlier threshold in pixels # answer - dictionary containing the RANSAC output ret = pycolmap.absolute_pose_estimation(points2D, points3D, cfg, max_error, min_inlier_ratio, min_num_iterations, max_num_iterations, confidence) # add pose to output kapture if ret['success'] and ret['num_inliers'] > 0: pose = kapture.PoseTransform(ret['qvec'], ret['tvec']) if write_detailed_report: num_2dpoints = len(points2D) points2D_final, K, distortion = get_camera_matrix_from_kapture( np.array(points2D, dtype=np.float), query_cam) points2D_final = list(points2D_final.reshape( (num_2dpoints, 2))) inliers = np.where(ret['inliers'])[0].tolist() reprojection_error = compute_reprojection_error( pose, ret['num_inliers'], inliers, points2D_final, points3D, K, distortion) cache = { "num_correspondences": len(points3D), "num_inliers": inliers, "inliers": ret['inliers'], "reprojection_error": reprojection_error, "stats": stats } cache_path = os.path.join(output_path, f'pycolmap_cache/{image_name}.json') save_to_json(cache, cache_path) trajectories[timestamp, sensor_id] = pose kapture_data_localized = kapture.Kapture( sensors=kapture_query_data.sensors, trajectories=trajectories, records_camera=kapture_query_data.records_camera, rigs=kapture_query_data.rigs) kapture.io.csv.kapture_to_dir(output_path, kapture_data_localized)
def kapture_to_openmvg(kapture_data: kapture.Kapture, kapture_path: str, image_action: TransferAction, openmvg_path: str) -> Dict: """ Convert the kapture data into an openMVG dataset stored as a dictionary. The format is defined here: https://openmvg.readthedocs.io/en/latest/software/SfM/SfM_OutputFormat/ :param kapture_data: the kapture data :param kapture_path: top directory of the kapture data and the images :param image_action: action to apply on images: link, copy, move or do nothing. :param openmvg_path: top directory of the openmvg data and images :return: an SfM_data, the openmvg structure, stored as a dictionary ready to be serialized """ assert kapture_data.cameras is not None assert kapture_data.records_camera is not None cameras = kapture_data.cameras # Check we don't have other sensors defined extra_sensor_number = len(kapture_data.sensors)-len(cameras) if extra_sensor_number > 0: logger.warning(f'We will ignore {extra_sensor_number} sensors that are not camera') records_camera = kapture_data.records_camera all_records_camera = list(kapture.flatten(records_camera)) trajectories = kapture_data.trajectories # openmvg does not support rigs if kapture_data.rigs: logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Compute root path and camera used in records sub_root_path: str = '' image_dirs = {} used_cameras = {} for _, cam_id, name in all_records_camera: used_cameras[cam_id] = cam_id img_dir = path.dirname(name) image_dirs[img_dir] = img_dir if len(image_dirs) > 1: # Find if they share a top path image_dirs_list = list(image_dirs.keys()) sub_root_path = path.commonpath(image_dirs_list) elif len(image_dirs) == 1: sub_root_path = next(iter(image_dirs.keys())) if image_action == TransferAction.skip: root_path = kapture_path else: # We will create a new hierarchy of images root_path = openmvg_path root_path = os.path.abspath(path.join(root_path, sub_root_path)) if image_action == TransferAction.root_link: if not sub_root_path: # We can not link directly to the top destination openmvg directory # We need an additional level root_path = path.join(root_path, 'images') kapture_records_path = get_image_fullpath(kapture_path, sub_root_path) # Do a unique images directory link # openmvg_root_path -> kapture/<records_dir>/openmvg_top_images_directory # beware that the paths are reverted in the symlink call os.symlink(kapture_records_path, root_path) sfm_data = {SFM_DATA_VERSION: SFM_DATA_VERSION_NUMBER, ROOT_PATH: root_path} polymorphic_id_current = 1 ptr_wrapper_id_current = 1 polymorphic_id_types = {} intrinsics, polymorphic_id_current, ptr_wrapper_id_current = _export_cameras(cameras, used_cameras, polymorphic_id_types, polymorphic_id_current, ptr_wrapper_id_current) views, extrinsics = _export_images_and_poses(all_records_camera, cameras, trajectories, image_action, kapture_path, root_path, sub_root_path, polymorphic_id_types, polymorphic_id_current, ptr_wrapper_id_current) sfm_data[VIEWS] = views sfm_data[INTRINSICS] = intrinsics sfm_data[EXTRINSICS] = extrinsics sfm_data[STRUCTURE] = [] sfm_data[CONTROL_POINTS] = [] return sfm_data
def colmap_localize_sift(kapture_path: str, colmap_path: str, input_database_path: str, input_reconstruction_path: str, colmap_binary: str, colmap_use_cpu: bool, colmap_gpu_index: str, vocab_tree_path: str, image_registrator_options: List[str], skip_list: List[str], force: bool) -> None: """ Localize images on a colmap model using default SIFT features with the kapture data. :param kapture_path: path to the kapture to use :param colmap_path: path to the colmap build :param input_database_path: path to the map colmap.db :param input_database_path: path to the map colmap.db :param input_reconstruction_path: path to the map reconstruction folder :param colmap_use_cpu: to use cpu only (and ignore gpu) or to use also gpu :param colmap_gpu_index: gpu index for sift extractor and mapper :param vocab_tree_path: path to the colmap vocabulary tree file :param image_registrator_options: options for the image registrator :param skip_list: list of steps to skip :param force: Silently overwrite kapture files if already exists. """ os.makedirs(colmap_path, exist_ok=True) # Set fixed name for COLMAP database # Load input files first to make sure it is OK logger.info('loading kapture files...') kapture_data = kapture.io.csv.kapture_from_dir(kapture_path) if not (kapture_data.records_camera and kapture_data.sensors): raise ValueError('records_camera, sensors are mandatory') if kapture_data.trajectories: logger.warning("Input data contains trajectories: they will be ignored") kapture_data.trajectories.clear() else: kapture_data.trajectories = kapture.Trajectories() if not os.path.isfile(vocab_tree_path): raise ValueError(f'Vocabulary Tree file does not exist: {vocab_tree_path}') # COLMAP does not fully support rigs. if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Prepare output # Set fixed name for COLMAP database colmap_db_path = path.join(colmap_path, 'colmap.db') image_list_path = path.join(colmap_path, 'images.list') reconstruction_path = path.join(colmap_path, "reconstruction") if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) safe_remove_file(image_list_path, force) safe_remove_any_path(reconstruction_path, force) os.makedirs(reconstruction_path, exist_ok=True) # Copy colmap db to output if not os.path.exists(colmap_db_path): shutil.copy(input_database_path, colmap_db_path) # find correspondences between the colmap db and the kapture data images_all = {image_path: (ts, cam_id) for ts, shot in kapture_data.records_camera.items() for cam_id, image_path in shot.items()} colmap_db = COLMAPDatabase.connect(colmap_db_path) colmap_image_ids = database_extra.get_colmap_image_ids_from_db(colmap_db) colmap_cameras = database_extra.get_camera_ids_from_database(colmap_db) colmap_images = database_extra.get_images_from_database(colmap_db) colmap_db.close() # dict ( kapture_camera -> colmap_camera_id ) colmap_camera_ids = {images_all[image_path][1]: colmap_cam_id for image_path, colmap_cam_id in colmap_images if image_path in images_all} images_to_add = {image_path: value for image_path, value in images_all.items() if image_path not in colmap_image_ids} flatten_images_to_add = [(ts, kapture_cam_id, image_path) for image_path, (ts, kapture_cam_id) in images_to_add.items()] if 'feature_extract' not in skip_list: logger.info("Step 1: Feature extraction using colmap") with open(image_list_path, 'w') as fid: for image in images_to_add.keys(): fid.write(image + "\n") colmap_lib.run_feature_extractor( colmap_binary, colmap_use_cpu, colmap_gpu_index, colmap_db_path, get_image_fullpath(kapture_path), image_list_path ) if 'matches' not in skip_list: logger.info("Step 2: Compute matches with colmap") colmap_lib.run_vocab_tree_matcher( colmap_binary, colmap_use_cpu, colmap_gpu_index, colmap_db_path, vocab_tree_path, image_list_path ) if 'fix_db_cameras' not in skip_list: logger.info("Step 3: Replace colmap generated cameras with kapture cameras") colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.foreign_keys_off(colmap_db) # remove colmap generated cameras after_feature_extraction_colmap_cameras = database_extra.get_camera_ids_from_database(colmap_db) colmap_cameras_to_remove = [cam_id for cam_id in after_feature_extraction_colmap_cameras if cam_id not in colmap_cameras] for cam_id in colmap_cameras_to_remove: database_extra.remove_camera(colmap_db, cam_id) # put the correct cameras and image extrinsic back into the database cameras_to_add = kapture.Sensors() for image_path, (ts, kapture_cam_id) in images_to_add.items(): if kapture_cam_id not in colmap_camera_ids: kapture_cam = kapture_data.sensors[kapture_cam_id] cameras_to_add[kapture_cam_id] = kapture_cam colmap_added_camera_ids = database_extra.add_cameras_to_database(cameras_to_add, colmap_db) colmap_camera_ids.update(colmap_added_camera_ids) database_extra.update_images_in_database_from_flatten( colmap_db, flatten_images_to_add, kapture_data.trajectories, colmap_camera_ids ) database_extra.foreign_keys_on(colmap_db) colmap_db.commit() colmap_db.close() if 'image_registrator' not in skip_list: logger.info("Step 4: Run image_registrator") # run image_registrator colmap_lib.run_image_registrator( colmap_binary, colmap_db_path, input_reconstruction_path, reconstruction_path, image_registrator_options ) # run model_converter if 'model_converter' not in skip_list: logger.info("Step 5: Export reconstruction results to txt") colmap_lib.run_model_converter( colmap_binary, reconstruction_path, reconstruction_path )
def export_openmvg_sfm_data(kapture_path: str, kapture_data: kapture.Kapture, openmvg_sfm_data_file_path: str, openmvg_image_root_path: str, image_action: TransferAction, image_path_flatten: bool, force: bool, kapture_to_openmvg_view_ids: dict = {}) -> Dict: """ Convert the kapture data into an openMVG dataset stored as a dictionary. The format is defined here: https://openmvg.readthedocs.io/en/latest/software/SfM/SfM_OutputFormat/ :param kapture_data: the kapture data :param kapture_path: top directory of the kapture data and the images :param openmvg_sfm_data_file_path: input path to the SfM data file to be written. :param openmvg_image_root_path: input path to openMVG image directory to be created. :param image_action: action to apply on images: link, copy, move or do nothing. :param image_path_flatten: flatten image path (eg. to avoid image name collision in openMVG regions). :param force: if true, will remove existing openMVG data without prompting the user. :param kapture_to_openmvg_view_ids: input/output mapping of kapture image name to corresponding openmvg view id. :return: an SfM_data, the openmvg structure, stored as a dictionary ready to be serialized """ if kapture_data.cameras is None or kapture_data.records_camera is None: raise ValueError( 'export_openmvg_sfm_data needs kapture camera and records_camera.') if image_action == TransferAction.root_link: raise NotImplementedError( 'root link is not implemented, use skip instead.') # refer to the original image dir when skipping image transfer. if image_action == TransferAction.skip: openmvg_image_root_path = get_image_fullpath(kapture_path) if openmvg_image_root_path is None: raise ValueError( f'openmvg_image_root_path must be defined to be able to perform {image_action}.' ) # make sure directory is ready to contain openmvg_sfm_data_file_path os.makedirs(path.dirname(openmvg_sfm_data_file_path), exist_ok=True) # Check we don't have other sensors defined if len(kapture_data.sensors) != len(kapture_data.cameras): extra_sensor_number = len(kapture_data.sensors) - len( kapture_data.cameras) logger.warning( f'We will ignore {extra_sensor_number} sensors that are not camera' ) # openmvg does not support rigs if kapture_data.rigs: logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Compute root path and camera used in records kapture_to_openmvg_cam_ids = {} # kapture_cam_id -> openmvg_cam_id for i, (_, _, kapture_image_name) in enumerate( kapture.flatten(kapture_data.records_camera)): if kapture_image_name not in kapture_to_openmvg_view_ids: kapture_to_openmvg_view_ids[kapture_image_name] = i # polymorphic_status = PolymorphicStatus({}, 1, 1) polymorphic_registry = CerealPointerRegistry( id_key=JSON_KEY.POLYMORPHIC_ID, value_key=JSON_KEY.POLYMORPHIC_NAME) ptr_wrapper_registry = CerealPointerRegistry(id_key=JSON_KEY.ID, value_key=JSON_KEY.DATA) logger.debug(f'exporting intrinsics ...') openmvg_sfm_data_intrinsics = export_openmvg_intrinsics( kapture_cameras=kapture_data.cameras, kapture_to_openmvg_cam_ids=kapture_to_openmvg_cam_ids, polymorphic_registry=polymorphic_registry, ptr_wrapper_registry=ptr_wrapper_registry, ) logger.debug(f'exporting views ...') openmvg_sfm_data_views = export_openmvg_views( kapture_cameras=kapture_data.cameras, kapture_images=kapture_data.records_camera, kapture_trajectories=kapture_data.trajectories, kapture_to_openmvg_cam_ids=kapture_to_openmvg_cam_ids, kapture_to_openmvg_view_ids=kapture_to_openmvg_view_ids, polymorphic_registry=polymorphic_registry, ptr_wrapper_registry=ptr_wrapper_registry, image_path_flatten=image_path_flatten, ) logger.debug(f'exporting poses ...') openmvg_sfm_data_poses = export_openmvg_poses( kapture_images=kapture_data.records_camera, kapture_trajectories=kapture_data.trajectories, kapture_to_openmvg_view_ids=kapture_to_openmvg_view_ids) # structure : correspond to kapture observations + 3D points logger.debug(f'exporting structure ...') openmvg_sfm_data_structure = export_openmvg_structure( kapture_points_3d=kapture_data.points3d, kapture_to_openmvg_view_ids=kapture_to_openmvg_view_ids, kapture_observations=kapture_data.observations, kapture_keypoints=kapture_data.keypoints, kapture_path=kapture_path) openmvg_sfm_data = { JSON_KEY.SFM_DATA_VERSION: OPENMVG_SFM_DATA_VERSION_NUMBER, JSON_KEY.ROOT_PATH: path.abspath(openmvg_image_root_path), JSON_KEY.INTRINSICS: openmvg_sfm_data_intrinsics, JSON_KEY.VIEWS: openmvg_sfm_data_views, JSON_KEY.EXTRINSICS: openmvg_sfm_data_poses, JSON_KEY.STRUCTURE: openmvg_sfm_data_structure, JSON_KEY.CONTROL_POINTS: [], } logger.debug(f'Saving to openmvg {openmvg_sfm_data_file_path}...') with open(openmvg_sfm_data_file_path, "w") as fid: json.dump(openmvg_sfm_data, fid, indent=4) # do the actual image transfer if not image_action == TransferAction.skip: job_copy = ( ( # source path -> dest path get_image_fullpath(kapture_path, kapture_image_name), path.join( openmvg_image_root_path, get_openmvg_image_path(kapture_image_name, image_path_flatten))) for _, _, kapture_image_name in kapture.flatten(kapture_data.records_camera)) source_filepath_list, destination_filepath_list = zip(*job_copy) transfer_files_from_dir( source_filepath_list=source_filepath_list, destination_filepath_list=destination_filepath_list, copy_strategy=image_action, force_overwrite=force)
def export_colmap(kapture_dirpath: str, colmap_database_filepath: str, colmap_reconstruction_dirpath: Optional[str], colmap_rig_filepath: str = None, force_overwrite_existing: bool = False) -> None: """ Exports kapture data to colmap database and or reconstruction text files. :param kapture_dirpath: kapture top directory :param colmap_database_filepath: path to colmap database file :param colmap_reconstruction_dirpath: path to colmap reconstruction directory :param colmap_rig_filepath: path to colmap rig file :param force_overwrite_existing: Silently overwrite colmap files if already exists. """ os.makedirs(path.dirname(colmap_database_filepath), exist_ok=True) if colmap_reconstruction_dirpath: os.makedirs(colmap_reconstruction_dirpath, exist_ok=True) assert colmap_database_filepath if path.isfile(colmap_database_filepath): to_delete = force_overwrite_existing or (input( 'database file already exist, would you like to delete it ? [y/N]' ).lower() == 'y') if to_delete: logger.info('deleting already existing {}'.format( colmap_database_filepath)) os.remove(colmap_database_filepath) logger.info( 'creating colmap database in {}'.format(colmap_database_filepath)) db = COLMAPDatabase.connect(colmap_database_filepath) if not is_colmap_db_empty(db): raise ValueError( 'the existing colmap database is not empty : {}'.format( colmap_database_filepath)) logger.info('loading kapture files...') kapture_data = csv.kapture_from_dir(kapture_dirpath) assert isinstance(kapture_data, kapture.Kapture) # COLMAP does not fully support rigs. if kapture_data.rigs is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) # write colmap database kapture_to_colmap(kapture_data, kapture_dirpath, db) if colmap_reconstruction_dirpath: # create text files colmap_camera_ids = get_colmap_camera_ids_from_db( db, kapture_data.records_camera) colmap_image_ids = get_colmap_image_ids_from_db(db) export_to_colmap_txt( colmap_reconstruction_dirpath=colmap_reconstruction_dirpath, kapture_data=kapture_data, kapture_dirpath=kapture_dirpath, colmap_camera_ids=colmap_camera_ids, colmap_image_ids=colmap_image_ids) if colmap_rig_filepath: try: if kapture_data.rigs is None: raise ValueError('No rig to export.') export_colmap_rigs(colmap_rig_filepath, kapture_data.rigs, kapture_data.records_camera, colmap_camera_ids) except Exception as e: warnings.warn(e) logger.warning(e) db.close()
def pycolmap_rig_localize_from_loaded_data( kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: TarCollection, kapture_query_data: kapture.Kapture, output_path: str, pairsfile_path: str, rig_ids: List[str], apply_rigs_remove: bool, max_error: float, min_inlier_ratio: float, min_num_iterations: int, max_num_iterations: int, confidence: float, keypoints_type: Optional[str], duplicate_strategy: DuplicateCorrespondencesStrategy, rerank_strategy: RerankCorrespondencesStrategy, write_detailed_report: bool, force: bool) -> None: """ Localize images from a multi camera rig using pycolmap :param kapture_data: loaded kapture data (incl. points3d) :param kapture_path: path to the kapture to use :param tar_handlers: collection of pre-opened tar archives :param kapture_data: loaded kapture data (mapping and query images) :param output_path: path to the write the localization results :param pairsfile_path: pairs to use :param rig_ids: list of rig ids that should be localized :param apply_rigs_remove: apply rigs remove before saving poses to disk :param max_error: RANSAC inlier threshold in pixel, shared between all cameras :param min_inlier_ratio: abs_pose_options.ransac_options.min_inlier_ratio :param min_num_iterations: abs_pose_options.ransac_options.min_num_trials :param max_num_iterations: abs_pose_options.ransac_options.max_num_trials :param confidence: abs_pose_options.ransac_options.confidence :param keypoints_type: types of keypoints (and observations) to use :param force: Silently overwrite kapture files if already exists. """ assert has_pycolmap if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.points3d and kapture_data.observations): raise ValueError('records_camera, sensors, keypoints, matches, ' 'points3d, observations are mandatory for map+query') if not (kapture_query_data.records_camera and kapture_query_data.sensors): raise ValueError('records_camera, sensors are mandatory for query') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection( kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches assert kapture_query_data.rigs is not None assert len(kapture_query_data.rigs) >= 1 if len(rig_ids) == 0: rig_ids = get_top_level_rig_ids(kapture_query_data.rigs) final_camera_list = get_all_cameras_from_rig_ids( rig_ids, kapture_query_data.sensors, kapture_query_data.rigs) assert len(final_camera_list) > 0 if kapture_query_data.trajectories: logger.warning( "Input query data contains trajectories: they will be ignored") kapture_query_data.trajectories.clear() os.umask(0o002) os.makedirs(output_path, exist_ok=True) delete_existing_kapture_files(output_path, force_erase=force) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = kapture.io.csv.table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kapture_data.matches[keypoints_type].normalize() keypoints_filepaths = keypoints_to_filepaths( kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path, tar_handlers) obs_for_keypoints_type = { point_id: per_keypoints_type_subdict[keypoints_type] for point_id, per_keypoints_type_subdict in kapture_data.observations.items() if keypoints_type in per_keypoints_type_subdict } point_id_from_obs = { (img_name, kp_id): point_id for point_id in obs_for_keypoints_type.keys() for img_name, kp_id in obs_for_keypoints_type[point_id] } timestamps = list(kapture_query_data.records_camera.keys()) # kapture for localized images + pose trajectories = kapture.Trajectories() progress_bar = tqdm(total=len(timestamps), disable=logging.getLogger().level >= logging.CRITICAL) for timestamp in timestamps: for rig_id in final_camera_list.keys(): # with S number of sensors # N number of correspondences # points2D - SxNx2 array with pixel coordinates # points3D - SxNx3 array with world coordinates # tvec - Sx3 array with rig relative translations # qvec - Sx4 array with rig relative quaternions # cameras_dict - array of dict of length S points2D = [] points3D = [] tvec = [] qvec = [] cameras_dict = [] cameras = [] # Sx2 array for reproj error stats = [] for sensor_id, relative_pose in final_camera_list[rig_id].items(): if (timestamp, sensor_id) not in kapture_query_data.records_camera: continue img_query = kapture_query_data.records_camera[(timestamp, sensor_id)] if img_query not in pairs: continue keypoints_filepath = keypoints_filepaths[img_query] kapture_keypoints_query = image_keypoints_from_file( filepath=keypoints_filepath, dsize=kapture_data.keypoints[keypoints_type].dsize, dtype=kapture_data.keypoints[keypoints_type].dtype) tvec.append(relative_pose.t_raw) qvec.append(relative_pose.r_raw) col_cam_id, width, height, params, _ = get_colmap_camera( kapture_query_data.sensors[sensor_id]) cameras_dict.append({ 'model': CAMERA_MODEL_NAMES[col_cam_id], 'width': int(width), 'height': int(height), 'params': params }) points2D_it, _, points3D_it, stats_it = get_correspondences( kapture_data, keypoints_type, kapture_path, tar_handlers, img_query, pairs[img_query], point_id_from_obs, kapture_keypoints_query, None, duplicate_strategy, rerank_strategy) if write_detailed_report: cameras.append(kapture_query_data.sensors[sensor_id]) stats.append(stats_it) points2D.append(points2D_it) points3D.append(points3D_it) if len(cameras_dict) == 0: progress_bar and progress_bar.update(1) continue # compute absolute pose # inlier_threshold - RANSAC inlier threshold in pixels # answer - dictionary containing the RANSAC output ret = pycolmap.rig_absolute_pose_estimation( points2D, points3D, cameras_dict, qvec, tvec, max_error, min_inlier_ratio, min_num_iterations, max_num_iterations, confidence) # add pose to output kapture if ret['success'] and ret['num_inliers'] > 0: pose = kapture.PoseTransform(ret['qvec'], ret['tvec']) trajectories[timestamp, rig_id] = pose if write_detailed_report: points2D_final = [] camera_params = [] for points2D_it, query_cam in zip(points2D, cameras): num_2dpoints = len(points2D_it) points2D_final_it, K, distortion = get_camera_matrix_from_kapture( np.array(points2D_it, dtype=np.float), query_cam) points2D_final_it = list( points2D_final_it.reshape((num_2dpoints, 2))) points2D_final.append(points2D_final_it) camera_params.append((K, distortion)) num_correspondences = [ len(points2D_it) for points2D_it in points2D ] # convert ret['inliers'] indexes_flat = [ i for i, points2D_it in enumerate(points2D) for _ in points2D_it ] inliers = [[] for _ in range(len(points2D))] for i, (is_inlier, cam_index) in enumerate( zip(ret['inliers'], indexes_flat)): if is_inlier: inliers[cam_index].append(i) cumulative_len_correspondences = [] s = 0 for num_correspondences_it in num_correspondences: cumulative_len_correspondences.append(s) s += num_correspondences_it inliers = [[ v - cumulative_len_correspondences[i] for v in inliers[i] ] for i in range(len(inliers))] num_inliers = [len(inliers_it) for inliers_it in inliers] per_image_reprojection_error = [] for tvec_it, qvec_it, points2D_it, points3D_it, inliers_it, camera_params_it in zip( tvec, qvec, points2D_final, points3D, inliers, camera_params): if len(inliers_it) == 0: per_image_reprojection_error.append(np.nan) else: pose_relative_it = kapture.PoseTransform( r=qvec_it, t=tvec_it) # rig to sensor # pose = world to rig pose_it = kapture.PoseTransform.compose( [pose_relative_it, pose]) # world to sensor reprojection_error = compute_reprojection_error( pose_it, len(inliers_it), inliers_it, points2D_it, points3D_it, camera_params_it[0], camera_params_it[1]) per_image_reprojection_error.append( reprojection_error) cache = { "num_correspondences": num_correspondences, "num_inliers": num_inliers, "inliers": inliers, "reprojection_error": per_image_reprojection_error, "stats": stats } cache_path = os.path.join( output_path, f'pycolmap_rig_cache/{timestamp}.json') save_to_json(cache, cache_path) progress_bar and progress_bar.update(1) progress_bar and progress_bar.close() # save output kapture if apply_rigs_remove: rigs_remove_inplace(trajectories, kapture_query_data.rigs) kapture_query_data.trajectories = trajectories kapture.io.csv.kapture_to_dir(output_path, kapture_query_data)
def reconstruct(self, kapture_data): os.makedirs(self._colmap_path, exist_ok=True) if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.trajectories): raise ValueError( 'records_camera, sensors, keypoints, matches, trajectories are mandatory' ) # Set fixed name for COLMAP database colmap_db_path = path.join(self._colmap_path, 'colmap.db') reconstruction_path = path.join(self._colmap_path, "reconstruction") priors_txt_path = path.join(self._colmap_path, "priors_for_reconstruction") safe_remove_file(colmap_db_path, True) safe_remove_any_path(reconstruction_path, True) safe_remove_any_path(priors_txt_path, True) os.makedirs(reconstruction_path, exist_ok=True) # COLMAP does not fully support rigs. print("Step 1. Remove rigs") if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() print("Step 2. Kapture to colmap") colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.kapture_to_colmap(kapture_data, kapture_data.kapture_path, colmap_db, export_two_view_geometry=True) colmap_db.close() os.makedirs(priors_txt_path, exist_ok=True) print("Step 3. Generate priors for reconstruction") colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.generate_priors_for_reconstruction( kapture_data, colmap_db, priors_txt_path) colmap_db.close() # Point triangulator print("Step 4. Point triangulator") reconstruction_path = path.join(self._colmap_path, "reconstruction") os.makedirs(reconstruction_path, exist_ok=True) run_point_triangulator(self._colmap_binary, colmap_db_path, kapture_data.image_path, priors_txt_path, reconstruction_path, self._point_triangulator_options) print("Step 5. Model converter") run_model_converter(self._colmap_binary, reconstruction_path, reconstruction_path) print("Step 5. Reconstruction import") points3d, observations = import_from_colmap_points3d_txt( os.path.join(reconstruction_path, "points3D.txt"), kapture_data.image_names) kapture_data.observations = observations kapture_data.points3d = points3d
def local_sfm(map_plus_query_path: str, map_plus_query_gv_path: str, query_path: str, pairsfile_path: str, output_path_root: str, colmap_binary: str, force: bool): """ Localize query images in a COLMAP model built from topk retrieved images. :param map_plus_query_path: path to the kapture data consisting of mapping and query data (sensors and reconstruction) :param map_plus_query_gv_path: path to the kapture data consisting of mapping and query data after geometric verification (sensors and reconstruction) :param query_path: path to the query kapture data (sensors) :param pairsfile_path: path to the pairsfile that contains the topk retrieved mapping images for each query image :param output_path_root: root path where outputs should be stored :param colmap_binary: path to the COLMAP binary :param force: silently overwrite already existing results """ # load query kapture (we use query kapture to reuse sensor_ids etc.) kdata_query = kapture_from_dir(query_path) if kdata_query.trajectories: logger.warning( "Query data contains trajectories: they will be ignored") kdata_query.trajectories.clear() else: kdata_query.trajectories = kapture.Trajectories() # load output kapture output_path = os.path.join(output_path_root, 'localized') if os.path.exists(os.path.join(output_path, 'sensors/trajectories.txt')): kdata_output = kapture_from_dir(output_path) if kdata_query.records_camera == kdata_output.records_camera and len( kdata_output.trajectories) != 0 and not force: kdata_query.trajectories = kdata_output.trajectories # load kapture maps kdata_map = kapture_from_dir(map_plus_query_path) if kdata_map.rigs != None: rigs_remove_inplace(kdata_map.trajectories, kdata_map.rigs) kdata_map_gv = kapture_from_dir(map_plus_query_gv_path) if kdata_map_gv.rigs != None: rigs_remove_inplace(kdata_map_gv.trajectories, kdata_map_gv.rigs) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = table_from_file(fid) for img_query, img_map, score in table: if not img_query in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kdata_sub_colmap_path = os.path.join(output_path_root, 'colmap') kdata_reg_query_path = os.path.join(output_path_root, 'query_registered') sub_kapture_pairsfile_path = os.path.join(output_path_root, 'tmp_pairs_map.txt') query_img_kapture_pairsfile_path = os.path.join(output_path_root, 'tmp_pairs_query.txt') # loop over query images for img_query, img_list_map in pairs.items(): if pose_found(kdata_query, img_query): logger.info(f'{img_query} already processed, skipping...') continue else: logger.info(f'processing {img_query}') # write pairsfile for sub-kapture map_pairs = write_pairfile_from_img_list(img_list_map, sub_kapture_pairsfile_path) # write pairsfile for query_img_kapture query_pairs = write_pairfile_img_vs_img_list( img_query, img_list_map, query_img_kapture_pairsfile_path) # create sub-kapture kdata_sub = sub_kapture_from_img_list(kdata_map, map_plus_query_path, img_list_map, map_pairs) kdata_sub_gv = sub_kapture_from_img_list(kdata_map_gv, map_plus_query_gv_path, img_list_map, map_pairs) # match missing pairs for mapping compute_matches_from_loaded_data(map_plus_query_path, kdata_sub, map_pairs) # kdata_sub needs to be re-created to add the new matches kdata_sub = sub_kapture_from_img_list(kdata_map, map_plus_query_path, img_list_map, map_pairs) # run colmap gv on missing pairs if len(kdata_sub.matches) != len(kdata_sub_gv.matches): run_colmap_gv_from_loaded_data(kdata_sub, kdata_sub_gv, map_plus_query_path, map_plus_query_gv_path, colmap_binary, [], True) # kdata_sub_gv needs to be re-created to add the new matches kdata_sub_gv = sub_kapture_from_img_list(kdata_map_gv, map_plus_query_gv_path, img_list_map, map_pairs) # sanity check if len(map_pairs) != len(kdata_sub_gv.matches): logger.info(f'not all mapping matches available') # build COLMAP map try: colmap_build_map_from_loaded_data(kdata_sub_gv, map_plus_query_gv_path, kdata_sub_colmap_path, colmap_binary, False, [], ['model_converter'], True) except ValueError: logger.info(f'{img_query} was not localized') continue if not os.path.exists( os.path.join(kdata_sub_colmap_path, 'reconstruction/images.bin')): logger.info( f'colmap mapping for {img_query} did not work, image was not localized' ) continue # create single image kapture (kdata_sub needs to be recreated because descriptors are deleted in build_colmap_model) kdata_sub = sub_kapture_from_img_list(kdata_map, map_plus_query_path, img_list_map, map_pairs) kdata_sub_gv = sub_kapture_from_img_list(kdata_map_gv, map_plus_query_gv_path, img_list_map, map_pairs) query_img_kapture = add_image_to_kapture(kdata_map, map_plus_query_path, kdata_sub, img_query, query_pairs) query_img_kapture_gv = add_image_to_kapture(kdata_map_gv, map_plus_query_gv_path, kdata_sub_gv, img_query, query_pairs) # match missing pairs for localization compute_matches_from_loaded_data(map_plus_query_path, query_img_kapture, query_pairs) # query_img_kapture needs to be re-created to add the new matches query_img_kapture = add_image_to_kapture(kdata_map, map_plus_query_path, kdata_sub, img_query, query_pairs) # run colmap gv on missing pairs if len(query_img_kapture.matches) != len(query_img_kapture_gv.matches): run_colmap_gv_from_loaded_data(query_img_kapture, query_img_kapture_gv, map_plus_query_path, map_plus_query_gv_path, colmap_binary, [], True) # query_img_kapture_gv needs to be re-created to add the new matches query_img_kapture_gv = add_image_to_kapture( kdata_map_gv, map_plus_query_gv_path, kdata_sub_gv, img_query, query_pairs) # sanity check if len(query_pairs) != len(query_img_kapture_gv.matches): logger.info(f'not all query matches available') # localize in COLMAP map try: colmap_localize_from_loaded_data( query_img_kapture_gv, map_plus_query_gv_path, os.path.join(kdata_sub_colmap_path, 'registered'), os.path.join(kdata_sub_colmap_path, 'colmap.db'), os.path.join(kdata_sub_colmap_path, 'reconstruction'), colmap_binary, False, [ '--Mapper.ba_refine_focal_length', '0', '--Mapper.ba_refine_principal_point', '0', '--Mapper.ba_refine_extra_params', '0', '--Mapper.min_num_matches', '4', '--Mapper.init_min_num_inliers', '4', '--Mapper.abs_pose_min_num_inliers', '4', '--Mapper.abs_pose_min_inlier_ratio', '0.05', '--Mapper.ba_local_max_num_iterations', '50', '--Mapper.abs_pose_max_error', '20', '--Mapper.filter_max_reproj_error', '12' ], [], True) except ValueError: logger.info(f'{img_query} was not localized') continue if not os.path.exists( os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'reconstruction/images.txt')): logger.info( f'colmap localization of {img_query} did not work, image was not localized' ) continue # add to results kapture kdata_reg_query = import_colmap( kdata_reg_query_path, os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'colmap.db'), os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'reconstruction'), None, None, True, True, True, TransferAction.skip) if add_pose_to_query_kapture(kdata_reg_query, kdata_query, img_query): logger.info('successfully localized') # write results (after each image to see the progress) kapture_to_dir(output_path, kdata_query) # clean up (e.g. remove temporal files and folders) safe_remove_any_path(kdata_sub_colmap_path, True) safe_remove_any_path(kdata_reg_query_path, True) safe_remove_file(sub_kapture_pairsfile_path, True) safe_remove_file(query_img_kapture_pairsfile_path, True) logger.info('all done')
def kapture_to_openmvg(kapture_data: kapture.Kapture, kapture_path: str, image_action: TransferAction, openmvg_path: str) -> Dict: """ Convert the kapture data into an openMVG dataset stored as a dictionary. The format is defined here: https://openmvg.readthedocs.io/en/latest/software/SfM/SfM_OutputFormat/ :param kapture_data: the kapture data :param kapture_path: top directory of the kapture data and the images :param image_action: action to apply on images: link, copy, move or do nothing. :param openmvg_path: top directory of the openmvg data and images :return: an SfM_data, the openmvg structure, stored as a dictionary ready to be serialized """ assert kapture_data.cameras is not None assert kapture_data.records_camera is not None cameras = kapture_data.cameras # Check we don't have other sensors defined extra_sensor_number = len(kapture_data.sensors)-len(cameras) if extra_sensor_number > 0: logger.warning(f'We will ignore {extra_sensor_number} sensors that are not camera') records_camera = kapture_data.records_camera all_records_camera = list(kapture.flatten(records_camera)) trajectories = kapture_data.trajectories # openmvg does not support rigs if kapture_data.rigs: logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() # Compute root path and camera used in records sub_root_path: str = '' image_dirs = {} used_cameras = {} for _, cam_id, name in all_records_camera: used_cameras[cam_id] = cam_id img_dir = path.dirname(name) image_dirs[img_dir] = img_dir if len(image_dirs) > 1: # Find if they share a top path image_dirs_list = list(image_dirs.keys()) sub_root_path = path.commonpath(image_dirs_list) elif len(image_dirs) == 1: sub_root_path = next(iter(image_dirs.keys())) if image_action == TransferAction.skip: root_path = kapture_path else: # We will create a new hierarchy of images root_path = openmvg_path root_path = os.path.abspath(path.join(root_path, sub_root_path)) if image_action == TransferAction.root_link: if not sub_root_path: # We can not link directly to the top destination openmvg directory # We need an additional level root_path = path.join(root_path, 'images') kapture_records_path = get_image_fullpath(kapture_path, sub_root_path) # Do a unique images directory link # openmvg_root_path -> kapture/<records_dir>/openmvg_top_images_directory # beware that the paths are reverted in the symlink call os.symlink(kapture_records_path, root_path) sfm_data = {SFM_DATA_VERSION: SFM_DATA_VERSION_NUMBER, ROOT_PATH: root_path} views = [] intrinsics = [] extrinsics = [] polymorphic_id_current = 1 ptr_wrapper_id_current = 1 polymorphic_id_types = {} # process all cameras for cam_id, camera in cameras.items(): # Ignore not used cameras if not used_cameras.get(cam_id): logger.warning(f'Skipping camera definition {cam_id} {camera.name} without recorded images.') continue cam_type = camera.camera_type camera_params = camera.camera_params if cam_type == kapture.CameraType.SIMPLE_PINHOLE: # w, h, f, cx, cy model_used = CameraModel.pinhole data = _get_intrinsic_pinhole(camera_params) elif cam_type == kapture.CameraType.PINHOLE: # w, h, f, cx, cy model_used = CameraModel.pinhole faked_params = [camera_params[0], camera_params[1], # width height (camera_params[2] + camera_params[3])/2, # fx+fy/2 as f camera_params[4], camera_params[5]] # cx cy data = _get_intrinsic_pinhole(faked_params) elif cam_type == kapture.CameraType.SIMPLE_RADIAL: # w, h, f, cx, cy, k model_used = CameraModel.pinhole_radial_k1 data = _get_intrinsic_pinhole_radial_k1(camera_params) elif cam_type == kapture.CameraType.RADIAL: # w, h, f, cx, cy, k1, k2, k3 model_used = CameraModel.pinhole_radial_k3 faked_params = [camera_params[0], camera_params[1], # width height camera_params[2], # f camera_params[3], camera_params[4], # cx cy camera_params[5], camera_params[6], 0 # k1, k2, k3 ] data = _get_intrinsic_pinhole_radial_k3(faked_params) elif cam_type == kapture.CameraType.FULL_OPENCV or cam_type == kapture.CameraType.OPENCV: # w, h, f, cx, cy, k1, k2, k3, t1, t2 model_used = CameraModel.pinhole_brown_t2 k3 = camera_params[10] if len(camera_params) > 10 else 0 faked_params = [camera_params[0], camera_params[1], # width height (camera_params[2] + camera_params[3])/2, # fx+fy/2 as f camera_params[4], camera_params[5], # cx cy camera_params[6], camera_params[7], k3, # k1, k2, k3 camera_params[8], camera_params[9] # p1, p2 (=t1, t2) ] data = _get_intrinsic_pinhole_brown_t2(faked_params) elif cam_type == kapture.CameraType.OPENCV_FISHEYE: logger.warning('OpenCV fisheye model is not compatible with OpenMVG. Forcing distortion to 0') # w, h, f, cx, cy, k1, k2, k3, k4 model_used = CameraModel.fisheye faked_params = [camera_params[0], camera_params[1], # width height (camera_params[2] + camera_params[3]) / 2, # fx+fy/2 as f camera_params[4], camera_params[5], # cx cy 0, 0, # k1, k2 0, 0 # k3, k4 ] data = _get_intrinsic_fisheye(faked_params) elif cam_type == kapture.CameraType.RADIAL_FISHEYE or cam_type == kapture.CameraType.SIMPLE_RADIAL_FISHEYE: logger.warning('OpenCV fisheye model is not compatible with OpenMVG. Forcing distortion to 0') # w, h, f, cx, cy, k1, k2, k3, k4 model_used = CameraModel.fisheye faked_params = [camera_params[0], camera_params[1], # width height camera_params[2], # f camera_params[3], camera_params[4], # cx cy 0, 0, # k1, k2 0, 0 # k3, k4 ] data = _get_intrinsic_fisheye(faked_params) elif cam_type == kapture.CameraType.UNKNOWN_CAMERA: logger.info(f'Camera {cam_id}: Unknown camera model, using simple radial') # Choose simple radial model, to allow openMVG to determine distortion param # w, h, f, cx, cy, k model_used = CameraModel.pinhole_radial_k1 faked_params = [camera_params[0], camera_params[1], # width height max(camera_params[0], camera_params[1])*DEFAULT_FOCAL_LENGTH_FACTOR, # max(w,h)*1.2 as f int(camera_params[0]/2), int(camera_params[1]/2), # cx cy 0.0] # k1 data = _get_intrinsic_pinhole_radial_k1(faked_params) else: raise ValueError(f'Camera model {cam_type.value} not supported') intrinsic = {} if model_used not in polymorphic_id_types: # if this is the first time model_used is encountered # set the first bit of polymorphic_id_current to 1 intrinsic[POLYMORPHIC_ID] = polymorphic_id_current | NEW_ID_MASK intrinsic[POLYMORPHIC_NAME] = model_used.name polymorphic_id_types[model_used] = polymorphic_id_current polymorphic_id_current += 1 else: intrinsic[POLYMORPHIC_ID] = polymorphic_id_types[model_used] # it is assumed that this camera is only encountered once # set the first bit of ptr_wrapper_id_current to 1 data_wrapper = {ID: ptr_wrapper_id_current | NEW_ID_MASK, DATA: data} ptr_wrapper_id_current += 1 intrinsic[PTR_WRAPPER] = data_wrapper intrinsics.append({KEY: cam_id, VALUE: intrinsic}) global_timestamp = 0 # process all images for timestamp, cam_id, kapture_name in all_records_camera: local_path = path.dirname(path.relpath(kapture_name, sub_root_path)) filename = path.basename(kapture_name) if image_action != TransferAction.skip and image_action != TransferAction.root_link: # Process the image action src_path = get_image_fullpath(kapture_path, kapture_name) dst_path = path.join(root_path, local_path, filename) dst_dir = path.dirname(dst_path) if not path.isdir(dst_dir): os.makedirs(dst_dir, exist_ok=True) # Check if already exist if path.exists(dst_path): os.unlink(dst_path) # Create file or link if image_action == TransferAction.copy: shutil.copy2(src_path, dst_path) elif image_action == TransferAction.move: shutil.move(src_path, dst_path) else: # Link if image_action == TransferAction.link_relative: # Compute relative path src_path = path.relpath(src_path, dst_dir) os.symlink(src_path, dst_path) camera_params = cameras[cam_id].camera_params view_data = {LOCAL_PATH: local_path, FILENAME: filename, WIDTH: int(camera_params[0]), HEIGHT: int(camera_params[1]), ID_VIEW: global_timestamp, ID_INTRINSIC: cam_id, ID_POSE: global_timestamp} view = {} # retrieve image pose from trajectories if timestamp not in trajectories: view[POLYMORPHIC_ID] = VIEW_SPECIAL_POLYMORPHIC_ID else: # there is a pose for that timestamp # The poses are stored both as priors (in the 'views' table) and as known poses (in the 'extrinsics' table) assert cam_id in trajectories[timestamp] if VIEW_PRIORS not in polymorphic_id_types: # if this is the first time view_priors is encountered # set the first bit of polymorphic_id_current to 1 view[POLYMORPHIC_ID] = polymorphic_id_current | NEW_ID_MASK view[POLYMORPHIC_NAME] = VIEW_PRIORS polymorphic_id_types[VIEW_PRIORS] = polymorphic_id_current polymorphic_id_current += 1 else: view[POLYMORPHIC_ID] = polymorphic_id_types[VIEW_PRIORS] pose_tr = trajectories[timestamp].get(cam_id) prior_q = pose_tr.r prior_t = pose_tr.inverse().t_raw pose_data = {CENTER: prior_t, ROTATION: quaternion.as_rotation_matrix(prior_q).tolist()} view_data[USE_POSE_CENTER_PRIOR] = True view_data[CENTER_WEIGHT] = [1.0, 1.0, 1.0] view_data[CENTER] = prior_t view_data[USE_POSE_ROTATION_PRIOR] = True view_data[ROTATION_WEIGHT] = 1.0 view_data[ROTATION] = pose_data[ROTATION] extrinsics.append({KEY: global_timestamp, VALUE: pose_data}) # it is assumed that this view is only encountered once # set the first bit of ptr_wrapper_id_current to 1 view_wrapper = {ID: ptr_wrapper_id_current | NEW_ID_MASK, DATA: view_data} ptr_wrapper_id_current += 1 view[PTR_WRAPPER] = view_wrapper views.append({KEY: global_timestamp, VALUE: view}) global_timestamp += 1 sfm_data[VIEWS] = views sfm_data[INTRINSICS] = intrinsics sfm_data[EXTRINSICS] = extrinsics sfm_data[STRUCTURE] = [] sfm_data[CONTROL_POINTS] = [] return sfm_data
def local_sfm_from_loaded_data(kdata_map: kapture.Kapture, kdata_map_gv: kapture.Kapture, kdata_query: kapture.Kapture, map_plus_query_path: str, map_plus_query_gv_path: str, tar_handlers_map: Optional[TarCollection], tar_handlers_map_gv: Optional[TarCollection], descriptors_type: Optional[str], pairsfile_path: str, output_path_root: str, colmap_binary: str, force: bool): """ Localize query images in a COLMAP model built from topk retrieved images. :param map_plus_query_path: path to the kapture data consisting of mapping and query data (sensors and reconstruction) :param map_plus_query_gv_path: path to the kapture data consisting of mapping and query data after geometric verification (sensors and reconstruction) :param query_path: path to the query kapture data (sensors) :param descriptors_type: type of descriptors, name of the descriptors subfolder :param pairsfile_path: path to the pairsfile that contains the topk retrieved mapping images for each query image :param output_path_root: root path where outputs should be stored :param colmap_binary: path to the COLMAP binary :param force: silently overwrite already existing results """ # load query kapture (we use query kapture to reuse sensor_ids etc.) if kdata_query.trajectories: logger.warning("Query data contains trajectories: they will be ignored") kdata_query.trajectories.clear() else: kdata_query.trajectories = kapture.Trajectories() # clear query trajectories in map_plus_query kdata_map_cleared_trajectories = kapture.Trajectories() query_image_list = set(kdata_query.records_camera.data_list()) for timestamp, subdict in kdata_map.records_camera.items(): for sensor_id, image_name in subdict.items(): if image_name in query_image_list: continue if (timestamp, sensor_id) in kdata_map.trajectories: pose = kdata_map.trajectories.get(timestamp)[sensor_id] kdata_map_cleared_trajectories.setdefault(timestamp, {})[sensor_id] = pose kdata_map.trajectories = kdata_map_cleared_trajectories # load output kapture output_path = os.path.join(output_path_root, 'localized') if os.path.exists(os.path.join(output_path, 'sensors/trajectories.txt')): kdata_output = kapture_from_dir(output_path) if kdata_query.records_camera == kdata_output.records_camera and len( kdata_output.trajectories) != 0 and not force: kdata_query.trajectories = kdata_output.trajectories if kdata_map.rigs is not None: rigs_remove_inplace(kdata_map.trajectories, kdata_map.rigs) if kdata_map_gv.rigs is not None: rigs_remove_inplace(kdata_map_gv.trajectories, kdata_map_gv.rigs) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kdata_sub_colmap_path = os.path.join(output_path_root, 'colmap') kdata_reg_query_path = os.path.join(output_path_root, 'query_registered') sub_kapture_pairsfile_path = os.path.join(output_path_root, 'tmp_pairs.txt') if descriptors_type is None: descriptors_type = try_get_only_key_from_collection(kdata_map.descriptors) assert descriptors_type is not None assert descriptors_type in kdata_map.descriptors keypoints_type = kdata_map.descriptors[descriptors_type].keypoints_type # init matches for kdata_map and kdata_map_gv if kdata_map.matches is None: kdata_map.matches = {} if keypoints_type not in kdata_map.matches: kdata_map.matches[keypoints_type] = kapture.Matches() if kdata_map_gv.matches is None: kdata_map_gv.matches = {} if keypoints_type not in kdata_map_gv.matches: kdata_map_gv.matches[keypoints_type] = kapture.Matches() # run all matching # loop over query images img_skip_list = set() for img_query, img_list_map in pairs.items(): if pose_found(kdata_query, img_query): logger.info(f'{img_query} already processed, skipping...') img_skip_list.add(img_query) continue else: map_pairs = get_pairfile_from_img_list(img_list_map) query_pairs = get_pairfile_img_vs_img_list(img_query, img_list_map) with open(sub_kapture_pairsfile_path, 'w') as fid: logger.info(f'matching for {img_query}') table_to_file(fid, map_pairs) table_to_file(fid, query_pairs) pairs_all = map_pairs + query_pairs pairs_all = [(i, j) for i, j, _ in pairs_all] # match missing pairs # kdata_map.matches is being updated by compute_matches_from_loaded_data compute_matches_from_loaded_data(map_plus_query_path, tar_handlers_map, kdata_map, descriptors_type, pairs_all) # if kdata_map have matches in tar, they need to be switched to read mode matches_handler = retrieve_tar_handler_from_collection(kapture.Matches, keypoints_type, tar_handlers_map) if matches_handler is not None: matches_handler.close() tarfile_path = get_feature_tar_fullpath(kapture.Matches, keypoints_type, map_plus_query_path) tar_handlers_map.matches[keypoints_type] = TarHandler(tarfile_path, 'r') # run all gv # loop over query images for img_query, img_list_map in pairs.items(): if img_query in img_skip_list: continue else: # recompute the pairs map_pairs = get_pairfile_from_img_list(img_list_map) query_pairs = get_pairfile_img_vs_img_list(img_query, img_list_map) with open(sub_kapture_pairsfile_path, 'w') as fid: logger.info(f'geometric verification of {img_query}') table_to_file(fid, map_pairs) table_to_file(fid, query_pairs) pairs_all = map_pairs + query_pairs pairs_all = [(i, j) for i, j, _ in pairs_all] if all(pair in kdata_map_gv.matches[keypoints_type] for pair in pairs_all): continue # create a sub kapture in order to minimize the amount of data exported to colmap # kdata_sub needs to be re-created to add the new matches kdata_sub = sub_kapture_from_img_list(kdata_map, img_list_map + [img_query], pairs_all, keypoints_type, descriptors_type) kdata_sub_gv = sub_kapture_from_img_list(kdata_map_gv, img_list_map + [img_query], pairs_all, keypoints_type, descriptors_type) # run colmap gv on missing pairs run_colmap_gv_from_loaded_data(kdata_sub, kdata_sub_gv, map_plus_query_path, map_plus_query_gv_path, tar_handlers_map, tar_handlers_map_gv, colmap_binary, keypoints_type, [], True) # update kdata_map_gv.matches kdata_map_gv.matches[keypoints_type].update(kdata_sub_gv.matches[keypoints_type]) # if kdata_map_gv have matches in tar, they need to be switched to read mode matches_gv_handler = retrieve_tar_handler_from_collection(kapture.Matches, keypoints_type, tar_handlers_map_gv) if matches_gv_handler is not None: print(matches_gv_handler) matches_gv_handler.close() tarfile_path = get_feature_tar_fullpath(kapture.Matches, keypoints_type, map_plus_query_gv_path) tar_handlers_map_gv.matches[keypoints_type] = TarHandler(tarfile_path, 'r') # loop over query images for img_query, img_list_map in pairs.items(): if img_query in img_skip_list: continue else: map_pairs = get_pairfile_from_img_list(img_list_map) with open(sub_kapture_pairsfile_path, 'w') as fid: logger.info(f'mapping and localization for {img_query}') table_to_file(fid, map_pairs) map_pairs = [(i, j) for i, j, _ in map_pairs] kdata_sub_gv = sub_kapture_from_img_list(kdata_map_gv, img_list_map, map_pairs, keypoints_type, descriptors_type) # sanity check if len(map_pairs) != len(kdata_sub_gv.matches[keypoints_type]): logger.info(f'not all mapping matches available') # build COLMAP map try: colmap_build_map_from_loaded_data( kdata_sub_gv, map_plus_query_gv_path, tar_handlers_map_gv, kdata_sub_colmap_path, colmap_binary, keypoints_type, False, [], ['model_converter'], True) except ValueError: logger.info(f'{img_query} was not localized') continue if not os.path.exists(os.path.join(kdata_sub_colmap_path, 'reconstruction/images.bin')): logger.info(f'colmap mapping for {img_query} did not work, image was not localized') continue query_pairs = get_pairfile_img_vs_img_list(img_query, img_list_map) with open(sub_kapture_pairsfile_path, 'w') as fid: table_to_file(fid, query_pairs) query_pairs = [(i, j) for i, j, _ in query_pairs] query_img_kapture_gv = add_image_to_kapture(kdata_map_gv, kdata_sub_gv, img_query, query_pairs, keypoints_type, descriptors_type) # sanity check if len(query_pairs) != len(query_img_kapture_gv.matches[keypoints_type]): logger.info(f'not all query matches available') # localize in COLMAP map try: colmap_localize_from_loaded_data( query_img_kapture_gv, map_plus_query_gv_path, tar_handlers_map_gv, os.path.join(kdata_sub_colmap_path, 'registered'), os.path.join(kdata_sub_colmap_path, 'colmap.db'), os.path.join(kdata_sub_colmap_path, 'reconstruction'), colmap_binary, keypoints_type, False, ['--Mapper.ba_refine_focal_length', '0', '--Mapper.ba_refine_principal_point', '0', '--Mapper.ba_refine_extra_params', '0', '--Mapper.min_num_matches', '4', '--Mapper.init_min_num_inliers', '4', '--Mapper.abs_pose_min_num_inliers', '4', '--Mapper.abs_pose_min_inlier_ratio', '0.05', '--Mapper.ba_local_max_num_iterations', '50', '--Mapper.abs_pose_max_error', '20', '--Mapper.filter_max_reproj_error', '12'], [], True) except ValueError: logger.info(f'{img_query} was not localized') continue if not os.path.exists(os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'reconstruction/images.txt')): logger.info(f'colmap localization of {img_query} did not work, image was not localized') continue # add to results kapture kdata_reg_query = import_colmap( kdata_reg_query_path, os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'colmap.db'), os.path.join(os.path.join(kdata_sub_colmap_path, 'registered'), 'reconstruction'), None, None, True, True, True, TransferAction.skip) if add_pose_to_query_kapture(kdata_reg_query, kdata_query, img_query): logger.info('successfully localized') # write results (after each image to see the progress) kapture_to_dir(output_path, kdata_query) # clean up (e.g. remove temporal files and folders) safe_remove_any_path(kdata_sub_colmap_path, True) safe_remove_any_path(kdata_reg_query_path, True) safe_remove_file(sub_kapture_pairsfile_path, True) logger.info('all done')