def add_keypoints_to_database(database: COLMAPDatabase,
keypoints: kapture.Keypoints,
kapture_dir_path: str,
colmap_image_ids: dict) -> None:
"""
Add keypoints to the colmap database.
:param database: colmap database.
:param keypoints: kapture keypoints to add
:param kapture_dir_path: kapture data top directory
:param colmap_image_ids: kapture camera identifier -> colmap camera identifier dictionary
"""
keypoints_filepaths = kapture.io.features.keypoints_to_filepaths(
keypoints, kapture_dir_path)
for image_filename, keypoints_filepath in keypoints_filepaths.items():
image_keypoints = image_keypoints_from_file(keypoints_filepath,
keypoints.dtype,
keypoints.dsize)
colmap_image_id = colmap_image_ids[image_filename]
# Make sure keypoints are np.float32 and support by colmap
if image_keypoints.shape[1] not in {2, 4, 6}:
image_keypoints = image_keypoints[:, 0:2]
image_keypoints = image_keypoints.astype(np.float32)
database.add_keypoints(colmap_image_id, image_keypoints)
database.commit()
def test_write_read(self):
nb_keypoints = 5
dim_keypoint = 4
type_keypoint = float
image_keypoints = np.random.random((nb_keypoints, dim_keypoint)).astype(type_keypoint)
binary.image_keypoints_to_file(self._temp_filepath, image_keypoints)
image_keypoints_read = binary.image_keypoints_from_file(self._temp_filepath,
dtype=type_keypoint,
dsize=dim_keypoint)
self.assertEqual(image_keypoints.shape, image_keypoints_read.shape)
self.assertEqual(image_keypoints.dtype, image_keypoints_read.dtype)
self.assertAlmostEqual(image_keypoints.tolist(), image_keypoints_read.tolist())
def export_openmvg_structure(
kapture_points_3d: kapture.Points3d,
kapture_to_openmvg_view_ids: Dict[str, int],
kapture_observations: Optional[kapture.Observations] = None,
kapture_keypoints: Optional[kapture.Keypoints] = None,
kapture_path: Optional[str] = None,
):
# early check
if kapture_points_3d is None:
logger.warning('no 3D points to export.')
return
xyz_coordinates = kapture_points_3d[:, 0:3]
include_2d_observations = kapture_observations is not None
openmvg_structure = []
# this loop can be very long, lets show some progress
hide_progress_bars = logger.getEffectiveLevel() > logging.INFO
for point_idx, coords in enumerate(tqdm(xyz_coordinates, disable=hide_progress_bars)):
point_3d_structure = {
'key': point_idx,
'value': {
'X': coords.tolist(),
'observations': []
}
}
if include_2d_observations and point_idx in kapture_observations:
for kapture_image_name, feature_point_id in kapture_observations[point_idx]:
openmvg_view_id = kapture_to_openmvg_view_ids[kapture_image_name]
point_2d_observation = {'key': openmvg_view_id,
'value': {'id_feat': feature_point_id, }}
if kapture_path and kapture_keypoints is not None:
# if given, load keypoints to populate 2D coordinates of the feature.
keypoints_file_path = get_keypoints_fullpath(kapture_path, kapture_image_name)
try:
keypoints_data = image_keypoints_from_file(keypoints_file_path,
dtype=kapture_keypoints.dtype,
dsize=kapture_keypoints.dsize)
point_2d_observation['value']['x'] = keypoints_data[feature_point_id, 0:2].tolist()
except FileNotFoundError:
logger.warning(f'unable to load keypoints file {keypoints_file_path}')
point_3d_structure['value']['observations'].append(point_2d_observation)
openmvg_structure.append(point_3d_structure)
return openmvg_structure
def add_descriptors_to_database(database: COLMAPDatabase,
descriptors: kapture.Descriptors,
kapture_dir_path: str,
colmap_image_ids: dict) -> None:
"""
Add descriptors to the colmap database.
:param database: colmap database.
:param descriptors: kapture descriptors to add
:param kapture_dir_path: kapture data top directory
:param colmap_image_ids: kapture camera identifier -> colmap camera identifier dictionary
"""
descriptors_filepaths = kapture.io.features.descriptors_to_filepaths(descriptors, kapture_dir_path)
for image_filename, descriptors_filepath in descriptors_filepaths.items():
image_descriptors = image_keypoints_from_file(descriptors_filepath, descriptors.dtype, descriptors.dsize)
colmap_image_id = colmap_image_ids[image_filename]
database.add_descriptors(colmap_image_id, image_descriptors)
database.commit()
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 export_opensfm(
kapture_rootdir: str,
opensfm_rootdir: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.copy) -> None:
"""
:param kapture_rootdir:
:param opensfm_rootdir:
:param force_overwrite_existing:
:param images_import_method:
:return:
"""
disable_tqdm = logger.getEffectiveLevel(
) > logging.INFO # dont display tqdm for non-verbose levels
# load reconstruction
kapture_data = kapture.io.csv.kapture_from_dir(
kapture_dirpath=kapture_rootdir)
# export cameras
opensfm_cameras = {}
kapture_cameras = {
cam_id: cam
for cam_id, cam in kapture_data.sensors.items()
if cam.sensor_type == 'camera'
}
for cam_id, kapture_camera in kapture_cameras.items():
opensfm_cameras[cam_id] = export_opensfm_camera(kapture_camera)
# export shots
opensfm_shots = {}
for timestamp, camera_id, image_filename in tqdm(kapture.flatten(
kapture_data.records_camera),
disable=disable_tqdm):
# retrieve pose (if there is one).
# opensfm_shots = {image_filename: shot}
# shot = {camera , rotation, translation, capture_time, gps_position, ...}
opensfm_shot = {
'capture_time': 0, # in ms != timestamp
'camera': camera_id,
}
if (timestamp, camera_id) in kapture_data.trajectories:
pose = kapture_data.trajectories[timestamp, camera_id]
rotation_vector = quaternion.as_rotation_vector(pose.r)
translation_vector = pose.t.flatten()
opensfm_shot.update({
'rotation': rotation_vector.tolist(),
'translation': translation_vector.tolist()
})
opensfm_shots[image_filename] = opensfm_shot
# pack it
opensfm_reconstruction = {
'cameras': opensfm_cameras,
'shots': opensfm_shots,
}
# images
logger.info(
f'writing image files "{path.join(opensfm_rootdir, "images")}".')
image_filenames = [
f for _, _, f in kapture.flatten(kapture_data.records_camera)
]
kapture_image_filepaths = [
get_record_fullpath(kapture_rootdir, image_filename)
for image_filename in image_filenames
]
opensfm_image_filepaths = [
path.join(opensfm_rootdir, 'images', image_filename)
for image_filename in image_filenames
]
transfer_files_from_dir(
source_filepath_list=kapture_image_filepaths,
destination_filepath_list=opensfm_image_filepaths,
force_overwrite=force_overwrite_existing,
copy_strategy=images_import_method,
)
# export features files (keypoints + descriptors)
opensfm_features_suffix = '.features.npz'
opensfm_features_dirpath = path.join(opensfm_rootdir, 'features')
logger.info(
f'exporting keypoint and descriptors to {opensfm_features_dirpath}')
os.makedirs(opensfm_features_dirpath, exist_ok=True)
for image_filename in tqdm(image_filenames, disable=disable_tqdm):
opensfm_features = {}
# look and load for keypoints in kapture
if kapture_data.keypoints is not None and image_filename in kapture_data.keypoints:
kapture_keypoints_filepath = get_keypoints_fullpath(
kapture_dirpath=kapture_rootdir, image_filename=image_filename)
logger.debug(f'loading {kapture_keypoints_filepath}')
kapture_keypoint = image_keypoints_from_file(
kapture_keypoints_filepath,
dtype=kapture_data.keypoints.dtype,
dsize=kapture_data.keypoints.dsize)
opensfm_features['points'] = kapture_keypoint
# look and load for descriptors in kapture
if kapture_data.descriptors is not None and image_filename in kapture_data.descriptors:
kapture_descriptor_filepath = get_descriptors_fullpath(
kapture_dirpath=kapture_rootdir, image_filename=image_filename)
logger.debug(f'loading {kapture_descriptor_filepath}')
kapture_descriptor = image_descriptors_from_file(
kapture_descriptor_filepath,
dtype=kapture_data.descriptors.dtype,
dsize=kapture_data.descriptors.dsize)
opensfm_features['descriptors'] = kapture_descriptor
# writing opensfm feature file
if len(opensfm_features) > 0:
opensfm_features_filepath = path.join(
opensfm_features_dirpath,
image_filename + opensfm_features_suffix)
logger.debug(f'writing {opensfm_features_filepath}')
os.makedirs(path.dirname(opensfm_features_filepath), exist_ok=True)
np.save(opensfm_features_filepath, opensfm_features)
# export matches files
if kapture_data.matches is not None:
opensfm_matches_suffix = '_matches.pkl.gz'
opensfm_matches_dirpath = path.join(opensfm_rootdir, 'matches')
os.makedirs(opensfm_matches_dirpath, exist_ok=True)
logger.info(f'exporting matches to {opensfm_matches_dirpath}')
opensfm_pairs = {}
for image_filename1, image_filename2 in kapture_data.matches:
opensfm_pairs.setdefault(image_filename1,
[]).append(image_filename2)
for image_filename1 in tqdm(image_filenames, disable=disable_tqdm):
opensfm_matches = {}
opensfm_matches_filepath = path.join(
opensfm_matches_dirpath,
image_filename1 + opensfm_matches_suffix)
logger.debug(f'loading matches for {image_filename1}')
for image_filename2 in opensfm_pairs.get(image_filename1, []):
# print(image_filename1, image_filename2)
kapture_matches_filepath = get_matches_fullpath(
(image_filename1, image_filename2),
kapture_dirpath=kapture_rootdir)
kapture_matches = image_matches_from_file(
kapture_matches_filepath)
opensfm_matches[image_filename2] = kapture_matches[:,
0:2].astype(
np.int)
os.makedirs(path.dirname(opensfm_matches_filepath), exist_ok=True)
with gzip.open(opensfm_matches_filepath, 'wb') as f:
pickle.dump(opensfm_matches, f)
# export 3D-points files
if kapture_data.points3d is not None:
logger.info('exporting points 3-D')
opensfm_reconstruction['points'] = {}
for i, (x, y, z, r, g, b) in tqdm(enumerate(kapture_data.points3d),
disable=disable_tqdm):
opensfm_reconstruction['points'][i] = {
'coordinates': [x, y, z],
'color': [r, g, b]
}
# write json files #################################################################################################
os.makedirs(opensfm_rootdir, exist_ok=True)
# write reconstruction.json
opensfm_reconstruction_filepath = path.join(opensfm_rootdir,
'reconstruction.json')
logger.info(
f'writing reconstruction file "{opensfm_reconstruction_filepath}".')
with open(opensfm_reconstruction_filepath, 'wt') as f:
json.dump([opensfm_reconstruction], f, indent=4)
# write camera_models.json
opensfm_cameras_filepath = path.join(opensfm_rootdir, 'camera_models.json')
logger.info(f'writing camera models file "{opensfm_cameras_filepath}".')
with open(opensfm_cameras_filepath, 'wt') as f:
json.dump(opensfm_cameras, f, indent=4)
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 get_ba_params(path, ff, lf, results, kapt, sensor_id):
frames = [(id, fname[sensor_id])
for id, fname in kapt.records_camera.items()
if id >= ff and (lf is None or id < lf)]
fname2id = {fname: id for id, fname in frames}
poses = np.array([[
*tools.q_to_angleaxis(kapt.trajectories[id][sensor_id].r, True),
*kapt.trajectories[id][sensor_id].t
] for id, fname in frames]).astype(float)
pts3d = kapt.points3d[:, :3]
if SET_3D_POINT_ALT:
pts3d[:, 1] = -TAKEOFF_LAWN_ALT
feat = kapt.keypoints[FEATURE_NAME]
uv_map = {}
for id_f, fname in frames:
uvs = image_keypoints_from_file(
os.path.join(path, 'kapture', 'reconstruction', 'keypoints',
FEATURE_NAME, fname + '.kpt'), feat.dtype, feat.dsize)
uv_map[id_f] = uvs
f_uv = {}
for id3, r in kapt.observations.items():
for fname, id2 in r[FEATURE_NAME]:
if fname in fname2id:
id_f = fname2id[fname]
f_uv.setdefault(id_f, {})[id3] = uv_map[id_f][id2, :]
# f_uv = {id_f: {id3: uv_map[id_f][id2, :]
# for id3 in range(len(pts3d))
# for id2 in range(len(uv_map[id_f]))
# if (fname, id2) in kapt.observations.get(id3, {}).get(VO_FEATURE_NAME, {})}
# for id_f, fname in frames}
obs_kp = list(set.union(*[set(m.keys()) for m in f_uv.values()]))
cam_idxs, pt3d_idxs, pts2d = list(
map(
np.array,
zip(*[(i, id3, uv.flatten())
for i, (id_f, kps_uv) in enumerate(f_uv.items())
for id3, uv in kps_uv.items()])))
meas_idxs = np.array([
i for i, r in enumerate(results)
if r['meas'] is not None and i < len(poses)
],
dtype=int)
meas_q = {i: results[i]['pose'].prior.quat.conj() for i in meas_idxs}
meas_r = np.array([
tools.q_times_v(meas_q[i], -results[i]['pose'].prior.loc)
for i in meas_idxs
],
dtype=np.float32)
meas_aa = np.array(
[tools.q_to_angleaxis(meas_q[i], compact=True) for i in meas_idxs],
dtype=np.float32)
return poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp
def test_maupertuis_import_db_only(self):
kapture_data = import_colmap_database(self._database_filepath, self._temp_dirpath,
no_geometric_filtering=True)
# check the numbers
self.assertIsNone(kapture_data.trajectories)
self.assertIsNone(kapture_data.points3d)
self.assertIsNone(kapture_data.records_lidar)
self.assertIsNone(kapture_data.records_wifi)
self.assertIsNone(kapture_data.records_gnss)
self.assertEqual(1, len(kapture_data.sensors))
self.assertEqual(4, len(kapture_data.records_camera))
self.assertEqual(4, len(kapture_data.keypoints))
self.assertEqual(4, len(kapture_data.descriptors))
self.assertEqual(6, len(kapture_data.matches))
# check camera
camera = kapture_data.sensors['cam_00001']
self.assertEqual(kapture.SENSOR_TYPE_CAMERA, camera.sensor_type)
self.assertEqual(kapture.CameraType.SIMPLE_PINHOLE, camera.camera_type)
self.assertAlmostEqual(camera.camera_params, [1919.0, 1079.0, 2302.7999999999997, 959.5, 539.5])
# check snapshots
snapshots = kapture_data.records_camera
self.assertTrue(all('cam_00001' in ts for ts in snapshots.values()))
self.assertEqual(['00.jpg', '01.jpg', '02.jpg', '03.jpg'],
[filename for _, _, filename in kapture.flatten(snapshots, True)])
# check keypoints
keypoints = kapture_data.keypoints
self.assertEqual(np.float32, keypoints.dtype)
self.assertEqual(6, keypoints.dsize)
self.assertEqual({'00.jpg', '01.jpg', '02.jpg', '03.jpg'}, keypoints)
keypoints_filepaths_actual = kapture.io.features.keypoints_to_filepaths(keypoints, self._temp_dirpath)
keypoints_filepaths_expected = {
f'{i:02d}.jpg': path_secure(f'{self._temp_dirpath}/reconstruction/keypoints/{i:02d}.jpg.kpt')
for i in [0, 1, 2, 3]}
self.assertDictEqual(keypoints_filepaths_actual, keypoints_filepaths_expected)
# check a keypoints file
image_keypoints_filepaths = sorted(
kapture.io.features.keypoints_to_filepaths(keypoints, self._temp_dirpath).values())
image_keypoints = image_keypoints_from_file(image_keypoints_filepaths[0], keypoints.dtype, keypoints.dsize)
self.assertEqual((6424, 6), image_keypoints.shape)
self.assertAlmostEqual([1290.908447265625, 4.156360626220703, -1.3475048542022705,
1.4732409715652466, -1.4732409715652466, -1.3475048542022705],
image_keypoints[0].tolist())
self.assertAlmostEqual([1381.316650390625, 668.8056640625, 59.981021881103516,
46.423213958740234, -46.423213958740234, 59.981021881103516],
image_keypoints[-1].tolist())
# check descriptors
descriptors = kapture_data.descriptors
self.assertEqual(np.uint8, descriptors.dtype)
self.assertEqual(128, descriptors.dsize)
self.assertEqual({'00.jpg', '01.jpg', '02.jpg', '03.jpg'}, descriptors)
descriptors_filepaths_actual = kapture.io.features.descriptors_to_filepaths(descriptors, self._temp_dirpath)
descriptors_filepaths_expected = {
f'{i:02d}.jpg': path_secure(f'{self._temp_dirpath}/reconstruction/descriptors/{i:02d}.jpg.desc')
for i in [0, 1, 2, 3]}
self.assertDictEqual(descriptors_filepaths_actual, descriptors_filepaths_expected)
# check a descriptors file
image_descriptors_filepaths = sorted(kapture.io.features.descriptors_to_filepaths(
descriptors, self._temp_dirpath).values())
image_descriptors = image_descriptors_from_file(
image_descriptors_filepaths[0], descriptors.dtype, descriptors.dsize)
self.assertEqual(image_keypoints.shape[0], image_descriptors.shape[0])
# check matches
matches = kapture_data.matches
self.assertEqual({('01.jpg', '03.jpg'), ('00.jpg', '02.jpg'),
('00.jpg', '03.jpg'), ('02.jpg', '03.jpg'),
('00.jpg', '01.jpg'), ('01.jpg', '02.jpg')},
set(matches))
def load_keypoints(keypoints_type, input_path, image_name, dtype, dsize,
tar_handlers):
keypoints_path = get_keypoints_fullpath(keypoints_type, input_path,
image_name, tar_handlers)
return image_keypoints_from_file(keypoints_path, dtype, dsize)
def _export_opensfm_features_and_matches(image_filenames, kapture_data,
kapture_root_dir, opensfm_root_dir,
disable_tqdm):
"""
export features files (keypoints + descriptors) and matches
"""
opensfm_features_suffix = '.features.npz'
opensfm_features_dir_path = path.join(opensfm_root_dir, 'features')
logger.info(
f'exporting keypoint and descriptors to {opensfm_features_dir_path}')
os.makedirs(opensfm_features_dir_path, exist_ok=True)
for image_filename in tqdm(image_filenames, disable=disable_tqdm):
opensfm_features = {}
# look and load for keypoints in kapture
if kapture_data.keypoints is not None and image_filename in kapture_data.keypoints:
kapture_keypoints_filepath = get_keypoints_fullpath(
kapture_dirpath=kapture_root_dir,
image_filename=image_filename)
logger.debug(f'loading {kapture_keypoints_filepath}')
kapture_keypoint = image_keypoints_from_file(
kapture_keypoints_filepath,
dtype=kapture_data.keypoints.dtype,
dsize=kapture_data.keypoints.dsize)
opensfm_features['points'] = kapture_keypoint
# look and load for descriptors in kapture
if kapture_data.descriptors is not None and image_filename in kapture_data.descriptors:
kapture_descriptor_filepath = get_descriptors_fullpath(
kapture_dirpath=kapture_root_dir,
image_filename=image_filename)
logger.debug(f'loading {kapture_descriptor_filepath}')
kapture_descriptor = image_descriptors_from_file(
kapture_descriptor_filepath,
dtype=kapture_data.descriptors.dtype,
dsize=kapture_data.descriptors.dsize)
opensfm_features['descriptors'] = kapture_descriptor
# writing opensfm feature file
if len(opensfm_features) > 0:
opensfm_features_filepath = path.join(
opensfm_features_dir_path,
image_filename + opensfm_features_suffix)
logger.debug(f'writing {opensfm_features_filepath}')
os.makedirs(path.dirname(opensfm_features_filepath), exist_ok=True)
np.save(opensfm_features_filepath, opensfm_features)
# export matches files
if kapture_data.matches is not None:
opensfm_matches_suffix = '_matches.pkl.gz'
opensfm_matches_dir_path = path.join(opensfm_root_dir, 'matches')
os.makedirs(opensfm_matches_dir_path, exist_ok=True)
logger.info(f'exporting matches to {opensfm_matches_dir_path}')
opensfm_pairs = {}
for image_filename1, image_filename2 in kapture_data.matches:
opensfm_pairs.setdefault(image_filename1,
[]).append(image_filename2)
for image_filename1 in tqdm(image_filenames, disable=disable_tqdm):
opensfm_matches = {}
opensfm_matches_filepath = path.join(
opensfm_matches_dir_path,
image_filename1 + opensfm_matches_suffix)
logger.debug(f'loading matches for {image_filename1}')
for image_filename2 in opensfm_pairs.get(image_filename1, []):
# print(image_filename1, image_filename2)
kapture_matches_filepath = get_matches_fullpath(
(image_filename1, image_filename2),
kapture_dirpath=kapture_root_dir)
kapture_matches = image_matches_from_file(
kapture_matches_filepath)
opensfm_matches[image_filename2] = kapture_matches[:,
0:2].astype(
np.int)
os.makedirs(path.dirname(opensfm_matches_filepath), exist_ok=True)
with gzip.open(opensfm_matches_filepath, 'wb') as f:
pickle.dump(opensfm_matches, f)
def export_openmvg_regions(kapture_path: str,
kapture_keypoints: Optional[kapture.Keypoints],
kapture_descriptors: kapture.Descriptors,
openmvg_regions_dir_path: str,
image_path_flatten: bool):
"""
exports openMVG regions ie keypoints and descriptors.
:param kapture_path: input path to root kapture directory.
:param kapture_keypoints: input kapture keypoints. Could be None if no keypoints.
:param kapture_descriptors: input kapture descriptors. Could be None if no descriptors.
:param openmvg_regions_dir_path: input path to output openMVG regions directory.
:param image_path_flatten: if true, it means that image path are to be flatten.
:return:
"""
# early check we should do
if kapture_keypoints is None or kapture_descriptors is None:
logger.warning('no keypoints or descriptors to export.')
return
# make sure output directory is ready
os.makedirs(openmvg_regions_dir_path, exist_ok=True)
# only able to export SIFT
if any([
f.type_name.upper() != 'SIFT'
for f in [kapture_keypoints, kapture_descriptors]
]):
raise ValueError(
f'unable to export other regions than sift '
f'(got {kapture_keypoints.type_name}/{kapture_descriptors.type_name})'
)
os.makedirs(openmvg_regions_dir_path, exist_ok=True)
polymorphic_registry = CerealPointerRegistry(
id_key=JSON_KEY.POLYMORPHIC_ID, value_key=JSON_KEY.POLYMORPHIC_NAME)
# create image_describer.json
fake_regions_type = {
"ptr_wrapper": {
"valid": 1,
"data": {
"value0": [],
"value1": []
}
}
}
fake_regions_type.update(polymorphic_registry.get_ids_dict('SIFT_Regions'))
image_describer = {'regions_type': fake_regions_type}
image_describer_file_path = path.join(openmvg_regions_dir_path,
'image_describer.json')
with open(image_describer_file_path, 'w') as fid:
json.dump(image_describer, fid, indent=4)
# this loop can be very long, lets show some progress
hide_progress_bars = logger.getEffectiveLevel() > logging.INFO
# copy keypoints files
keypoints = keypoints_to_filepaths(kapture_keypoints, kapture_path)
for kapture_image_name, kapture_keypoint_file_path in tqdm(
keypoints.items(), disable=hide_progress_bars):
openmvg_keypoint_file_name = get_openmvg_image_path(
kapture_image_name, image_path_flatten)
openmvg_keypoint_file_name = path.splitext(
path.basename(openmvg_keypoint_file_name))[0] + '.feat'
openmvg_keypoint_file_path = path.join(openmvg_regions_dir_path,
openmvg_keypoint_file_name)
keypoints_data = image_keypoints_from_file(kapture_keypoint_file_path,
kapture_keypoints.dtype,
kapture_keypoints.dsize)
keypoints_data = keypoints_data[:, 0:4]
np.savetxt(openmvg_keypoint_file_path, keypoints_data, fmt='%10.5f')
# copy descriptors files
"""
from openMVG regions_factory.hpp
using SIFT_Regions = Scalar_Regions<SIOPointFeature, unsigned char, 128>;
using AKAZE_Float_Regions = Scalar_Regions<SIOPointFeature, float, 64>;
using AKAZE_Liop_Regions = Scalar_Regions<SIOPointFeature, unsigned char, 144>;
using AKAZE_Binary_Regions = Binary_Regions<SIOPointFeature, 64>;
"""
descriptors = descriptors_to_filepaths(kapture_descriptors, kapture_path)
for kapture_image_name, kapture_descriptors_file_path in tqdm(
descriptors.items(), disable=hide_progress_bars):
openmvg_descriptors_file_name = get_openmvg_image_path(
kapture_image_name, image_path_flatten)
openmvg_descriptors_file_name = path.splitext(
path.basename(openmvg_descriptors_file_name))[0] + '.desc'
openmvg_descriptors_file_path = path.join(
openmvg_regions_dir_path, openmvg_descriptors_file_name)
kapture_descriptors_data = image_descriptors_from_file(
kapture_descriptors_file_path, kapture_descriptors.dtype,
kapture_descriptors.dsize)
# assign a byte array of [size_t[1] + uint8[nb features x 128]
size_t_len = 64 // 8
openmvg_descriptors_data = np.empty(
dtype=np.uint8,
shape=(kapture_descriptors_data.size + size_t_len, ))
openmvg_descriptors_data[0:size_t_len].view(
dtype=np.uint64)[0] = kapture_descriptors_data.shape[0]
openmvg_descriptors_data[
size_t_len:] = kapture_descriptors_data.flatten()
array_to_file(openmvg_descriptors_file_path, openmvg_descriptors_data)
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 load_from_kapture(self, kapture_data, minimal_observation_count=10):
image_list = [
filename
for _, _, filename in kapture.flatten(kapture_data.records_camera)
]
descriptors = []
keypoints = []
points3d = []
mask = []
image_indexes = {}
image_index_list = []
keypoint_index_list = []
self.keypoint_count = 0
for i, image_path in enumerate(image_list):
descriptors_full_path = get_descriptors_fullpath(
self._descriptor_name, kapture_data.kapture_path, image_path)
kapture_descriptors = kapture_data.descriptors[
self._descriptor_name]
descriptors.append(
image_descriptors_from_file(descriptors_full_path,
kapture_descriptors.dtype,
kapture_descriptors.dsize))
keypoints_full_path = get_keypoints_fullpath(
self._descriptor_name, kapture_data.kapture_path, image_path)
kapture_keypoints = kapture_data.keypoints[self._descriptor_name]
keypoints.append(
image_keypoints_from_file(keypoints_full_path,
kapture_keypoints.dtype,
kapture_keypoints.dsize))
point_count = len(keypoints[i])
points3d.append(np.zeros((point_count, 3), dtype=np.float32))
mask.append(np.zeros(point_count, dtype=np.bool))
image_indexes[image_path] = i
image_index_list.append(np.array([i] * point_count))
keypoint_index_list.append(np.arange(point_count))
self.keypoint_count += point_count
for point_index, observation in kapture_data.observations.items():
if len(observation[
self._descriptor_name]) > minimal_observation_count:
for observation_image_name, image_keypoints_index in observation[
self._descriptor_name]:
image_index = image_indexes[observation_image_name]
mask[image_index][image_keypoints_index] = True
points3d[image_index][
image_keypoints_index] = kapture_data.points3d[
point_index][:3]
for i in range(len(mask)):
self.image_index_list.extend(list(image_index_list[i]))
self.keypoint_index_list.extend(list(keypoint_index_list[i]))
self.masked_image_index_list.extend(
list(image_index_list[i][mask[i]]))
self.masked_keypoint_index_list.extend(
list(keypoint_index_list[i][mask[i]]))
self.descriptors = descriptors
self.keypoints = keypoints
self.points3d = points3d
self.mask = mask
self.image_index_from_image_name = image_indexes
self.load_trajectory(kapture_data)