def setUp(self):
self.gnss_a = kapture.RecordsGnss()
self.gnss_b = kapture.RecordsGnss()
# build 2 gps track using 2 different insertion methods
# gnss_a[timestamp, gps_id] = values and gnss_b[timestamp] = {gps_id: values}
for timestamp in range(3):
gps_snapshots = {}
for gps_id in ['gps1', 'gps2']:
values = np.random.uniform(0., 50., size=(5, )).tolist()
self.gnss_a[timestamp, gps_id] = kapture.RecordGnss(*values)
gps_snapshots[gps_id] = kapture.RecordGnss(*values)
self.gnss_b[timestamp] = gps_snapshots
def records_gnss_from_file(
filepath: str,
gnss_ids: Optional[Set[str]] = None
) -> kapture.RecordsGnss:
"""
Reads RecordsGnss from CSV file.
:param filepath: input file path
:param gnss_ids: input set of valid device ids. Any record of other than the given ones will be ignored.
If omitted, then it loads all devices.
:return: GNSS records
"""
records_gnss = kapture.RecordsGnss()
with open(filepath) as file:
table = table_from_file(file)
# timestamp, device_id, x, y, z, utc, dop
for timestamp, device_id, x, y, z, utc, dop in table:
timestamp = int(timestamp)
device_id = str(device_id)
if gnss_ids is not None and device_id not in gnss_ids:
# just ignore
continue
records_gnss[timestamp, device_id] = kapture.RecordGnss(x, y, z, utc, dop)
return records_gnss
def test_export(self):
temp_kapture_dirpath = path.join(self._tempdir.name, 'kapture')
shutil.copytree(self._kapture_dirpath, temp_kapture_dirpath)
kapture_data = kapture.io.csv.kapture_from_dir(temp_kapture_dirpath)
images_filepaths = images_to_filepaths(kapture_data.records_camera,
temp_kapture_dirpath)
# make sure there is no EXIF in images
for image_filepath in images_filepaths.values():
clear_exif(image_filepath)
# insert gps to exif
export_gps_to_exif(kapture_data=kapture_data,
kapture_dirpath=temp_kapture_dirpath)
rebuilt_records = kapture.RecordsGnss()
for timestamp, cam_id, image_name in kapture.flatten(
kapture_data.records_camera):
image_filepath = get_image_fullpath(temp_kapture_dirpath,
image_name)
exif_data = read_exif(image_filepath)
rebuilt_records[timestamp, 'GPS_' +
cam_id] = convert_gps_to_kapture_record(exif_data)
self.assertTrue(
equal_records_gnss(kapture_data.records_gnss, rebuilt_records))
def _import_gnss(opensfm_root_dir, kapture_sensors, image_sensors, image_timestamps, disable_tqdm) \
-> Optional[kapture.RecordsGnss]:
"""
Imports the GNSS info from the images exif.
"""
# gps from pre-extracted exif, in exif/image_name.jpg.exif
kapture_gnss = None
opensfm_exif_dir_path = path.join(opensfm_root_dir, 'exif')
opensfm_exif_suffix = '.exif'
if path.isdir(opensfm_exif_dir_path):
logger.info('importing GNSS from exif ...')
camera_ids = set(image_sensors.values())
# add a gps sensor for each camera
map_cam_to_gnss_sensor = {
cam_id: 'GPS_' + cam_id
for cam_id in camera_ids
}
for gnss_id in map_cam_to_gnss_sensor.values():
kapture_sensors[gnss_id] = kapture.Sensor(
sensor_type='gnss', sensor_params=['EPSG:4326'])
# build epsg_code for all cameras
kapture_gnss = kapture.RecordsGnss()
opensfm_exif_filepath_list = (
path.join(dir_path, filename)
for dir_path, _, filename_list in os.walk(opensfm_exif_dir_path)
for filename in filename_list
if filename.endswith(opensfm_exif_suffix))
for opensfm_exif_filepath in tqdm(opensfm_exif_filepath_list,
disable=disable_tqdm):
image_filename = path.relpath(
opensfm_exif_filepath,
opensfm_exif_dir_path)[:-len(opensfm_exif_suffix)]
image_timestamp = image_timestamps[image_filename]
image_sensor_id = image_sensors[image_filename]
gnss_timestamp = image_timestamp
gnss_sensor_id = map_cam_to_gnss_sensor[image_sensor_id]
with open(opensfm_exif_filepath, 'rt') as f:
js_root = json.load(f)
if 'gps' not in js_root:
logger.warning(f'NO GPS data in "{opensfm_exif_filepath}"')
continue
gps_coords = {
'x': js_root['gps']['longitude'],
'y': js_root['gps']['latitude'],
'z': js_root['gps'].get('altitude', 0.0),
'dop': js_root['gps'].get('dop', 0),
'utc': 0,
}
logger.debug(
f'found GPS data for ({gnss_timestamp}, {gnss_sensor_id}) in "{opensfm_exif_filepath}"'
)
kapture_gnss[gnss_timestamp,
gnss_sensor_id] = kapture.RecordGnss(**gps_coords)
return kapture_gnss
def test_init_gnss_epsg(self):
gps_id1, gps_id2 = 'gps1', 'gps2'
y, x, z = 51.388920, 30.099134, 15.0
unix_ts = int(datetime(year=1986, month=4, day=26).timestamp())
records_gnss = kapture.RecordsGnss()
records_gnss[0, gps_id1] = kapture.RecordGnss(x + 0, y + 0, z + 0, unix_ts + 0, 9.)
records_gnss[1, gps_id1] = kapture.RecordGnss(x + 1, y + 1, z + 1, unix_ts + 1, 2.)
records_gnss[1, gps_id2] = kapture.RecordGnss(x + 2, y + 2, z + 2, unix_ts + 2, 2.)
records_gnss[2] = {gps_id1: kapture.RecordGnss(x + 3, y + 3, z + 3, unix_ts + 3, 0.)}
self.assertEqual(3, len(records_gnss))
self.assertEqual(4, len(sorted(kapture.flatten(records_gnss))))
self.assertIn((0, gps_id1), records_gnss)
self.assertEqual(30.099134, records_gnss[0, gps_id1].x)
self.assertEqual(51.388920, records_gnss[0, gps_id1].y)
self.assertEqual(15., records_gnss[0, gps_id1].z)
self.assertEqual(9., records_gnss[0, gps_id1].dop)
def extract_gps_from_exif(kapture_data: kapture.Kapture, kapture_dirpath: str):
"""
Extract GPS coordinates from kapture dataset, returns the new sensor and gnss records.
Gnss timestamps and sensor ids are guessed from timestamps and camera_id from images.
The GNSS sensor_id are built prefixing 'GPS_'<cam_id>, with cam_id the sensor_id of the corresponding camera.
:param kapture_data: input kapture data, must contains sensors and records_camera.
:param kapture_dirpath: input path to kapture directory.
:return:
"""
# only load sensors + records_data:
disable_tqdm = logger.getEffectiveLevel() != logging.INFO
# make up new gps ids
cam_to_gps_id = { # cam_id -> gps_id
cam_id: 'GPS_' + cam_id
for cam_id, sensor in kapture_data.sensors.items()
if sensor.sensor_type == 'camera'
} # cam_id -> gps_id
# set all gps to EPSG:4326
gps_epsg_codes = {gps_id: 'EPSG:4326' for gps_id in cam_to_gps_id.values()}
# add new gps ids to sensors
gnss_kapture_sensors = kapture.Sensors()
for gps_id, epsg in gps_epsg_codes.items():
gnss_kapture_sensors[gps_id] = kapture.Sensor(sensor_type='gnss',
sensor_params=[epsg])
image_filepaths = images_to_filepaths(kapture_data.records_camera,
kapture_dirpath=kapture_dirpath)
records_gnss = kapture.RecordsGnss()
for timestamp, cam_id, image_name in tqdm(kapture.flatten(
kapture_data.records_camera),
disable=disable_tqdm):
image_filepath = image_filepaths[image_name]
logger.debug(f'extracting GPS tags from {image_filepath}')
gps_id = cam_to_gps_id[cam_id]
exif_data = read_exif(image_filepath)
gps_record = convert_gps_to_kapture_record(exif_data)
records_gnss[timestamp, gps_id] = gps_record
return gnss_kapture_sensors, records_gnss
def merge_records_gnss(
records_gnss_list: List[Optional[kapture.RecordsGnss]],
sensor_mappings: List[Dict[str, str]]) -> kapture.RecordsGnss:
"""
Merge several gnss records list into one list with new identifiers for the sensors.
:param records_gnss_list: list of gnss records to merge
:param sensor_mappings: mapping of the sensor identifiers to their new identifiers
:return: merged gnss records
"""
assert len(records_gnss_list) > 0
assert len(records_gnss_list) == len(sensor_mappings)
merged_gnss_records = kapture.RecordsGnss()
for gnss_records, sensor_mapping in zip(records_gnss_list,
sensor_mappings):
if gnss_records is None:
continue
for timestamp, sensor_id, record_gnss in kapture.flatten(gnss_records):
new_sensor_id = sensor_mapping[sensor_id]
merged_gnss_records[(timestamp, new_sensor_id)] = record_gnss
return merged_gnss_records
def import_opensfm(
opensfm_rootdir: str,
kapture_rootdir: str,
force_overwrite_existing: bool = False,
images_import_method: TransferAction = TransferAction.copy) -> None:
disable_tqdm = logger.getEffectiveLevel() != logging.INFO
# load reconstruction
opensfm_reconstruction_filepath = path.join(opensfm_rootdir,
'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_rootdir, exist_ok=True)
delete_existing_kapture_files(kapture_rootdir,
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_dirpath = path.join(opensfm_rootdir, '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_dirpath,
destination_kapture_dirpath=kapture_rootdir,
filename_list=filename_list,
copy_strategy=images_import_method)
# gps from pre-extracted exif, in exif/image_name.jpg.exif
kapture_gnss = None
opensfm_exif_dirpath = path.join(opensfm_rootdir, 'exif')
opensfm_exif_suffix = '.exif'
if path.isdir(opensfm_exif_dirpath):
logger.info('importing GNSS from exif ...')
camera_ids = set(image_sensors.values())
# add a gps sensor for each camera
map_cam_to_gnss_sensor = {
cam_id: 'GPS_' + cam_id
for cam_id in camera_ids
}
for gnss_id in map_cam_to_gnss_sensor.values():
kapture_sensors[gnss_id] = kapture.Sensor(
sensor_type='gnss', sensor_params=['EPSG:4326'])
# build epsg_code for all cameras
kapture_gnss = kapture.RecordsGnss()
opensfm_exif_filepath_list = (
path.join(dirpath, filename)
for dirpath, _, filename_list in os.walk(opensfm_exif_dirpath)
for filename in filename_list
if filename.endswith(opensfm_exif_suffix))
for opensfm_exif_filepath in tqdm(opensfm_exif_filepath_list,
disable=disable_tqdm):
image_filename = path.relpath(
opensfm_exif_filepath,
opensfm_exif_dirpath)[:-len(opensfm_exif_suffix)]
image_timestamp = image_timestamps[image_filename]
image_sensor_id = image_sensors[image_filename]
gnss_timestamp = image_timestamp
gnss_sensor_id = map_cam_to_gnss_sensor[image_sensor_id]
with open(opensfm_exif_filepath, 'rt') as f:
js_root = json.load(f)
if 'gps' not in js_root:
logger.warning(f'NO GPS data in "{opensfm_exif_filepath}"')
continue
gps_coords = {
'x': js_root['gps']['longitude'],
'y': js_root['gps']['latitude'],
'z': js_root['gps'].get('altitude', 0.0),
'dop': js_root['gps'].get('dop', 0),
'utc': 0,
}
logger.debug(
f'found GPS data for ({gnss_timestamp}, {gnss_sensor_id}) in "{opensfm_exif_filepath}"'
)
kapture_gnss[gnss_timestamp,
gnss_sensor_id] = kapture.RecordGnss(**gps_coords)
# import features (keypoints + descriptors)
kapture_keypoints = None # kapture.Keypoints(type_name='opensfm', dsize=4, dtype=np.float64)
kapture_descriptors = None # kapture.Descriptors(type_name='opensfm', dsize=128, dtype=np.uint8)
opensfm_features_dirpath = path.join(opensfm_rootdir, 'features')
opensfm_features_suffix = '.features.npz'
if path.isdir(opensfm_features_dirpath):
logger.info('importing keypoints and descriptors ...')
opensfm_features_file_list = (path.join(
dp, fn) for dp, _, fs in os.walk(opensfm_features_dirpath)
for fn in fs)
opensfm_features_file_list = (
filepath for filepath in opensfm_features_file_list
if filepath.endswith(opensfm_features_suffix))
for opensfm_feature_filename in tqdm(opensfm_features_file_list,
disable=disable_tqdm):
image_filename = path.relpath(
opensfm_feature_filename,
opensfm_features_dirpath)[:-len(opensfm_features_suffix)]
opensfm_image_features = np.load(opensfm_feature_filename)
opensfm_image_keypoints = opensfm_image_features['points']
opensfm_image_descriptors = opensfm_image_features['descriptors']
logger.debug(
f'parsing keypoints and descriptors in {opensfm_feature_filename}'
)
if kapture_keypoints is None:
# print(type(opensfm_image_keypoints.dtype))
# HAHOG = Hessian Affine feature point detector + HOG descriptor
kapture_keypoints = kapture.Keypoints(
type_name='HessianAffine',
dsize=opensfm_image_keypoints.shape[1],
dtype=opensfm_image_keypoints.dtype)
if kapture_descriptors is None:
kapture_descriptors = kapture.Descriptors(
type_name='HOG',
dsize=opensfm_image_descriptors.shape[1],
dtype=opensfm_image_descriptors.dtype)
# convert keypoints file
keypoint_filpath = kapture.io.features.get_features_fullpath(
data_type=kapture.Keypoints,
kapture_dirpath=kapture_rootdir,
image_filename=image_filename)
kapture.io.features.image_keypoints_to_file(
filepath=keypoint_filpath,
image_keypoints=opensfm_image_keypoints)
# register the file
kapture_keypoints.add(image_filename)
# convert descriptors file
descriptor_filpath = kapture.io.features.get_features_fullpath(
data_type=kapture.Descriptors,
kapture_dirpath=kapture_rootdir,
image_filename=image_filename)
kapture.io.features.image_descriptors_to_file(
filepath=descriptor_filpath,
image_descriptors=opensfm_image_descriptors)
# register the file
kapture_descriptors.add(image_filename)
# import matches
kapture_matches = kapture.Matches()
opensfm_matches_suffix = '_matches.pkl.gz'
opensfm_matches_dirpath = path.join(opensfm_rootdir, 'matches')
if path.isdir(opensfm_matches_dirpath):
logger.info('importing matches ...')
opensfm_matches_file_list = (path.join(
dp, fn) for dp, _, fs in os.walk(opensfm_matches_dirpath)
for fn in fs)
opensfm_matches_file_list = (
filepath for filepath in opensfm_matches_file_list
if filepath.endswith(opensfm_matches_suffix))
for opensfm_matches_filename in tqdm(opensfm_matches_file_list,
disable=disable_tqdm):
image_filename_1 = path.relpath(
opensfm_matches_filename,
opensfm_matches_dirpath)[:-len(opensfm_matches_suffix)]
logger.debug(f'parsing mathes in {image_filename_1}')
with gzip.open(opensfm_matches_filename, 'rb') as f:
opensfm_matches = pickle.load(f)
for image_filename_2, opensfm_image_matches in opensfm_matches.items(
):
image_pair = (image_filename_1, image_filename_2)
# register the pair to kapture
kapture_matches.add(*image_pair)
# convert the bin file to kapture
kapture_matches_filepath = kapture.io.features.get_matches_fullpath(
image_filename_pair=image_pair,
kapture_dirpath=kapture_rootdir)
kapture_image_matches = np.hstack([
opensfm_image_matches.astype(np.float64),
# no macthes scoring = assume all to one
np.ones(shape=(opensfm_image_matches.shape[0], 1),
dtype=np.float64)
])
kapture.io.features.image_matches_to_file(
kapture_matches_filepath, kapture_image_matches)
# 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.io.csv.kapture_to_dir(dirpath=kapture_rootdir,
kapture_data=kapture_data)