def convert_results_to_kapture(query_path: str, results: str, outpath: str):
"""
convert file with name qw qx qy qz tx ty tz to kapture
"""
skip_heavy_useless = [
kapture.Trajectories, kapture.RecordsLidar, kapture.RecordsWifi,
kapture.Keypoints, kapture.Descriptors, kapture.GlobalFeatures,
kapture.Matches, kapture.Points3d, kapture.Observations
]
kapture_query = kapture_from_dir(query_path, skip_list=skip_heavy_useless)
inverse_records_camera = {
image_name: (timestamp, sensor_id)
for timestamp, sensor_id, image_name in kapture.flatten(
kapture_query.records_camera)
}
trajectories = kapture.Trajectories()
with open(results) as fid:
lines = fid.readlines()
lines = [line.rstrip().split() for line in lines if line != '\n']
for line in lines:
image_name = line[0]
rotation = quaternion.quaternion(float(line[1]), float(line[2]),
float(line[3]), float(line[4]))
translation = [float(line[5]), float(line[6]), float(line[7])]
timestamp, sensor_id = inverse_records_camera[image_name]
trajectories[timestamp,
sensor_id] = kapture.PoseTransform(rotation, translation)
kapture_query.trajectories = trajectories
kapture_to_dir(outpath, kapture_query)
def test_kapture_format_version_from_disk(self):
kapture_data = kapture.Kapture()
kapture_data.sensors = kapture.Sensors()
csv.kapture_to_dir(self._tempdir.name, kapture_data)
version = csv.kapture_format_version(self._tempdir.name)
self.assertEqual(csv.current_format_version(), version,
"We have the current version")
def test_kapture_write(self):
kdata = kapture.Kapture()
# test it is not writing files for undefined parts
csv.kapture_to_dir(self._tempdir.name, kdata)
self.assertFalse(
path.exists(path.join(self._tempdir.name, 'sensors',
'sensors.txt')))
self.assertFalse(
path.exists(
path.join(self._tempdir.name, 'sensors', 'trajectories.txt')))
self.assertFalse(
path.exists(path.join(self._tempdir.name, 'sensors', 'rigs.txt')))
# test it is actually writing files for parts
kdata.sensors = kapture.Sensors()
kdata.trajectories = kapture.Trajectories()
kdata.rigs = kapture.Rigs()
csv.kapture_to_dir(self._tempdir.name, kdata)
self.assertTrue(
path.exists(path.join(self._tempdir.name, 'sensors',
'sensors.txt')))
self.assertTrue(
path.exists(
path.join(self._tempdir.name, 'sensors', 'trajectories.txt')))
self.assertTrue(
path.exists(path.join(self._tempdir.name, 'sensors', 'rigs.txt')))
def test_kapture_write_read(self):
kdata_expected = kapture.Kapture()
kdata_expected.sensors = kapture.Sensors()
kdata_expected.trajectories = kapture.Trajectories()
kdata_expected.rigs = kapture.Rigs()
csv.kapture_to_dir(self._tempdir.name, kdata_expected)
kdata_actual = csv.kapture_from_dir(self._tempdir.name)
def import_openmvg(input_path: str,
kapture_path: str,
image_action: TransferAction,
force_overwrite_existing: bool = False) -> None:
"""
Converts an openMVG JSON file to a kapture directory.
If an image action is provided (link, copy or move), links to the image files are created,
or the image files are copied or moved.
:param input_path: path to the openMVG file (or its directory)
:param kapture_path: path to the kapture directory where the data will be exported
:param image_action: action to apply to the images
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
"""
os.makedirs(kapture_path, exist_ok=True)
kapture.io.structure.delete_existing_kapture_files(
kapture_path, force_overwrite_existing)
if path.isdir(input_path):
input_file = path.join(input_path, DEFAULT_JSON_FILE_NAME)
else:
input_file = input_path
if not path.isfile(input_file):
raise ValueError(f'OpenMVG JSON file {input_file} does not exist')
logger.info(f'Loading openmvg file {input_file}')
with open(input_file, 'r') as f:
input_json = json.load(f)
kapture_data = openmvg_to_kapture(input_json, kapture_path,
image_action)
logger.info(f'Saving to kapture {kapture_path}')
kcsv.kapture_to_dir(kapture_path, kapture_data)
def merge_kaptures(kapture_path_list: List[str],
merged_path: str,
keep_sensor_ids: bool,
images_import_strategy: TransferAction = TransferAction.skip,
skip: List[str] = [],
force: bool = False) -> None:
"""
Merge a list of kapture dataset to a new one.
:param kapture_path_list: list of path to the top directory of the kapture datasets to merge
:param merged_path: path to the merged top directory kapture to create
:param keep_sensor_ids: if True, will keep the original sensor identifiers. Otherwise, might rename them.
:param skip: list of kapture data type names to optionally skip (trajectories, records_camera, descriptors, ...)
:param force: If True, silently overwrite kapture files if already exists.
"""
os.makedirs(merged_path, exist_ok=True)
delete_existing_kapture_files(merged_path, force_erase=force)
skip_list = []
if 'trajectories' in skip:
skip_list.append(kapture.Trajectories)
if 'records_camera' in skip:
skip_list.append(kapture.RecordsCamera)
if 'records_lidar' in skip:
skip_list.append(kapture.RecordsLidar)
if 'records_wifi' in skip:
skip_list.append(kapture.RecordsWifi)
if 'records_gnss' in skip:
skip_list.append(kapture.RecordsGnss)
if 'keypoints' in skip:
skip_list.append(kapture.Keypoints)
if 'descriptors' in skip:
skip_list.append(kapture.Descriptors)
if 'global_features' in skip:
skip_list.append(kapture.GlobalFeatures)
if 'matches' in skip:
skip_list.append(kapture.Matches)
if 'points3d' in skip:
skip_list.append(kapture.Points3d)
if 'observations' in skip:
skip_list.append(kapture.Observations)
kapture_data_list = []
for kapture_path in kapture_path_list:
logger.info(f'Loading {kapture_path}')
kapture_data = kapture_from_dir(kapture_path)
kapture_data_list.append(kapture_data)
if keep_sensor_ids:
merged_kapture = merge_keep_ids(kapture_data_list, skip_list, kapture_path_list,
merged_path, images_import_strategy)
else:
merged_kapture = merge_remap(kapture_data_list, skip_list, kapture_path_list,
merged_path, images_import_strategy)
logger.info('Writing merged kapture data...')
kapture_to_dir(merged_path, merged_kapture)
def import_image_folder(
images_path: str,
kapture_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Imports the images of a folder to a kapture. This creates only images and cameras.
:param images_path: path to directory containing the images.
:param kapture_path: path to kapture root directory.
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
os.makedirs(kapture_path, exist_ok=True)
delete_existing_kapture_files(kapture_path,
force_erase=force_overwrite_existing)
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
file_list = [
os.path.relpath(os.path.join(dirpath, filename), images_path)
for dirpath, dirs, filenames in os.walk(images_path)
for filename in filenames
]
file_list = sorted(file_list)
logger.info('starting conversion...')
for n, filename in enumerate(file_list):
# test if file is a valid image
try:
# lazy load
with Image.open(path.join(images_path, filename)) as im:
width, height = im.size
model_params = [width, height]
except (OSError, PIL.UnidentifiedImageError):
# It is not a valid image: skip it
logger.info(f'Skipping invalid image file {filename}')
continue
camera_id = f'sensor{n}'
images[(n, camera_id)] = path_secure(filename) # don't forget windows
cameras[camera_id] = kapture.Camera(kapture.CameraType.UNKNOWN_CAMERA,
model_params)
# import (copy) image files.
logger.info('import image files ...')
filename_list = [f for _, _, f in kapture.flatten(images)]
import_record_data_from_dir_auto(images_path, kapture_path, filename_list,
images_import_method)
# pack into kapture format
imported_kapture = kapture.Kapture(sensors=cameras, records_camera=images)
logger.info('writing imported data...')
kapture_to_dir(kapture_path, imported_kapture)
def save_to_kapture(self, trajectory_rig_id: Optional[str] = None) -> None:
"""
Save the data in kapture format.
:param trajectory_rig_id: the rig identifier of the trajectory points
"""
# Convert pose info to trajectories
if len(self.poses_info) > 0 and trajectory_rig_id is None:
raise ValueError("Must provide rig identifier for trajectory")
trajectories = kapture.Trajectories() if len(
self.poses_info) > 0 else None
for pose_info in self.poses_info:
t = pose_info.timestamp.to_nsec()
ros_translation = pose_info.pose6d.position
translation = [
ros_translation.x, ros_translation.y, ros_translation.z
]
ros_rotation = pose_info.pose6d.orientation
rotation = np.quaternion(ros_rotation.w, ros_rotation.x,
ros_rotation.y, ros_rotation.z)
# Transform the pose from the ROS body coordinate system defined here
# https://www.ros.org/reps/rep-0103.html#axis-orientation
# to the Kapture coordinate system
# ros pose seems to be the inverse of the extrinsic matrix
# i.e world position and rig orientation with respect to the world axis
pose6d = kapture.PoseTransform.compose([
pose_kapture_from_ros,
kapture.PoseTransform(rotation, translation).inverse(),
pose_ros_from_kapture
])
trajectories[(t, trajectory_rig_id)] = pose6d
self.logger.info(f'Saving {len(list(flatten(trajectories)))} poses')
# Convert image info to kapture image
records_camera = kapture.RecordsCamera()
for image_info in self.images_info:
t = image_info.timestamp.to_nsec()
records_camera[(t, image_info.camera_name)] = image_info.filename
self.logger.info(
f'Saving {len(list(flatten(records_camera)))} camera records')
kapture_data = kapture.Kapture(rigs=self._rigs,
sensors=self._sensors,
records_camera=records_camera,
trajectories=trajectories)
self.logger.info(f'Saving to kapture {self._kapture_path}')
kcsv.kapture_to_dir(self._kapture_path, kapture_data)
self.logger.info('Done')
def import_openmvg(sfm_data_path: str,
regions_dir_path: str,
matches_file_path: str,
kapture_path: str,
image_action: TransferAction,
force_overwrite_existing: bool = False) -> None:
"""
Converts an openMVG JSON file to a kapture directory.
If an image action is provided (link, copy or move), links to the image files are created,
or the image files are copied or moved.
:param sfm_data_path: path to the openMVG sfm_data file.
:param regions_dir_path: input path to directory containing regions (*.feat, *.desc)
:param matches_file_path: input path to openMVG matches file (eg. matches.f.bin)
:param kapture_path: path to the kapture directory where the data will be exported
:param image_action: action to apply to the images
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
"""
# sanity check
if not path.isfile(sfm_data_path):
raise ValueError(f'OpenMVG JSON file {sfm_data_path} does not exist.')
os.makedirs(kapture_path, exist_ok=True)
kapture.io.structure.delete_existing_kapture_files(
kapture_path, force_overwrite_existing)
logger.info(f'Loading sfm_data file {sfm_data_path}')
with open(sfm_data_path, 'r') as f:
input_json = json.load(f)
kapture_data = import_openmvg_sfm_data_json(input_json, kapture_path,
image_action)
if regions_dir_path:
logger.info(f'Loading regions from {regions_dir_path}')
import_openmvg_regions(regions_dir_path, kapture_data, kapture_path)
if matches_file_path:
logger.info(f'Loading matches from {matches_file_path}')
import_openmvg_matches(matches_file_path, kapture_data, kapture_path)
logger.info(f'Saving to kapture {kapture_path}')
kcsv.kapture_to_dir(kapture_path, kapture_data)
def _import_colmap_overcast_reference(robotcar_path, kapture_path,
force_overwrite_existing):
# Convert Colmap reference DB to kapture
kapture_train_dir = path.join(kapture_path, "mapping")
colmap_db_path = path.join(robotcar_path,
"3D-models/overcast-reference.db")
if path.exists(colmap_db_path):
delete_existing_kapture_files(kapture_train_dir,
force_overwrite_existing)
kapture_train_data = import_colmap(
kapture_dir_path=kapture_train_dir,
colmap_database_filepath=colmap_db_path,
colmap_reconstruction_dir_path='',
colmap_images_dir_path=path.join(robotcar_path, "images"),
no_geometric_filtering=True,
force_overwrite_existing=force_overwrite_existing,
images_import_strategy=TransferAction.skip)
logger.info(f'saving feature DB to kapture {kapture_train_dir} ...')
kapture_to_dir(kapture_train_dir, kapture_train_data)
else:
logger.warning(
f'Colmap feature DB {colmap_db_path} does not exist... skipping.')
def pose_approximation_from_pairsfile(input_path: str, pairsfile_path: str,
output_path: str,
query_path: Optional[str],
topk: Optional[int], method: str,
additional_parameters: dict,
force: bool):
"""
localize from pairsfile
"""
os.makedirs(output_path, exist_ok=True)
delete_existing_kapture_files(output_path, force_erase=force)
logger.info(f'pose_approximation. loading mapping: {input_path}')
kdata = kapture_from_dir(input_path,
None,
skip_list=[
kapture.Keypoints, kapture.Descriptors,
kapture.GlobalFeatures, kapture.Matches,
kapture.Points3d, kapture.Observations
])
if query_path is not None:
logger.info(f'pose_approximation. loading query: {query_path}')
kdata_query = kapture_from_dir(query_path,
skip_list=[
kapture.Keypoints,
kapture.Descriptors,
kapture.GlobalFeatures,
kapture.Matches, kapture.Points3d,
kapture.Observations
])
else:
kdata_query = kdata
logger.info(f'pose_approximation. loading pairs: {pairsfile_path}')
similarity_dict = get_ordered_pairs_from_file(pairsfile_path,
kdata_query.records_camera,
kdata.records_camera, topk)
query_images = set(similarity_dict.keys())
kdata_result = kapture.Kapture(sensors=kapture.Sensors(),
records_camera=kapture.RecordsCamera(),
trajectories=kapture.Trajectories())
for timestamp, cam_id, image_name in kapture.flatten(
kdata_query.records_camera):
if image_name not in query_images:
continue
if cam_id not in kdata_result.sensors:
kdata_result.sensors[cam_id] = kdata_query.sensors[cam_id]
kdata_result.records_camera[(timestamp, cam_id)] = image_name
if kdata.rigs is None:
map_trajectories = kdata.trajectories
else:
map_trajectories = kapture.rigs_remove(kdata.trajectories, kdata.rigs)
training_trajectories_reversed = {
image_name: map_trajectories[(timestamp, cam_id)]
for timestamp, cam_id, image_name in kapture.flatten(
kdata.records_camera) if (timestamp, cam_id) in map_trajectories
}
records_camera_reversed = {
image_name: (timestamp, cam_id)
for timestamp, cam_id, image_name in kapture.flatten(
kdata_result.records_camera)
}
for image_name, similar_images in similarity_dict.items():
pose_inv_list = [
training_trajectories_reversed[k].inverse()
for k, _ in similar_images
]
timestamp = records_camera_reversed[image_name][0]
cam_id = records_camera_reversed[image_name][1]
if method == 'equal_weighted_barycenter':
weight_list = [
1.0 / len(pose_inv_list) for _ in range(len(pose_inv_list))
]
else:
assert 'alpha' in additional_parameters
alpha = additional_parameters['alpha']
weights = np.zeros((len(pose_inv_list), ))
for i, (_, score) in enumerate(similar_images):
weights[i] = score
weights[:] = weights[:]**(alpha)
weights[:] = weights[:] / np.sum(weights[:])
weight_list = weights.tolist()
final_pose = average_pose_transform_weighted(pose_inv_list,
weight_list).inverse()
kdata_result.trajectories[(timestamp, cam_id)] = final_pose
kapture_to_dir(output_path, kdata_result)
logger.info('all done')
def import_extended_cmu_seasons(
cmu_path: str,
top_kaptures_path: str,
slice_range: List[int],
import_all_files: bool = False,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Import extended CMU data to kapture. Will make training and query kaptures for every CMU slice.
:param cmu_path: path to the top directory of the CMU dataset files
:param top_kaptures_path: top directory for the kaptures to create
:param slice_range: range of CMU slices to import
:param import_all_files: if Tre, will import all files
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
os.makedirs(top_kaptures_path, exist_ok=True)
cameras = import_extended_cmu_seasons_intrinsics(
path.join(cmu_path, 'intrinsics.txt'))
for slice_n in slice_range:
# prepare paths
slice_path = os.path.join(cmu_path, f'slice{slice_n}')
training_images_path = os.path.join(slice_path, 'database')
query_images_path = os.path.join(slice_path, 'query')
gt_trajectories_path = os.path.join(
slice_path, f'ground-truth-database-images-slice{slice_n}.txt')
query_image_list = os.path.join(slice_path,
f'test-images-slice{slice_n}.txt')
query_gt_path = os.path.join(slice_path, 'camera-poses')
query_gt_list = [
os.path.join(query_gt_path, f) for f in os.listdir(query_gt_path)
]
# Import training images
kapture_training_path = path.join(top_kaptures_path, f'slice{slice_n}',
"mapping")
delete_existing_kapture_files(kapture_training_path,
force_overwrite_existing)
training_records_camera, training_trajectories = import_extended_cmu_seasons_images(
gt_trajectories_path)
training_kapture = kapture.Kapture(
sensors=cameras,
records_camera=training_records_camera,
trajectories=training_trajectories)
if import_all_files:
_add_images_from_folder(training_images_path, training_kapture)
kapture_to_dir(kapture_training_path, training_kapture)
# finally import images
if images_import_method != TransferAction.skip:
filename_list = [
f
for _, _, f in kapture.flatten(training_kapture.records_camera)
]
logger.info(f'importing {len(filename_list)} image files ...')
import_record_data_from_dir_auto(training_images_path,
kapture_training_path,
filename_list,
images_import_method)
# Import query images
kapture_query_path = path.join(top_kaptures_path, f'slice{slice_n}',
"query")
delete_existing_kapture_files(kapture_query_path,
force_erase=force_overwrite_existing)
query_records_camera, query_trajectories = import_extended_cmu_seasons_images(
query_image_list)
query_kapture = kapture.Kapture(sensors=cameras,
records_camera=query_records_camera,
trajectories=query_trajectories)
# import query gt when possible
query_gt_kapture = []
for query_gt_path in query_gt_list:
query_gt_records_camera, query_gt_trajectories = import_extended_cmu_seasons_images(
query_gt_path)
query_gt_kapture.append(
kapture.Kapture(sensors=cameras,
records_camera=query_gt_records_camera,
trajectories=query_gt_trajectories))
data_to_merge = [query_kapture] + query_gt_kapture
query_kapture = merge_keep_ids(
data_to_merge,
skip_list=[],
data_paths=["" for _ in range(len(data_to_merge))],
kapture_path="",
images_import_method=TransferAction.skip)
if import_all_files:
_add_images_from_folder(query_images_path, query_kapture)
kapture_to_dir(kapture_query_path, query_kapture)
# finally import images
if images_import_method != TransferAction.skip:
filename_list = [
f for _, _, f in kapture.flatten(query_kapture.records_camera)
]
logger.info(f'importing {len(filename_list)} image files ...')
import_record_data_from_dir_auto(query_images_path,
kapture_query_path, filename_list,
images_import_method)
def import_robotcar_seasons(
robotcar_path:
str, # noqa: C901: function a bit long but not too complex
kapture_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip,
import_feature_db: bool = False,
skip_reconstruction: bool = False,
rig_collapse: bool = False,
use_colmap_intrinsics: bool = False,
import_v1: bool = False) -> None:
"""
Read the RobotCar Seasons data, creates several kaptures with training and query data.
:param robotcar_path: path to the robotcar top directory
:param kapture_path: path to the kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
:param import_feature_db: if True, will import the features from the database
:param skip_reconstruction: if True, will skip the reconstruction part from the training data
:param rig_collapse: if True, will collapse the rig
:param use_colmap_intrinsics: if True, will use the colmap intrinsics
:param import_v1: if True, will use the version 1 of the format
"""
os.makedirs(kapture_path, exist_ok=True)
cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics'))
rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics'))
logger.info("Importing test data")
# Test data
image_pattern = re.compile(
r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg')
queries_path = path.join(robotcar_path, '3D-models', 'individual',
'queries_per_location')
kapture_imported_query = {}
for root, dirs, files in os.walk(queries_path):
for query_file in files:
records_camera = kapture.RecordsCamera()
# Get list of query images
with open(path.join(queries_path, query_file)) as f:
for line in f:
matches = image_pattern.match(line)
image_path = line.strip()
if not matches:
logger.warning(f"Error matching line in {image_path}")
continue
matches = matches.groupdict()
timestamp = int(matches['timestamp'])
camera = str(matches['camera'])
# condition = str(matches['condition']) : not used ?
records_camera[timestamp, camera] = image_path
(query_name, _) = query_file.split('.')
kapture_test = kapture.Kapture(sensors=cameras,
rigs=rigs,
records_camera=records_camera)
kapture_imported_query[int(
query_name.split('_')[-1])] = kapture_test
# Training data
logger.info("Importing training data")
colmap_reconstructions_path = path.join(robotcar_path, '3D-models',
'individual',
'colmap_reconstructions')
kapture_imported_training = {}
for root, dirs, files in os.walk(colmap_reconstructions_path):
for colmap_reconstruction in dirs:
(loc_id, _) = colmap_reconstruction.split('_')
kapture_reconstruction_dir = path.join(kapture_path,
f"{int(loc_id):02d}",
"mapping")
delete_existing_kapture_files(kapture_reconstruction_dir,
force_overwrite_existing)
logger.info(f'Converting reconstruction {loc_id} to kapture ...')
kapture_reconstruction_data = import_robotcar_colmap_location(
robotcar_path,
path.join(colmap_reconstructions_path, colmap_reconstruction),
kapture_reconstruction_dir, rigs, skip_reconstruction)
# replace intrinsics with the ones found in the text files
if not use_colmap_intrinsics:
kapture_reconstruction_data.sensors = cameras
kapture_imported_training[int(
loc_id)] = kapture_reconstruction_data
if not import_v1:
_import_robotcar_v2_train(robotcar_path, kapture_imported_query,
kapture_imported_training, image_pattern)
# apply rig collapse
if rig_collapse:
logger.info('replacing camera poses with rig poses.')
for kapture_mapping in kapture_imported_training.values():
kapture.rigs_recover_inplace(kapture_mapping.trajectories, rigs,
['rear'])
# IO operations
robotcar_image_path = path.join(robotcar_path, "images")
for loc_id, kapture_query in kapture_imported_query.items():
loc_id_str = f"{loc_id:02d}"
logger.info(f'writing test data: {loc_id_str}')
kapture_test_dir = path.join(kapture_path, loc_id_str, "query")
delete_existing_kapture_files(kapture_test_dir,
force_overwrite_existing)
if not kapture_query.records_camera: # all images were removed
continue
kapture_to_dir(kapture_test_dir, kapture_query)
query_images = [
f for _, _, f in kapture.flatten(kapture_query.records_camera)
]
import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir,
query_images, images_import_method)
for loc_id, kapture_mapping in kapture_imported_training.items():
loc_id_str = f"{loc_id:02d}"
logger.info(f'writing mapping data: {loc_id_str}')
kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}",
"mapping")
kapture_to_dir(kapture_reconstruction_dir, kapture_mapping)
mapping_images = [
f for _, _, f in kapture.flatten(kapture_mapping.records_camera)
]
import_record_data_from_dir_auto(robotcar_image_path,
kapture_reconstruction_dir,
mapping_images, images_import_method)
if import_feature_db:
_import_colmap_overcast_reference(robotcar_path, kapture_path,
force_overwrite_existing)
def import_virtual_gallery(input_root_path: str,
configuration: str,
light_range: List[int],
loop_range: List[int],
camera_range: List[int],
occlusion_range: List[int],
as_rig: bool,
images_import_method: TransferAction,
kapture_path: str,
force_overwrite_existing: bool = False) -> None:
"""
Creates a kapture with a virtual gallery.
:param input_root_path: root path of virtual gallery
:param configuration: training, testing or all (both)
:param light_range: list of lights to include
:param loop_range: list of training loops to include
:param camera_range: list of training cameras to include
:param occlusion_range: list of testing occlusion levels to include
:param as_rig: in training trajectories, writes the position of the rig instead of individual cameras
:param kapture_path: path to kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
"""
# Check for existing files
os.makedirs(kapture_path, exist_ok=True)
delete_existing_kapture_files(kapture_path, force_overwrite_existing)
offset = 0
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
trajectories = kapture.Trajectories()
rigs = kapture.Rigs()
# Process all training data
if configuration == "training" or configuration == "all":
logger.info("Reading training files")
camera_range_set = set(camera_range)
training_intrinsics = import_training_intrinsics(input_root_path, light_range, loop_range, camera_range_set)
training_extrinsics = import_training_extrinsics(input_root_path, light_range, loop_range, camera_range_set)
convert_training_intrinsics(training_intrinsics, cameras)
convert_training_extrinsics(offset, training_extrinsics, images, trajectories, as_rig)
rigs.update(training_rig_config)
offset += len(training_extrinsics)
# Process all testing data
if configuration == "testing" or configuration == "all":
logger.info("Reading testing files")
testing_intrinsics = import_testing_intrinsics(input_root_path, light_range, occlusion_range)
testing_extrinsics = import_testing_extrinsics(input_root_path, light_range, occlusion_range)
convert_testing_intrinsics(testing_intrinsics, cameras)
convert_testing_extrinsics(offset, testing_extrinsics, images, trajectories)
offset += len(testing_extrinsics)
logger.info("Writing imported data to disk")
kapture_data = kapture.Kapture(sensors=cameras, records_camera=images, trajectories=trajectories, rigs=rigs or None)
# import images
image_list = [name for _, _, name in kapture.flatten(kapture_data.records_camera)]
import_record_data_from_dir_auto(input_root_path, kapture_path, image_list, images_import_method)
kapture_to_dir(kapture_path, kapture_data)
def import_7scenes(d7scenes_path: str,
kapture_dir_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip,
partition: Optional[str] = None
) -> None:
"""
Imports RGB-D Dataset 7-Scenes dataset and save them as kapture.
:param d7scenes_path: path to the 7scenes sequence root path
:param kapture_dir_path: path to kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
:param partition: if specified = 'mapping' or 'query'. Requires d7scenes_path/TestSplit.txt or TrainSplit.txt
to exists.
"""
os.makedirs(kapture_dir_path, exist_ok=True)
delete_existing_kapture_files(kapture_dir_path, force_erase=force_overwrite_existing)
logger.info('loading all content ...')
d7s_filename_re = re.compile(r'((?P<sequence>.+)/)?frame-(?P<frame_id>\d{6})\.(?P<suffix>\w*)\.(?P<ext>\w*)')
# populate all relevant files
d7s_filenames = (path_secure(path.relpath(path.join(dp, fn), d7scenes_path))
for dp, _, fs in os.walk(d7scenes_path) for fn in fs)
logger.info('populating 7-scenes files ...')
d7s_filenames = {filename: d7s_filename_re.search(filename).groupdict()
for filename in sorted(d7s_filenames)
if d7s_filename_re.search(filename)}
# reorg as shot[seq, id] = {color: , depth: , pose: , ...}
shots = {}
for timestamp, (filename, file_attribs) in enumerate(d7s_filenames.items()):
shot_id = (file_attribs.get('sequence'), file_attribs['frame_id'])
shots.setdefault(shot_id, {})[file_attribs['suffix']] = filename
# fake timestamps
for timestamp, shot_id in enumerate(shots):
shots[shot_id]['timestamp'] = timestamp
# if given, filter partition
if partition is not None:
# read the authors split file
partition_filepath = path.join(d7scenes_path, PARTITION_FILENAMES[partition])
if not path.isfile(partition_filepath):
raise FileNotFoundError(f'partition file is missing: {partition_filepath}.')
with open(partition_filepath, 'rt') as file:
split_sequences = [f'seq-{int(seq.strip()[len("sequence"):]):02}' for seq in file.readlines()]
assert len(split_sequences) > 0
# filter out
shots = {(seq, frame): shot
for (seq, frame), shot in shots.items()
if seq in split_sequences}
if len(shots) == 0:
raise FileNotFoundError('no file found: make sure the path to 7scenes sequence is valid.')
# eg. shots['seq-01', '000000'] =
# {
# 'color': 'seq-01/frame-000000.color.jpg',
# 'depth': 'seq-01/frame-000000.depth.png',
# 'pose': 'seq-01/frame-000000.pose.txt',
# 'timestamp': 0}
# images + depth maps
logger.info('populating image and depth maps files ...')
snapshots = kapture.RecordsCamera()
depth_maps = kapture.RecordsDepth()
for shot in shots.values():
snapshots[shot['timestamp'], RGB_SENSOR_ID] = shot['color']
kapture_depth_map_filename = shot['depth'][:-len('.png')] # kapture depth files are not png
depth_maps[shot['timestamp'], DEPTH_SENSOR_ID] = kapture_depth_map_filename
# poses
logger.info('import poses files ...')
trajectories = kapture.Trajectories()
for shot in shots.values():
pose_filepath = path.join(d7scenes_path, shot['pose'])
pose_mat = np.loadtxt(pose_filepath) # camera-to-world, 4×4 matrix in homogeneous coordinates
rotation_mat = pose_mat[0:3, 0:3]
position_vec = pose_mat[0:3, 3]
rotation_quat = quaternion.from_rotation_matrix(rotation_mat)
pose_world_from_cam = kapture.PoseTransform(r=rotation_quat, t=position_vec)
pose_cam_from_world = pose_world_from_cam.inverse()
trajectories[shot['timestamp'], RGBD_SENSOR_ID] = pose_cam_from_world
# sensors
"""
From authors: The RGB and depth camera have not been calibrated and we can’t provide calibration parameters at the
moment. The recorded frames correspond to the raw, uncalibrated camera images. In the KinectFusion pipeline we used
the following default intrinsics for the depth camera: Principle point (320,240), Focal length (585,585).
"""
sensors = kapture.Sensors()
camera_type = kapture.CameraType.SIMPLE_PINHOLE
camera_params = [640, 480, 585, 320, 240] # w, h, f, cx, cy
sensors[RGB_SENSOR_ID] = kapture.Camera(
name=RGB_SENSOR_ID,
camera_type=camera_type,
camera_params=camera_params
)
sensors[DEPTH_SENSOR_ID] = kapture.Camera(
name=DEPTH_SENSOR_ID,
camera_type=camera_type,
camera_params=camera_params,
sensor_type='depth'
)
# bind camera and depth sensor into a rig
logger.info('building rig with camera and depth sensor ...')
rigs = kapture.Rigs()
rigs[RGBD_SENSOR_ID, RGB_SENSOR_ID] = kapture.PoseTransform()
rigs[RGBD_SENSOR_ID, DEPTH_SENSOR_ID] = kapture.PoseTransform()
# import (copy) image files.
logger.info('copying image files ...')
image_filenames = [f for _, _, f in kapture.flatten(snapshots)]
import_record_data_from_dir_auto(d7scenes_path, kapture_dir_path, image_filenames, images_import_method)
# import (copy) depth map files.
logger.info('converting depth files ...')
depth_map_filenames = kapture.io.records.records_to_filepaths(depth_maps, kapture_dir_path)
hide_progress = logger.getEffectiveLevel() > logging.INFO
for depth_map_filename, depth_map_filepath_kapture in tqdm(depth_map_filenames.items(), disable=hide_progress):
depth_map_filepath_7scenes = path.join(d7scenes_path, depth_map_filename + '.png')
depth_map = np.array(Image.open(depth_map_filepath_7scenes))
# change invalid depth from 65535 to 0
depth_map[depth_map == 65535] = 0
# depth maps is in mm in 7scenes, convert it to meters
depth_map = depth_map.astype(np.float32) * 1.0e-3
kapture.io.records.records_depth_to_file(depth_map_filepath_kapture, depth_map)
# pack into kapture format
imported_kapture = kapture.Kapture(
records_camera=snapshots,
records_depth=depth_maps,
rigs=rigs,
trajectories=trajectories,
sensors=sensors)
logger.info('writing imported data ...')
kapture_to_dir(kapture_dir_path, imported_kapture)
def import_robotcar_seasons(
robotcar_path: str,
kapture_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip,
skip_reconstruction: bool = False,
rig_collapse: bool = False,
use_colmap_intrinsics: bool = False,
import_v1: bool = False) -> None:
"""
Read the RobotCar Seasons data, creates several kaptures with training and query data.
:param robotcar_path: path to the robotcar top directory
:param kapture_path: path to the kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
:param skip_reconstruction: if True, will skip the reconstruction part from the training data
:param rig_collapse: if True, will collapse the rig
"""
kapture_path = path.join(kapture_path, "base")
os.makedirs(kapture_path, exist_ok=True)
cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics'))
rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics'))
logger.info("Importing test data")
# Test data
image_pattern = re.compile(
r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg')
queries_path = path.join(robotcar_path, '3D-models', 'individual',
'queries_per_location')
kapture_imported_query = {}
for root, dirs, files in os.walk(queries_path):
for query_file in files:
records_camera = kapture.RecordsCamera()
# Get list of query images
with open(path.join(queries_path, query_file)) as f:
for line in f:
matches = image_pattern.match(line)
image_path = line.strip()
if not matches:
logger.warning(f"Error matching line in {image_path}")
continue
matches = matches.groupdict()
timestamp = int(matches['timestamp'])
camera = str(matches['camera'])
condition = str(matches['condition'])
records_camera[timestamp, camera] = image_path
(query_name, _) = query_file.split('.')
kapture_test = kapture.Kapture(sensors=cameras,
rigs=rigs,
records_camera=records_camera)
kapture_imported_query[int(
query_name.split('_')[-1])] = kapture_test
# Reference map data
logger.info("Importing reference map")
colmap_reconstructions_path = path.join(robotcar_path, '3D-models',
'individual',
'colmap_reconstructions')
kapture_imported_mapping = {}
for root, dirs, files in os.walk(colmap_reconstructions_path):
for colmap_reconstruction in dirs:
(loc_id, _) = colmap_reconstruction.split('_')
kapture_reconstruction_dir = path.join(kapture_path,
f"{int(loc_id):02d}",
"mapping")
delete_existing_kapture_files(kapture_reconstruction_dir,
force_erase=force_overwrite_existing)
logger.info(f'Converting reconstruction {loc_id} to kapture ...')
kapture_reconstruction_data = import_robotcar_colmap_location(
robotcar_path,
path.join(colmap_reconstructions_path, colmap_reconstruction),
kapture_reconstruction_dir, rigs, skip_reconstruction)
# replace intrinsics with the ones found in the text files
if not use_colmap_intrinsics:
kapture_reconstruction_data.sensors = cameras
kapture_imported_mapping[int(loc_id)] = kapture_reconstruction_data
if not import_v1:
queries_per_location = {
image_name: (ts, cam_id, loc_id)
for loc_id, kdata_test in kapture_imported_query.items() for ts,
cam_id, image_name in kapture.flatten(kdata_test.records_camera)
}
kapture_imported_training = {} # stores kapture for each submap
# read robotcar_v2_train.txt
v2_train_data = read_robotcar_v2_train(robotcar_path)
for image_name, pose in v2_train_data.items():
ts, cam_id, loc_id = queries_per_location[image_name]
assert cam_id == 'rear'
# create kapture object for submap if it doesn't exist
if loc_id not in kapture_imported_training:
kapture_loc_id = kapture.Kapture(sensors=cameras, rigs=rigs)
kapture_loc_id.records_camera = kapture.RecordsCamera()
kapture_loc_id.trajectories = kapture.Trajectories()
kapture_imported_training[loc_id] = kapture_loc_id
kapture_imported_training[loc_id].records_camera[
ts, cam_id] = image_name
kapture_imported_training[loc_id].trajectories[ts, cam_id] = pose
matches = image_pattern.match(image_name)
if not matches:
logger.warning(f"Error matching line in {image_name}")
continue
matches = matches.groupdict()
condition = str(matches['condition'])
timestamp = str(matches['timestamp'])
camera = str(matches['camera'])
# added left and right images in records_camera
left_image_name = condition + '/' + 'left' + '/' + timestamp + '.jpg'
right_image_name = condition + '/' + 'right' + '/' + timestamp + '.jpg'
kapture_imported_training[loc_id].records_camera[
ts, 'left'] = left_image_name
kapture_imported_training[loc_id].records_camera[
ts, 'right'] = right_image_name
# remove entries from query
del kapture_imported_query[loc_id].records_camera[ts][cam_id]
del kapture_imported_query[loc_id].records_camera[ts]['left']
del kapture_imported_query[loc_id].records_camera[ts]['right']
del kapture_imported_query[loc_id].records_camera[ts]
# all remaining query images are kept; reading robotcar_v2_test.txt is not necessary
# apply rig collapse
if rig_collapse:
logger.info('replacing camera poses with rig poses.')
for kdata_mapping in kapture_imported_mapping.values():
kapture.rigs_recover_inplace(kdata_mapping.trajectories, rigs,
'rear')
for kdata_training in kapture_imported_training.values():
kapture.rigs_recover_inplace(kdata_training.trajectories, rigs,
'rear')
# IO operations
robotcar_image_path = path.join(robotcar_path, "images")
for loc_id, kdata_query in kapture_imported_query.items():
loc_id_str = f"{loc_id:02d}"
logger.info(f'writing test data: {loc_id_str}')
kapture_test_dir = path.join(kapture_path, loc_id_str, "query")
delete_existing_kapture_files(kapture_test_dir,
force_erase=force_overwrite_existing)
if not kdata_query.records_camera: # all images were removed
continue
kapture_to_dir(kapture_test_dir, kdata_query)
query_images = [
f for _, _, f in kapture.flatten(kdata_query.records_camera)
]
import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir,
query_images, images_import_method)
for loc_id, kdata_mapping in kapture_imported_mapping.items():
loc_id_str = f"{loc_id:02d}"
logger.info(f'writing mapping data: {loc_id_str}')
kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}",
"mapping")
delete_existing_kapture_files(kapture_reconstruction_dir,
force_erase=force_overwrite_existing)
kapture_to_dir(kapture_reconstruction_dir, kdata_mapping)
mapping_images = [
f for _, _, f in kapture.flatten(kdata_mapping.records_camera)
]
import_record_data_from_dir_auto(robotcar_image_path,
kapture_reconstruction_dir,
mapping_images, images_import_method)
for loc_id, kdata_training in kapture_imported_training.items():
loc_id_str = f"{loc_id:02d}"
logger.info(f'writing training data: {loc_id_str}')
kapture_training_dir = path.join(kapture_path, f"{loc_id:02d}",
"training")
delete_existing_kapture_files(kapture_training_dir,
force_erase=force_overwrite_existing)
kapture_to_dir(kapture_training_dir, kdata_training)
mapping_images = [
f for _, _, f in kapture.flatten(kdata_training.records_camera)
]
import_record_data_from_dir_auto(robotcar_image_path,
kapture_training_dir, mapping_images,
images_import_method)
def import_12scenes(d12scenes_path: str,
kapture_dir_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip,
partition: Optional[str] = None
) -> None:
"""
Imports RGB-D Dataset 12-Scenes dataset and save them as kapture.
:param d12scenes_path: path to the 12scenes sequence root path
:param kapture_dir_path: path to kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
:param partition: if specified = 'mapping' or 'query'.
"""
os.makedirs(kapture_dir_path, exist_ok=True)
delete_existing_kapture_files(kapture_dir_path, force_erase=force_overwrite_existing)
logger.info('loading all content ...')
d7s_filename_re = re.compile(r'frame-(?P<frame_id>\d{6})\.(?P<suffix>\w*)\.(?P<ext>\w*)')
# populate all relevant files
d12images_path = os.path.join(d12scenes_path, 'data')
d7s_filenames = (path_secure(path.relpath(path.join(dp, fn), d12images_path))
for dp, _, fs in os.walk(d12images_path) for fn in fs)
logger.info('populating 12-scenes files ...')
d7s_filenames = {filename: d7s_filename_re.search(filename).groupdict()
for filename in sorted(d7s_filenames)
if d7s_filename_re.search(filename)}
# reorg as shot[seq, id] = {color: , depth: , pose: , ...}
shots = {}
for timestamp, (filename, file_attribs) in enumerate(d7s_filenames.items()):
shot_id = int(file_attribs['frame_id'])
shots.setdefault(shot_id, {})[file_attribs['suffix']] = filename
# fake timestamps
for timestamp, shot_id in enumerate(shots):
shots[shot_id]['timestamp'] = timestamp
# if given, filter partition
if partition is not None:
# read the authors split file
partition_filepath = path.join(d12scenes_path, 'split.txt')
if not path.isfile(partition_filepath):
raise FileNotFoundError(f'partition file is missing: {partition_filepath}.')
with open(partition_filepath, 'rt') as file:
# note from dsac++; the first sequence is used for testing, everything else for training
d7s_split_exp = r'^sequence(?P<sequence>\d+) \[frames=(?P<count>\d+)\] \[start=(?P<start_frame>\d+) ;' \
r' end=(?P<end_frame>\d+)\]$'
d7s_split_re = re.compile(d7s_split_exp)
split_sequences = [re.match(d7s_split_re, line) for line in file.readlines()]
if len(split_sequences) < 1 or not split_sequences[0]:
raise ValueError('failed to parse split.txt file')
test_split = (int(split_sequences[0].group('start_frame')), int(split_sequences[0].group('end_frame')))
# filter out
if partition == "query":
shots = {frame: shot
for frame, shot in shots.items()
if test_split[0] <= frame <= test_split[1]
}
elif partition == "mapping":
shots = {frame: shot
for frame, shot in shots.items()
if frame < test_split[0] or frame > test_split[1]
}
else:
raise ValueError('invalid partition name')
if len(shots) == 0:
raise FileNotFoundError('no file found: make sure the path to 12scenes sequence is valid.')
# eg. shots['000000'] =
# {
# 'color': 'seq-01/frame-000000.color.jpg',
# 'depth': 'seq-01/frame-000000.depth.png',
# 'pose': 'seq-01/frame-000000.pose.txt',
# 'timestamp': 0}
# images + depth maps
logger.info('populating image and depth maps files ...')
snapshots = kapture.RecordsCamera()
depth_maps = kapture.RecordsDepth()
for shot in shots.values():
snapshots[shot['timestamp'], RGB_SENSOR_ID] = shot['color']
kapture_depth_map_filename = shot['depth'][:-len('.png')] # kapture depth files are not png
depth_maps[shot['timestamp'], DEPTH_SENSOR_ID] = kapture_depth_map_filename
kapture_registered_depth_map_filename = shot['depth'][:-len('.png')] + '.reg' # kapture depth files are not png
depth_maps[shot['timestamp'], REG_DEPTH_SENSOR_ID] = kapture_registered_depth_map_filename
# poses
logger.info('import poses files ...')
trajectories = kapture.Trajectories()
for shot in shots.values():
pose_filepath = path.join(d12images_path, shot['pose'])
pose_mat = np.loadtxt(pose_filepath) # camera-to-world, 4×4 matrix in homogeneous coordinates
with open(pose_filepath, 'r') as file:
if 'INF' in file.read():
timestamp = shot['timestamp']
image_name = shot['color']
logger.debug(f'ts={timestamp}, name={image_name}: ignored inf pose')
continue
rotation_mat = pose_mat[0:3, 0:3]
position_vec = pose_mat[0:3, 3]
rotation_quat = quaternion.from_rotation_matrix(rotation_mat)
pose_world_from_cam = kapture.PoseTransform(r=rotation_quat, t=position_vec)
pose_cam_from_world = pose_world_from_cam.inverse()
trajectories[shot['timestamp'], RGBD_SENSOR_ID] = pose_cam_from_world
# sensors
"""
Read info.txt
"""
info_filepath = path.join(d12scenes_path, 'info.txt')
if not path.isfile(info_filepath):
raise FileNotFoundError(f'info file is missing: {info_filepath}.')
with open(info_filepath, 'rt') as file:
info_dict = {}
for line in file.readlines():
line_splits = line.rstrip().split(' = ')
info_dict[line_splits[0]] = line_splits[1]
sensors = kapture.Sensors()
camera_type = kapture.CameraType.PINHOLE
assert 'm_calibrationColorIntrinsic' in info_dict
assert 'm_colorWidth' in info_dict
assert 'm_colorHeight' in info_dict
rgb_intrinsics = [float(v) for v in info_dict['m_calibrationColorIntrinsic'].split(' ')]
# w, h, fx, fy, cx, cy
rgb_camera_params = [int(info_dict['m_colorWidth']), int(info_dict['m_colorHeight']),
rgb_intrinsics[0], rgb_intrinsics[5], rgb_intrinsics[2], rgb_intrinsics[6]]
sensors[RGB_SENSOR_ID] = kapture.Camera(
name=RGB_SENSOR_ID,
camera_type=camera_type,
camera_params=rgb_camera_params
)
assert 'm_calibrationDepthIntrinsic' in info_dict
assert 'm_depthWidth' in info_dict
assert 'm_depthHeight' in info_dict
depth_intrinsics = [float(v) for v in info_dict['m_calibrationDepthIntrinsic'].split(' ')]
# w, h, fx, fy, cx, cy
depth_camera_params = [int(info_dict['m_depthWidth']), int(info_dict['m_depthHeight']),
depth_intrinsics[0], depth_intrinsics[5], depth_intrinsics[2], depth_intrinsics[6]]
sensors[DEPTH_SENSOR_ID] = kapture.Camera(
name=DEPTH_SENSOR_ID,
camera_type=camera_type,
camera_params=depth_camera_params,
sensor_type='depth'
)
sensors[REG_DEPTH_SENSOR_ID] = kapture.Camera(
name=REG_DEPTH_SENSOR_ID,
camera_type=camera_type,
camera_params=rgb_camera_params,
sensor_type='depth'
)
# bind camera and depth sensor into a rig
logger.info('building rig with camera and depth sensor ...')
rigs = kapture.Rigs()
rigs[RGBD_SENSOR_ID, RGB_SENSOR_ID] = kapture.PoseTransform()
rigs[RGBD_SENSOR_ID, DEPTH_SENSOR_ID] = kapture.PoseTransform()
rigs[RGBD_SENSOR_ID, REG_DEPTH_SENSOR_ID] = kapture.PoseTransform()
# import (copy) image files.
logger.info('copying image files ...')
image_filenames = [f for _, _, f in kapture.flatten(snapshots)]
import_record_data_from_dir_auto(d12images_path, kapture_dir_path, image_filenames, images_import_method)
# import (copy) depth map files.
logger.info('converting depth files ...')
depth_map_filenames = kapture.io.records.records_to_filepaths(depth_maps, kapture_dir_path)
hide_progress = logger.getEffectiveLevel() > logging.INFO
for depth_map_filename, depth_map_filepath_kapture in tqdm(depth_map_filenames.items(), disable=hide_progress):
if '.reg' in depth_map_filename:
continue
depth_map_filepath_12scenes = path.join(d12images_path, depth_map_filename + '.png')
depth_map = np.array(Image.open(depth_map_filepath_12scenes))
# depth maps is in mm in 12scenes, convert it to meters
depth_map = depth_map.astype(np.float32) * 1.0e-3
kapture.io.records.records_depth_to_file(depth_map_filepath_kapture, depth_map)
# register depth to rgb
reg_depth_map = register_depth(get_K(camera_type, depth_camera_params), get_K(camera_type, rgb_camera_params),
np.eye(4), depth_map, rgb_camera_params[0], rgb_camera_params[1])
kapture.io.records.records_depth_to_file(depth_map_filepath_kapture + '.reg', reg_depth_map)
# pack into kapture format
imported_kapture = kapture.Kapture(
records_camera=snapshots,
records_depth=depth_maps,
rigs=rigs,
trajectories=trajectories,
sensors=sensors)
logger.info('writing imported data ...')
kapture_to_dir(kapture_dir_path, imported_kapture)
def save(self):
kapture_to_dir(self.kapture_path, self)
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 import_image_list(images_list_filenames: List[str],
images_dirpath: str,
kapture_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Imports the list of images to a kapture. This creates only images and cameras.
:param images_list_filenames: list of text files containing image file names
:param images_dirpath: path to images directory.
:param kapture_path: path to kapture root directory.
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
assert isinstance(images_list_filenames, list)
os.makedirs(kapture_path, exist_ok=True)
delete_existing_kapture_files(kapture_path, force_erase=force_overwrite_existing)
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
offset = 0
logger.info('starting conversion...')
for images_list_filename in images_list_filenames:
logger.info(f'loading {images_list_filename}')
with open(images_list_filename) as file:
images_list = file.readlines()
# remove end line char and empty lines
images_list = [line.rstrip() for line in images_list if line != '\n']
for i in range(0, len(images_list)):
line = images_list[i].split()
image_file_name = line[0]
if len(line) > 1:
model = line[1]
model_params = line[2:]
else:
model = kapture.CameraType.UNKNOWN_CAMERA.value
try:
# lazy open
with Image.open(path.join(images_dirpath, image_file_name)) as im:
width, height = im.size
model_params = [width, height]
except (OSError, PIL.UnidentifiedImageError):
# It is not a valid image: skip it
logger.info(f'Skipping invalid image file {image_file_name}')
continue
camera_id = f'sensor{i + offset}'
cameras[camera_id] = kapture.Camera(model, model_params)
images[(i + offset, camera_id)] = image_file_name
offset += len(images_list)
# import (copy) image files.
logger.info('import image files ...')
filename_list = [f for _, _, f in kapture.flatten(images)]
import_record_data_from_dir_auto(images_dirpath, kapture_path, filename_list, images_import_method)
# pack into kapture format
imported_kapture = kapture.Kapture(sensors=cameras, records_camera=images)
logger.info('writing imported data...')
kapture_to_dir(kapture_path, imported_kapture)
def create_3D_model_from_depth_from_loaded_data(
kdata: kapture.Kapture, input_path: str, tar_handlers: TarCollection,
output_path: str, keypoints_type: Optional[str], depth_sensor_id: str,
topk: int, method: Method, cellsizes: List[str], force: bool):
"""
Create 3D model from a kapture dataset that has registered depth data
Assumes the kapture data is already loaded
"""
logger.info(f'create 3D model using depth data')
if os.path.exists(output_path) and not force:
print(f'outpath already exists, use --force to overwrite')
return -1
if kdata.rigs is not None:
assert kdata.trajectories is not None
kapture.rigs_remove_inplace(kdata.trajectories, kdata.rigs)
if keypoints_type is None:
keypoints_type = try_get_only_key_from_collection(kdata.keypoints)
assert keypoints_type is not None
assert kdata.keypoints is not None
assert keypoints_type in kdata.keypoints
if method == Method.voxelgrid:
vg = VoxelGrid(cellsizes)
# add all 3D points to map that correspond to a keypoint
logger.info('adding points from scan to kapture')
points3d = []
observations = kapture.Observations()
progress_bar = tqdm(total=len(
list(kapture.flatten(kdata.records_camera, is_sorted=True))),
disable=logger.level >= logging.CRITICAL)
for timestamp, sensor_id, sensing_filepath in kapture.flatten(
kdata.records_camera, is_sorted=True):
logger.info(
f'total 3d points: {len(points3d)}, processing {sensing_filepath}')
# check if images have a pose
if timestamp not in kdata.trajectories:
logger.info('{} does not have a pose. skipping ...'.format(
sensing_filepath))
continue
# check if depth map exists
depth_map_record = ''
if timestamp in kdata.records_depth:
if depth_sensor_id is None:
depth_id = sensor_id + '_depth'
else:
depth_id = depth_sensor_id
if depth_id in kdata.records_depth[timestamp]:
depth_map_record = kdata.records_depth[timestamp][depth_id]
depth_map_size = tuple(
[int(x) for x in kdata.sensors[depth_id].camera_params[0:2]])
depth_path = get_depth_map_fullpath(input_path, depth_map_record)
if not os.path.exists(depth_path):
logger.info('no 3D data found for {}. skipping ...'.format(
sensing_filepath))
continue
depth_map = depth_map_from_file(depth_path, depth_map_size)
img = Image.open(get_image_fullpath(input_path,
sensing_filepath)).convert('RGB')
assert img.size[0] == depth_map_size[0]
assert img.size[1] == depth_map_size[1]
kps_raw = load_keypoints(keypoints_type, input_path, sensing_filepath,
kdata.keypoints[keypoints_type].dtype,
kdata.keypoints[keypoints_type].dsize,
tar_handlers)
_, camera_sensor_C, camera_dist = get_camera_matrix_from_kapture(
np.zeros((1, 0, 2), dtype=np.float64), kdata.sensors[sensor_id])
cv2_keypoints, depth_sensor_C, depth_dist = get_camera_matrix_from_kapture(
kps_raw, kdata.sensors[depth_id])
assert np.isclose(depth_sensor_C, camera_sensor_C).all()
assert np.isclose(depth_dist, camera_dist).all()
if np.count_nonzero(camera_dist) > 0:
epsilon = np.finfo(np.float64).eps
stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 500, epsilon)
undistorted_cv2_keypoints = cv2.undistortPointsIter(
cv2_keypoints,
camera_sensor_C,
camera_dist,
R=None,
P=camera_sensor_C,
criteria=stop_criteria)
else:
undistorted_cv2_keypoints = cv2_keypoints
cv2_keypoints = cv2_keypoints.reshape((kps_raw.shape[0], 2))
undistorted_cv2_keypoints = undistorted_cv2_keypoints.reshape(
(kps_raw.shape[0], 2))
points3d_img = []
rgb_img = []
kp_idxs = []
for idx_kp, kp in enumerate(cv2_keypoints[0:topk]):
u = round(kp[0])
v = round(kp[1])
undist_kp = undistorted_cv2_keypoints[idx_kp]
undist_u = round(undist_kp[0])
undist_v = round(undist_kp[1])
if u >= 0 and u < depth_map_size[
0] and v >= 0 and v < depth_map_size[1]:
if depth_map[v, u] == 0:
continue
pt3d = project_kp_to_3D(undist_u, undist_v, depth_map[v, u],
depth_sensor_C[0,
2], depth_sensor_C[1,
2],
depth_sensor_C[0,
0], depth_sensor_C[1,
1])
points3d_img.append(pt3d)
rgb_img.append(img.getpixel((u, v)))
kp_idxs.append(idx_kp)
# transform to world coordinates (pt3d from a depth map is in camera coordinates)
# we use sensor_id here because we assume that the image and the corresponding depthmap have the same pose
# and sometimes, the pose might only be provided for the images
cam_to_world = kdata.trajectories[timestamp][sensor_id].inverse()
if len(points3d_img) == 0:
continue
points3d_img = cam_to_world.transform_points(np.array(points3d_img))
for idx_kp, pt3d, rgb in zip(kp_idxs, points3d_img, rgb_img):
if not np.isnan(pt3d).any():
# apply transform (alignment)
if method == Method.voxelgrid:
assert vg is not None
if not vg.exists(pt3d):
# add 3D point
points3d.append(list(pt3d) + list(rgb))
# add observation
observations.add(
len(points3d) - 1, keypoints_type,
sensing_filepath, idx_kp)
vg.add(pt3d, len(points3d) - 1, sensing_filepath)
else:
ret = vg.append(pt3d, sensing_filepath)
if ret is not None:
observations.add(ret[0], keypoints_type,
sensing_filepath, idx_kp)
elif method == Method.all:
# add 3D point
points3d.append(list(pt3d) + list(rgb))
# add observation
observations.add(
len(points3d) - 1, keypoints_type, sensing_filepath,
idx_kp)
# save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply'))
progress_bar.update(1)
progress_bar.close()
kdata.points3d = kapture.Points3d(np.array(points3d))
kdata.observations = observations
logger.info('saving ...')
kapture_to_dir(output_path, kdata)
# save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply'))
logger.info('all done')
def import_opensfm(
opensfm_root_dir: str,
kapture_root_dir: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.copy) -> None:
"""
Convert an openSfM structure to a kapture on disk. Also copy, move or link the images files if necessary.
:param opensfm_root_dir: the openSfM top directory
:param kapture_root_dir: top directory of kapture created
:param force_overwrite_existing: if true, will remove existing kapture data without prompting the user
:param images_import_method: action to apply on images: link, copy, move or do nothing.
:return: the constructed kapture object
"""
disable_tqdm = logger.getEffectiveLevel() != logging.INFO
# load reconstruction
opensfm_reconstruction_filepath = path.join(opensfm_root_dir,
'reconstruction.json')
with open(opensfm_reconstruction_filepath, 'rt') as f:
opensfm_reconstruction = json.load(f)
# remove the single list @ root
opensfm_reconstruction = opensfm_reconstruction[0]
# prepare space for output
os.makedirs(kapture_root_dir, exist_ok=True)
delete_existing_kapture_files(kapture_root_dir,
force_erase=force_overwrite_existing)
# import cameras
kapture_sensors = kapture.Sensors()
assert 'cameras' in opensfm_reconstruction
# import cameras
for osfm_camera_id, osfm_camera in opensfm_reconstruction['cameras'].items(
):
camera = import_camera(osfm_camera, name=osfm_camera_id)
kapture_sensors[osfm_camera_id] = camera
# import shots
logger.info('importing images and trajectories ...')
kapture_images = kapture.RecordsCamera()
kapture_trajectories = kapture.Trajectories()
opensfm_image_dir_path = path.join(opensfm_root_dir, 'images')
assert 'shots' in opensfm_reconstruction
image_timestamps, image_sensors = {}, {
} # used later to retrieve the timestamp of an image.
for timestamp, (image_filename, shot) in enumerate(
opensfm_reconstruction['shots'].items()):
sensor_id = shot['camera']
image_timestamps[image_filename] = timestamp
image_sensors[image_filename] = sensor_id
# in OpenSfm, (sensor, timestamp) is not unique.
rotation_vector = shot['rotation']
q = quaternion.from_rotation_vector(rotation_vector)
translation = shot['translation']
# capture_time = shot['capture_time'] # may be invalid
# gps_position = shot['gps_position']
kapture_images[timestamp, sensor_id] = image_filename
kapture_trajectories[timestamp,
sensor_id] = kapture.PoseTransform(r=q,
t=translation)
# copy image files
filename_list = [f for _, _, f in kapture.flatten(kapture_images)]
import_record_data_from_dir_auto(
source_record_dirpath=opensfm_image_dir_path,
destination_kapture_dirpath=kapture_root_dir,
filename_list=filename_list,
copy_strategy=images_import_method)
# Imports Gnss
kapture_gnss = _import_gnss(opensfm_root_dir, kapture_sensors,
image_sensors, image_timestamps, disable_tqdm)
# Imports descriptors, keypoints and matches
kapture_descriptors, kapture_keypoints, kapture_matches = _import_features_and_matches(
opensfm_root_dir, kapture_root_dir, disable_tqdm)
# import 3-D points
if 'points' in opensfm_reconstruction:
logger.info('importing points 3-D')
opensfm_points = opensfm_reconstruction['points']
points_data = []
for point_id in sorted(opensfm_points):
point_data = opensfm_points[point_id]
point_data = point_data['coordinates'] + point_data['color']
points_data.append(point_data)
kapture_points = kapture.Points3d(points_data)
else:
kapture_points = None
# saving kapture csv files
logger.info('saving kapture files')
kapture_data = kapture.Kapture(sensors=kapture_sensors,
records_camera=kapture_images,
records_gnss=kapture_gnss,
trajectories=kapture_trajectories,
keypoints=kapture_keypoints,
descriptors=kapture_descriptors,
matches=kapture_matches,
points3d=kapture_points)
kapture_to_dir(kapture_root_dir, kapture_data)
def pose_approximation(mapping_path: str,
query_path: str,
output_path: str,
global_features_type: Optional[str],
topk: int,
force_overwrite_existing: bool,
method: PoseApproximationMethods,
additional_parameters: dict):
"""
compute approximated pose from image retrieval results
:param mapping_path: input path to kapture input root directory
:type mapping_path: str
:param query_path: input path to a kapture root directory
:type query_path: str
:param output_path: output path to pairsfile
:type output_path: str
:param global_features_type: type of global_features, name of the global_features subfolder
:param topk: the max number of top retained images
:type topk: int
:param additional_parameters: store method specific args
:type additional_parameters: dict
"""
assert mapping_path != query_path
os.makedirs(output_path, exist_ok=True)
delete_existing_kapture_files(output_path, force_erase=force_overwrite_existing)
logger.info(f'pose_approximation. loading mapping: {mapping_path}')
with get_all_tar_handlers(mapping_path) as mapping_tar_handlers:
kdata_map = kapture_from_dir(mapping_path, None, skip_list=[kapture.Keypoints,
kapture.Descriptors,
kapture.Matches,
kapture.Observations,
kapture.Points3d],
tar_handlers=mapping_tar_handlers)
assert kdata_map.sensors is not None
assert kdata_map.records_camera is not None
assert kdata_map.global_features is not None
if global_features_type is None:
global_features_type = try_get_only_key_from_collection(kdata_map.global_features)
assert global_features_type is not None
assert global_features_type in kdata_map.global_features
global_features_config = GlobalFeaturesConfig(kdata_map.global_features[global_features_type].type_name,
kdata_map.global_features[global_features_type].dtype,
kdata_map.global_features[global_features_type].dsize,
kdata_map.global_features[global_features_type].metric_type)
logger.info(f'computing pairs with {global_features_type}...')
map_global_features_to_filepaths = global_features_to_filepaths(
kdata_map.global_features[global_features_type],
global_features_type,
mapping_path,
mapping_tar_handlers
)
mapping_list = list(sorted(map_global_features_to_filepaths.items()))
map_stacked_features = stack_global_features(global_features_config, mapping_list)
logger.info(f'pose_approximation. loading query: {query_path}')
with get_all_tar_handlers(query_path) as query_tar_handlers:
kdata_query = kapture_from_dir(query_path, None, skip_list=[kapture.Keypoints,
kapture.Descriptors,
kapture.Matches,
kapture.Observations,
kapture.Points3d],
tar_handlers=query_tar_handlers)
assert kdata_query.sensors is not None
assert kdata_query.records_camera is not None
assert kdata_query.global_features is not None
assert global_features_type in kdata_query.global_features
kdata_mapping_gfeat = kdata_map.global_features[global_features_type]
kdata_query_gfeat = kdata_query.global_features[global_features_type]
assert kdata_mapping_gfeat.type_name == kdata_query_gfeat.type_name
assert kdata_mapping_gfeat.dtype == kdata_query_gfeat.dtype
assert kdata_mapping_gfeat.dsize == kdata_query_gfeat.dsize
query_global_features_to_filepaths = global_features_to_filepaths(
kdata_query_gfeat,
global_features_type,
query_path,
query_tar_handlers
)
query_list = list(sorted(query_global_features_to_filepaths.items()))
query_stacked_features = stack_global_features(global_features_config, query_list)
logger.info('computing pose approximation from with'
f' {kdata_map.global_features[global_features_type].type_name}...')
# main code
weights = get_interpolation_weights(method,
query_stacked_features,
map_stacked_features,
topk,
additional_parameters)
out_trajectories = get_interpolated_pose(kdata_map, kdata_query, weights)
out_kapture = kapture.Kapture(sensors=kdata_query.sensors,
records_camera=kdata_query.records_camera,
trajectories=out_trajectories)
kapture_to_dir(output_path, out_kapture)
logger.info('all done')
def test_read_write(self):
kapture_data = csv.kapture_from_dir(self._samples_folder)
csv.kapture_to_dir(self._tempdir.name, kapture_data)
def write_to_dir(self):
kapture_to_dir(self.path, self.kapture)
def import_idl_dataset_cvpr17(idl_dataset_path: str,
gt_path: Union[str, None],
kapture_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Reads the IDL dataset and copy it to a kapture.
:param idl_dataset_path: path to the IDL dataset
:param gt_path: ground truth data path
:param kapture_path: path to the kapture top directory to create
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
os.makedirs(kapture_path, exist_ok=True)
delete_existing_kapture_files(kapture_path, force_erase=force_overwrite_existing)
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
trajectories = kapture.Trajectories()
file_list = [os.path.relpath(os.path.join(dirpath, filename), idl_dataset_path)
for dirpath, dirs, filenames in os.walk(idl_dataset_path)
for filename in filenames]
file_list = sorted(file_list)
logger.info('starting conversion...')
for n, filename in enumerate(file_list):
# test if file is a valid image
try:
# lazy load
with Image.open(path.join(idl_dataset_path, filename)) as im:
width, height = im.size
model_params = [width, height]
except Exception:
continue
camera_id = f'sensor{n}'
images[(n, camera_id)] = path_secure(filename) # don't forget windows
model = kapture.CameraType.UNKNOWN_CAMERA
if gt_path is not None:
# replace image extension with .camera
file_gt_path = os.path.splitext(os.path.join(gt_path, filename))[0] + ".camera"
if os.path.isfile(file_gt_path):
with open(file_gt_path) as fin:
lines = fin.readlines()
lines = (line.rstrip().split() for line in lines) # split fields
lines = list(lines)
fx = float(lines[0][0])
cx = float(lines[0][2])
fy = float(lines[1][1])
cy = float(lines[1][2])
width_file = float(lines[8][0])
height_file = float(lines[8][1])
assert (width_file == width)
assert (height_file == height)
model = kapture.CameraType.PINHOLE
model_params = [width, height, fx, fy, cx, cy]
rotation_matrix = [[float(v) for v in line] for line in lines[4:7]]
rotation = quaternion.from_rotation_matrix(rotation_matrix)
center_of_projection = [float(v) for v in lines[7]]
pose = kapture.PoseTransform(rotation, center_of_projection).inverse()
trajectories[(n, camera_id)] = pose
cameras[camera_id] = kapture.Camera(model, model_params)
# if no trajectory were added, no need to create the file
if not trajectories:
trajectories = None
# import (copy) image files.
logger.info('import image files ...')
filename_list = [f for _, _, f in kapture.flatten(images)]
import_record_data_from_dir_auto(idl_dataset_path, kapture_path, filename_list, images_import_method)
# pack into kapture format
imported_kapture = kapture.Kapture(sensors=cameras, records_camera=images, trajectories=trajectories)
logger.info('writing imported data...')
kapture_to_dir(kapture_path, imported_kapture)
def import_7scenes(d7scenes_path: str,
kapture_dir_path: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip,
partition: Optional[str] = None
) -> None:
"""
Imports RGB-D Dataset 7-Scenes dataset and save them as kapture.
:param d7scenes_path: path to the 7scenes sequence root path
:param kapture_dir_path: path to kapture top directory
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
:param partition: if specified = 'mapping' or 'query'. Requires d7scenes_path/TestSplit.txt or TrainSplit.txt
to exists.
"""
os.makedirs(kapture_dir_path, exist_ok=True)
delete_existing_kapture_files(kapture_dir_path, force_erase=force_overwrite_existing)
logger.info('loading all content ...')
d7s_filename_re = re.compile(r'((?P<sequence>.+)/)?frame-(?P<frame_id>\d{6})\.(?P<suffix>\w*)\.(?P<ext>\w*)')
# populate all relevant files
d7s_filenames = (path_secure(path.relpath(path.join(dp, fn), d7scenes_path))
for dp, _, fs in os.walk(d7scenes_path) for fn in fs)
logger.info('populating 7-scenes files ...')
d7s_filenames = {filename: d7s_filename_re.search(filename).groupdict()
for filename in sorted(d7s_filenames)
if d7s_filename_re.search(filename)}
# reorg as shot[seq, id] = {color: , depth: , pose: , ...}
shots = {}
for timestamp, (filename, file_attribs) in enumerate(d7s_filenames.items()):
shot_id = (file_attribs.get('sequence'), file_attribs['frame_id'])
shots.setdefault(shot_id, {})[file_attribs['suffix']] = filename
# fake timestamps
for timestamp, shot_id in enumerate(shots):
shots[shot_id]['timestamp'] = timestamp
# if given, filter partition
if partition is not None:
# read the authors split file
partition_filepath = path.join(d7scenes_path, PARTITION_FILENAMES[partition])
if not path.isfile(partition_filepath):
raise FileNotFoundError(f'partition file is missing: {partition_filepath}.')
with open(partition_filepath, 'rt') as file:
split_sequences = [f'seq-{int(seq.strip()[len("sequence"):]):02}' for seq in file.readlines()]
assert len(split_sequences) > 0
# filter out
shots = {(seq, frame): shot
for (seq, frame), shot in shots.items()
if seq in split_sequences}
if len(shots) == 0:
raise FileNotFoundError('no file found: make sure the path to 7scenes sequence is valid.')
# eg. shots['seq-01', '000000'] =
# {
# 'color': 'seq-01/frame-000000.color.jpg',
# 'depth': 'seq-01/frame-000000.depth.png',
# 'pose': 'seq-01/frame-000000.pose.txt',
# 'timestamp': 0}
# images + depth maps
logger.info('populating image and depth maps files ...')
snapshots = kapture.RecordsCamera()
depth_maps = kapture.RecordsDepth()
for shot in shots.values():
snapshots[shot['timestamp'], RGB_SENSOR_ID] = shot['color']
kapture_depth_map_filename = shot['depth'][:-len('.png')] # kapture depth files are not png
depth_maps[shot['timestamp'], DEPTH_SENSOR_ID] = kapture_depth_map_filename
kapture_registered_depth_map_filename = shot['depth'][:-len('.png')] + '.reg' # kapture depth files are not png
depth_maps[shot['timestamp'], REG_DEPTH_SENSOR_ID] = kapture_registered_depth_map_filename
# poses
logger.info('import poses files ...')
trajectories = kapture.Trajectories()
for shot in shots.values():
pose_filepath = path.join(d7scenes_path, shot['pose'])
pose_mat = np.loadtxt(pose_filepath) # camera-to-world, 4×4 matrix in homogeneous coordinates
rotation_mat = pose_mat[0:3, 0:3]
position_vec = pose_mat[0:3, 3]
rotation_quat = quaternion.from_rotation_matrix(rotation_mat)
pose_world_from_cam = kapture.PoseTransform(r=rotation_quat, t=position_vec)
pose_cam_from_world = pose_world_from_cam.inverse()
trajectories[shot['timestamp'], RGBD_SENSOR_ID] = pose_cam_from_world
# sensors
"""
From authors: The RGB and depth camera have not been calibrated and we can’t provide calibration parameters at the
moment. The recorded frames correspond to the raw, uncalibrated camera images. In the KinectFusion pipeline we used
the following default intrinsics for the depth camera: Principle point (320,240), Focal length (585,585).
----
We use the extr. kinect camera parameters from https://projet.liris.cnrs.fr/voir/activities-dataset/kinect-calibration.html.
"""
sensors = kapture.Sensors()
# camera_type = kapture.CameraType.OPENCV
# camera_params = [640, 480, 5.2161910696979987e+02, 5.2132946256749767e+02, 3.1755491910920682e+02, 2.5921654718027673e+02,
# 2.5673002693536984e-01, -9.3976085633794137e-01, -1.8605549188751580e-03, -2.2232238578189420e-03] # w, h, f, cx, cy, k1, k2, p1, p2, k3
camera_type = kapture.CameraType.SIMPLE_PINHOLE
# camera_params = [640, 480, 5.2161910696979987e+02, 5.2132946256749767e+02, 3.1755491910920682e+02, 2.5921654718027673e+02] # w, h, fx, fy, cx, cy
camera_params = [640, 480, 525, 320, 240] # w, h, f, cx, cy
sensors[RGB_SENSOR_ID] = kapture.Camera(
name=RGB_SENSOR_ID,
camera_type=camera_type,
camera_params=camera_params
)
# depth_camera_type = kapture.CameraType.OPENCV
# depth_camera_params = [640, 480, 5.8818670481438744e+02, 5.8724220649505514e+02, 3.1076280589210484e+02, 2.2887144980135292e+02,
# -1.8932947734719333e-01, 1.1358015104098631e+00, -4.4260345347128536e-03, -5.4869578635708153e-03, -2.2460143607712921e+00] # w, h, f, cx, cy, k1, k2, p1, p2, k3
depth_camera_type = kapture.CameraType.SIMPLE_PINHOLE
# depth_camera_params = [640, 480, 5.8818670481438744e+02, 5.8724220649505514e+02, 3.1076280589210484e+02, 2.2887144980135292e+02] # w, h, fx, fy, cx, cy
depth_camera_params = [640, 480, 585, 320, 240] # w, h, f, cx, cy
sensors[DEPTH_SENSOR_ID] = kapture.Camera(
name=DEPTH_SENSOR_ID,
camera_type=depth_camera_type,
camera_params=depth_camera_params,
sensor_type='depth'
)
sensors[REG_DEPTH_SENSOR_ID] = kapture.Camera(
name=REG_DEPTH_SENSOR_ID,
camera_type=depth_camera_type,
camera_params=camera_params,
sensor_type='depth'
)
# bind camera and depth sensor into a rig
R = np.array([[9.9996518012567637e-01, 2.6765126468950343e-03, -7.9041012313000904e-03],
[-2.7409311281316700e-03, 9.9996302803027592e-01, -8.1504520778013286e-03],
[7.8819942130445332e-03, 8.1718328771890631e-03, 9.9993554558014031e-01]])
T = np.array([-2.5558943178152542e-02, 1.0109636268061706e-04, 2.0318321729487039e-03])
Rt = np.vstack((np.hstack((R, T.reshape(3, 1))), np.array([0, 0, 0, 1])))
logger.info('building rig with camera and depth sensor ...')
rigs = kapture.Rigs()
rigs[RGBD_SENSOR_ID, RGB_SENSOR_ID] = kapture.PoseTransform(quaternion.from_rotation_matrix(R), T)
rigs[RGBD_SENSOR_ID, REG_DEPTH_SENSOR_ID] = kapture.PoseTransform(quaternion.from_rotation_matrix(R), T)
rigs[RGBD_SENSOR_ID, DEPTH_SENSOR_ID] = kapture.PoseTransform()
# import (copy) image files.
logger.info('copying image files ...')
image_filenames = [f for _, _, f in kapture.flatten(snapshots)]
import_record_data_from_dir_auto(d7scenes_path, kapture_dir_path, image_filenames, images_import_method)
# import (copy) depth map files.
logger.info('converting depth files ...')
depth_map_filenames = kapture.io.records.records_to_filepaths(depth_maps, kapture_dir_path)
hide_progress = logger.getEffectiveLevel() > logging.INFO
for depth_map_filename, depth_map_filepath_kapture in tqdm(depth_map_filenames.items(), disable=hide_progress):
if '.reg' in depth_map_filename:
continue
depth_map_filepath_7scenes = path.join(d7scenes_path, depth_map_filename + '.png')
depth_map = np.array(Image.open(depth_map_filepath_7scenes))
# change invalid depth from 65535 to 0
depth_map[depth_map == 65535] = 0
# depth maps is in mm in 7scenes, convert it to meters
depth_map = depth_map.astype(np.float32) * 1.0e-3
kapture.io.records.records_depth_to_file(depth_map_filepath_kapture, depth_map)
# register depth to rgb
reg_depth_map = register_depth(get_K(depth_camera_type, depth_camera_params), get_K(camera_type, camera_params),
Rt, depth_map, camera_params[0], camera_params[1])
kapture.io.records.records_depth_to_file(depth_map_filepath_kapture + '.reg', reg_depth_map)
# pack into kapture format
imported_kapture = kapture.Kapture(
records_camera=snapshots,
records_depth=depth_maps,
rigs=rigs,
trajectories=trajectories,
sensors=sensors)
logger.info('writing imported data ...')
kapture_to_dir(kapture_dir_path, imported_kapture)
def import_bundler(
bundler_path: str,
image_list_path: str,
image_dir_path: str,
kapture_dir_path: str,
ignore_trajectories: bool,
add_reconstruction: bool,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Imports bundler data and save them as kapture.
:param bundler_path: path to the bundler model file
:param image_list_path: path to the file containing the list of image names
:param image_dir_path: input path to bundler image directory.
:param kapture_dir_path: path to kapture top directory
:param ignore_trajectories: if True, will not import the trajectories
:param add_reconstruction: if True, will create 3D points and observations
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
os.makedirs(kapture_dir_path, exist_ok=True)
delete_existing_kapture_files(kapture_dir_path,
force_erase=force_overwrite_existing)
logger.info('loading all content...')
# if there is a filter list, parse it
with open(image_list_path) as file:
file_content = file.readlines()
# remove end line char and empty lines
image_list = [line.rstrip() for line in file_content if line != '\n']
with open(bundler_path) as file:
bundler_content = file.readlines()
# remove end line char and empty lines
bundler_content = [
line.rstrip() for line in bundler_content if line != '\n'
]
assert bundler_content[0] == "# Bundle file v0.3"
# <num_cameras> <num_points>
line_1 = bundler_content[1].split()
number_of_cameras = int(line_1[0])
number_of_points = int(line_1[1])
offset = 2
number_of_lines_per_camera = 5 # 1 camera + 3 rotation + 1 translation
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
trajectories = kapture.Trajectories() if not ignore_trajectories else None
points3d = [] if add_reconstruction else None
keypoints = kapture.Keypoints('sift', np.float32,
2) if add_reconstruction else None
observations = kapture.Observations() if add_reconstruction else None
image_mapping = [] # bundler camera_id -> (name, width, height)
for i in range(0, number_of_cameras):
start_index = i * number_of_lines_per_camera + offset
file_name = image_list[i]
# process camera info
line_camera = bundler_content[start_index].split()
focal_length = float(line_camera[0])
k1 = float(line_camera[1])
k2 = float(line_camera[2])
# lazy open
with Image.open(path.join(image_dir_path, file_name)) as im:
width, height = im.size
image_mapping.append((file_name, width, height))
camera = kapture.Camera(
MODEL,
[width, height, focal_length, width / 2, height / 2, k1, k2])
camera_id = f'sensor{i}'
cameras[camera_id] = camera
# process extrinsics
rotation_matrix = [[float(v) for v in line.split()]
for line in bundler_content[start_index +
1:start_index + 4]]
quaternion_wxyz = quaternion.from_rotation_matrix(rotation_matrix)
translation = np.array(
[float(v) for v in bundler_content[start_index + 4].split()])
pose = kapture.PoseTransform(quaternion_wxyz, translation)
# The Bundler model uses a coordinate system that differs from the *computer vision camera
# coordinate system*. More specifically, they use the camera coordinate system typically used
# in *computer graphics*. In this camera coordinate system, the camera is looking down the
# `-z`-axis, with the `x`-axis pointing to the right and the `y`-axis pointing upwards.
# rotation Pi around the x axis to get the *computer vision camera
# coordinate system*
rotation_around_x = quaternion.quaternion(0.0, 1.0, 0.0, 0.0)
transformation = kapture.PoseTransform(rotation_around_x,
np.array([0, 0, 0]))
images[(i, camera_id)] = file_name
if trajectories is not None:
# transformation.inverse() is equal to transformation (rotation around -Pi or Pi around X is the same)
trajectories[(i, camera_id)] = kapture.PoseTransform.compose(
[transformation, pose, transformation])
if points3d is not None and number_of_points > 0:
assert keypoints is not None
assert observations is not None
offset += number_of_cameras * number_of_lines_per_camera
number_of_lines_per_point = 3 # position color viewlist
# (image_name, bundler_keypoint_id ) -> keypoint_id
known_keypoints = {}
local_keypoints = {}
for i in range(0, number_of_points):
start_index = i * number_of_lines_per_point + offset
position = [float(v) for v in bundler_content[start_index].split()]
# apply transformation
position = [position[0], -position[1], -position[2]]
color = [
float(v) for v in bundler_content[start_index + 1].split()
]
# <view list>: length of the list + [<camera> <key> <x> <y>]
# x, y origin is the center of the image
view_list = bundler_content[start_index + 2].split()
number_of_observations = int(view_list[0])
for j in range(number_of_observations):
camera_id = int(view_list[1 + 4 * j + 0])
keypoint_id = int(view_list[1 + 4 * j + 1])
x = float(view_list[1 + 4 * j + 2])
y = float(view_list[1 + 4 * j + 3])
file_name, width, height = image_mapping[camera_id]
# put (0,0) in upper left corner
x += (width / 2)
y += (height / 2)
# init local_keypoints if needed
if file_name not in local_keypoints:
local_keypoints[file_name] = []
# do not add the same keypoint twice
if (file_name, keypoint_id) not in known_keypoints:
# in the kapture format, keypoint id is different. Note that it starts from 0
known_keypoints[(file_name, keypoint_id)] = len(
local_keypoints[file_name])
local_keypoints[file_name].append([x, y])
keypoint_idx = known_keypoints[(file_name, keypoint_id)]
observations.add(i, file_name, keypoint_idx)
points3d.append(position + color)
points3d = np.array(points3d)
# finally, convert local_keypoints to np.ndarray and add them to the global keypoints variable
keypoints = kapture.Keypoints('sift', np.float32, 2)
for image_filename, keypoints_array in local_keypoints.items():
keypoints_np_array = np.array(keypoints_array).astype(np.float32)
keypoints_out_path = kapture.io.features.get_keypoints_fullpath(
kapture_dir_path, image_filename)
kapture.io.features.image_keypoints_to_file(
keypoints_out_path, keypoints_np_array)
keypoints.add(image_filename)
if points3d is not None:
points3d = kapture.Points3d(points3d)
# import (copy) image files.
logger.info('import image files ...')
filename_list = [f for _, _, f in kapture.flatten(images)]
import_record_data_from_dir_auto(image_dir_path, kapture_dir_path,
filename_list, images_import_method)
# pack into kapture format
imported_kapture = kapture.Kapture(sensors=cameras,
records_camera=images,
trajectories=trajectories,
points3d=points3d,
keypoints=keypoints,
observations=observations)
logger.info('writing imported data...')
kapture_to_dir(kapture_dir_path, imported_kapture)
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')
def import_nvm(nvm_file_path: str,
nvm_images_path: str,
kapture_path: str,
filter_list_path: Optional[str],
ignore_trajectories: bool,
add_reconstruction: bool,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.skip) -> None:
"""
Imports nvm data to kapture format.
:param nvm_file_path: path to nvm file
:param nvm_images_path: path to NVM images directory.
:param kapture_path: path to kapture root directory.
:param filter_list_path: path to the optional file containing a list of images to process
:param ignore_trajectories: if True, will not create trajectories
:param add_reconstruction: if True, will add observations, keypoints and 3D points.
:param force_overwrite_existing: Silently overwrite kapture files if already exists.
:param images_import_method: choose how to import actual image files.
"""
# TODO implement [optional calibration]
# doc : http://ccwu.me/vsfm/doc.html#nvm
os.makedirs(kapture_path, exist_ok=True)
delete_existing_kapture_files(kapture_path, force_erase=force_overwrite_existing)
logger.info('loading all content...')
# if there is a filter list, parse it
# keep it as Set[str] to easily find images
if filter_list_path:
with open(filter_list_path) as file:
file_content = file.readlines()
# remove end line char and empty lines
filter_list = {line.rstrip() for line in file_content if line != '\n'}
else:
filter_list = None
# now do the nvm
with open(nvm_file_path) as file:
nvm_content = file.readlines()
# remove end line char and empty lines
nvm_content = [line.rstrip() for line in nvm_content if line != '\n']
# only NVM_V3 is supported
assert nvm_content[0] == "NVM_V3"
# offset represents the line pointer
offset = 1
# camera_id_offset keeps tracks of used camera_id in case of multiple reconstructed models
camera_id_offset = 0
# point_id_offset keeps tracks of used point_id in case of multiple reconstructed models
point_id_offset = 0
cameras = kapture.Sensors()
images = kapture.RecordsCamera()
trajectories = kapture.Trajectories() if not ignore_trajectories else None
observations = kapture.Observations() if add_reconstruction else None if add_reconstruction else None
keypoints = kapture.Keypoints('sift', np.float32, 2) if add_reconstruction else None
points3d = [] if add_reconstruction else None
# break if number of cameras == 0 or reached end of file
while True:
# <Model1> <Model2> ...
# Each reconstructed <model> contains the following
# <Number of cameras> <List of cameras>
# <Number of 3D points> <List of points>
# In practice,
# <Number of cameras>
# <List of cameras>, one per line
# <Number of 3D points>
# <List of points>, one per line
number_of_cameras = int(nvm_content[offset])
offset += 1
if number_of_cameras == 0: # a line with <0> signify the end of models
break
logger.debug('importing model cameras...')
# parse all cameras for current model
image_idx_to_image_name = parse_cameras(number_of_cameras,
nvm_content,
offset,
camera_id_offset,
filter_list,
nvm_images_path,
cameras,
images,
trajectories)
offset += number_of_cameras
camera_id_offset += number_of_cameras
# parse all points3d
number_of_points = int(nvm_content[offset])
offset += 1
if points3d is not None and number_of_points > 0:
assert keypoints is not None
assert observations is not None
logger.debug('importing model points...')
parse_points3d(kapture_path,
number_of_points,
nvm_content,
offset,
point_id_offset,
image_idx_to_image_name,
filter_list,
points3d,
keypoints,
observations)
point_id_offset += number_of_points
offset += number_of_points
# reached end of file?
if offset >= len(nvm_content):
break
# do not export values if none were found.
if points3d is not None:
points3d = kapture.Points3d(points3d)
# import (copy) image files.
logger.info('import image files ...')
images_filenames = [f for _, _, f in kapture.flatten(images)]
import_record_data_from_dir_auto(nvm_images_path, kapture_path, images_filenames, images_import_method)
# pack into kapture format
imported_kapture = kapture.Kapture(sensors=cameras, records_camera=images, trajectories=trajectories,
points3d=points3d, keypoints=keypoints, observations=observations)
logger.info('writing imported data...')
kapture_to_dir(kapture_path, imported_kapture)
