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')
