def setUp(self):
self._rigs = kapture.Rigs()
looking_not_straight = quaternion.from_rotation_vector(
[0, np.deg2rad(5.), 0])
self._rigs['rig', 'cam1'] = kapture.PoseTransform(
t=[-10, 0, 0], r=looking_not_straight).inverse()
self._rigs['rig', 'cam2'] = kapture.PoseTransform(
t=[+10, 0, 0], r=looking_not_straight.inverse()).inverse()
self._trajectories_rigs = kapture.Trajectories()
for timestamp, ratio in enumerate(np.linspace(0., 1., num=8)):
looking_around = quaternion.from_rotation_vector(
[0, np.deg2rad(360. * ratio), 0])
self._trajectories_rigs[timestamp, 'rig'] = kapture.PoseTransform(
t=[0, 0, -100.], r=looking_around)
self._trajectories_cams = kapture.Trajectories()
for timestamp, rig_id, pose_rig_from_world in kapture.flatten(
self._trajectories_rigs, is_sorted=True):
for rig_id2, cam_id, pose_cam_from_rig in kapture.flatten(
self._rigs):
assert rig_id == rig_id
pose_cam_from_world = kapture.PoseTransform.compose(
[pose_cam_from_rig, pose_rig_from_world])
self._trajectories_cams[timestamp,
cam_id] = pose_cam_from_world
def get_interpolated_pose(kdata_map: kapture.Kapture, kdata_query: kapture.Kapture,
weights: Dict[str, List[Tuple[str, float]]]):
"""
compute the approximated pose for all query images given the precomputed weights
:param kdata_map: map images + their poses as kapture data
:type kdata_map: kapture.Kapture
:param kdata_query: query images as kapture data
:type kdata_query: kapture.Kapture
:param weights: weights for the pose interpolation
:type weights: Dict[str, List[Tuple[str, float]]]
"""
output_trajectories = kapture.Trajectories()
assert kdata_map.trajectories is not None
assert kdata_map.records_camera is not None
reverse_map_records_camera = {image_name: (timestamp, sensor_id)
for timestamp, sensor_id, image_name in kapture.flatten(kdata_map.records_camera)}
if kdata_map.rigs is not None:
input_trajectories = kapture.rigs_remove(kdata_map.trajectories, kdata_map.rigs)
else:
input_trajectories = kdata_map.trajectories
assert kdata_query.records_camera is not None
reverse_query_records_camera = {image_name: (timestamp, sensor_id)
for timestamp, sensor_id, image_name in kapture.flatten(kdata_query.records_camera)}
for query_image_name, weighted_map_images in weights.items():
pose_inv_list = [input_trajectories[reverse_map_records_camera[name]].inverse()
for name, _ in weighted_map_images]
weight_list = [w for _, w in weighted_map_images]
output_trajectories[reverse_query_records_camera[query_image_name]] = average_pose_transform_weighted(
pose_inv_list,
weight_list
).inverse()
return output_trajectories
def _import_trajectories(silda_dir_path, image_name_to_ids,
hide_progress_bars) -> kapture.Trajectories:
logger.info('Processing trajectories ...')
trajectories = kapture.Trajectories()
with open(path.join(silda_dir_path, 'silda-train-poses.txt')) as file:
lines = file.readlines()
lines = (line.rstrip().split() for line in lines)
extrinsics = {
line[0]: np.array(line[1:8], dtype=np.float)
for line in lines
}
for silda_image_name, pose_params in tqdm(extrinsics.items(),
disable=hide_progress_bars):
# Silda poses are 7-dim vectors with the rotation quaternion,
# and the translation vector. The order needs to be:
# qw,qx,qy,qz,tx,ty,tz
# The parameters should be described in terms of camera to world transformations
if silda_image_name not in image_name_to_ids:
# if this is not referenced: means its part of the corpus to be ignored.
continue
pose = kapture.PoseTransform(pose_params[0:4],
pose_params[4:7]).inverse()
timestamp, cam_id = image_name_to_ids[silda_image_name]
trajectories[timestamp, cam_id] = pose
# if query, trajectories is empty, so juste do not save it
if len(trajectories) == 0:
trajectories = None
return trajectories
def test_type_checking(self):
traj = kapture.Trajectories()
valid_ts, valid_id, valid_pose = 0, 'cam0', kapture.PoseTransform()
invalid_ts, invalid_id, invalid_pose = '0', float(0), 'pose'
self.assertRaises(TypeError, traj.__setitem__, (invalid_ts, valid_id),
valid_pose)
self.assertRaises(TypeError, traj.__setitem__, (valid_ts, invalid_id),
valid_pose)
self.assertRaises(TypeError, traj.__setitem__, (valid_ts, valid_id),
invalid_pose)
self.assertRaises(TypeError, traj.__setitem__,
(invalid_ts, invalid_id), invalid_pose)
self.assertRaises(TypeError, traj.__setitem__, invalid_ts,
{valid_id: valid_pose})
self.assertRaises(TypeError, traj.__setitem__, valid_ts,
{invalid_id: valid_pose})
self.assertRaises(TypeError, traj.__setitem__, valid_ts,
{valid_id: invalid_pose})
self.assertRaises(TypeError, traj.__setitem__, invalid_ts, valid_pose)
self.assertRaises(TypeError, traj.__contains__, invalid_ts, valid_id)
self.assertRaises(TypeError, traj.__contains__, valid_ts, invalid_id)
self.assertRaises(TypeError, traj.__contains__, invalid_ts, invalid_id)
self.assertRaises(TypeError, traj.__delitem__, invalid_ts)
self.assertRaises(TypeError, traj.__delitem__, (valid_ts, invalid_id))
def test_rig_remove(self):
rigs = kapture.Rigs()
rigs['rig0', 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[100, 0, 0])
rigs['rig0', 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[-100, 0, 0])
trajectories = kapture.Trajectories()
trajectories[0, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 0])
trajectories[1, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 10])
trajectories[2, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 20])
trajectories_ = kapture.rigs_remove(trajectories, rigs)
# timestamps should be unchanged
self.assertEqual(trajectories_.keys(), trajectories.keys())
self.assertNotEqual(trajectories_.key_pairs(),
trajectories.key_pairs())
self.assertEqual(len(trajectories_.key_pairs()),
len(trajectories.key_pairs()) * len(rigs.key_pairs()))
self.assertIn((0, 'cam0'), trajectories_.key_pairs())
self.assertIn((0, 'cam1'), trajectories_.key_pairs())
self.assertIn((2, 'cam0'), trajectories_.key_pairs())
self.assertIn((2, 'cam1'), trajectories_.key_pairs())
self.assertAlmostEqual(trajectories_[2, 'cam1'].t_raw,
[-100.0, 0.0, 20.0])
self.assertAlmostEqual(trajectories_[2, 'cam1'].r_raw,
[1.0, 0.0, 0.0, 0.0])
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_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 merge_trajectories(
trajectories_list: List[Optional[kapture.Trajectories]],
rig_mappings: List[Dict[str, str]],
sensor_mappings: List[Dict[str, str]]) -> kapture.Trajectories:
"""
Merge several trajectories list into one list with new identifiers for the rigs and the sensors.
:param trajectories_list: list of trajectories to merge
:param rig_mappings: mapping of the rigs identifiers to their new identifiers
:param sensor_mappings: mapping of the sensor identifiers to their new identifiers
:return: merged trajectories
"""
assert len(trajectories_list) > 0
assert len(trajectories_list) == len(rig_mappings)
assert len(trajectories_list) == len(sensor_mappings)
merged_trajectories = kapture.Trajectories()
for trajectories, rig_mapping, sensor_mapping in zip(
trajectories_list, rig_mappings, sensor_mappings):
if trajectories is None:
continue
for timestamp, sensor_id, pose in kapture.flatten(trajectories):
if sensor_id in rig_mapping:
new_sensor_id = rig_mapping[sensor_id]
else:
new_sensor_id = sensor_mapping[sensor_id]
merged_trajectories[(timestamp, new_sensor_id)] = pose
return merged_trajectories
def test_trajectories_write(self):
pose1 = kapture.PoseTransform(r=[1.0, 0.0, 0.0, 0.0],
t=[0.0, 0.0, 0.0])
pose2 = kapture.PoseTransform(r=[0.5, 0.5, 0.5, 0.5], t=[4., 2., -2.])
content_expected = [
csv.KAPTURE_FORMAT_1 + kapture_linesep,
'# timestamp, device_id, qw, qx, qy, qz, tx, ty, tz' +
kapture_linesep,
' 0, cam0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0' +
kapture_linesep,
' 0, cam1, 0.5, 0.5, 0.5, 0.5, 4.0, 2.0, -2.0' +
kapture_linesep,
' 100, cam2, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0' +
kapture_linesep
]
trajectories = kapture.Trajectories()
timestamp1, timestamp2 = 0, 100
sensor_id1, sensor_id2, sensor_id3 = 'cam0', 'cam1', 'cam2'
trajectories[(timestamp1, sensor_id1)] = pose1
trajectories[(timestamp1, sensor_id2)] = pose2
trajectories[(timestamp2, sensor_id3)] = pose1
csv.trajectories_to_file(self._temp_filepath, trajectories)
with open(self._temp_filepath, 'rt') as f:
content_actual = f.readlines()
self.assertListEqual(content_actual, content_expected)
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 test_import(self):
colmap_rigs = [{
"cameras": [{
"camera_id": 0,
"image_prefix": "camL"
}, {
"camera_id": 1,
"image_prefix": "camR"
}],
"ref_camera_id":
0
}]
rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json(
rigs_colmap=colmap_rigs)
self.assertEqual([('rig0', get_camera_kapture_id_from_colmap_id(0)),
('rig0', get_camera_kapture_id_from_colmap_id(1))],
rigs_kapture.key_pairs())
self.assertIsNone(reconstructed_images)
self.assertIsNone(reconstructed_trajectories)
# the records
images = kapture.RecordsCamera()
images[0, get_camera_kapture_id_from_colmap_id(0)] = 'camL/0000.jpg'
images[1, get_camera_kapture_id_from_colmap_id(1)] = 'camR/0000.jpg'
images[2, get_camera_kapture_id_from_colmap_id(0)] = 'camL/0001.jpg'
images[3, get_camera_kapture_id_from_colmap_id(1)] = 'camR/0001.jpg'
rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json(
rigs_colmap=colmap_rigs, images=images)
# check timestamps has been recovered.
self.assertEqual([(0, get_camera_kapture_id_from_colmap_id(0)),
(0, get_camera_kapture_id_from_colmap_id(1)),
(1, get_camera_kapture_id_from_colmap_id(0)),
(1, get_camera_kapture_id_from_colmap_id(1))],
reconstructed_images.key_pairs())
# trajectories
trajectories = kapture.Trajectories()
trajectories[
0,
get_camera_kapture_id_from_colmap_id(0)] = kapture.PoseTransform()
trajectories[
1,
get_camera_kapture_id_from_colmap_id(1)] = kapture.PoseTransform()
trajectories[
2,
get_camera_kapture_id_from_colmap_id(0)] = kapture.PoseTransform()
trajectories[
3,
get_camera_kapture_id_from_colmap_id(1)] = kapture.PoseTransform()
rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json(
rigs_colmap=colmap_rigs, images=images, trajectories=trajectories)
self.assertEqual([(0, get_camera_kapture_id_from_colmap_id(0)),
(0, get_camera_kapture_id_from_colmap_id(1)),
(1, get_camera_kapture_id_from_colmap_id(0)),
(1, get_camera_kapture_id_from_colmap_id(1))],
reconstructed_trajectories.key_pairs())
def import_extended_cmu_seasons_images(
image_list_file_path: str
) -> Tuple[kapture.RecordsCamera, kapture.Trajectories]:
"""
Read image list, name.jpg or name.jpg qw qx qy qz cx cy cz
:param image_list_file_path: path to the image list file
:return: kapture images and trajectories
"""
records_camera = kapture.RecordsCamera()
trajectories = kapture.Trajectories()
# name.jpg qw qx qy qz cx cy cz
# or
# name.jpg
with open(image_list_file_path) as fin:
table = fin.readlines()
# remove comment lines
table = (line for line in table if not line.startswith('#'))
# remove empty lines
table = (line for line in table if line.strip())
# trim trailing EOL
table = (line.rstrip("\n\r") for line in table)
# split space
table = (re.split(r'\s+', line) for line in table)
# remove empty split
table = ([s for s in line if s] for line in table)
image_pattern = re.compile(ECMU_IMAGE_PATTERN)
for line in table:
image_name = line[0]
timestamp, camera_id = _parse_image_name(image_name, image_pattern)
if camera_id is None or timestamp is None:
continue
records_camera[(timestamp, camera_id)] = image_name
if len(line) > 1: # also contains trajectory
qw, qx, qy, qz, cx, cy, cz = line[1:]
quaternion_array = float_array_or_none([qw, qx, qy, qz])
assert quaternion_array is not None
center_array = float_array_or_none([cx, cy, cz])
assert center_array is not None
rotation = quaternion.from_float_array(quaternion_array)
# C = -R^T * t -> t = -R * C
translation = np.matmul(
quaternion.as_rotation_matrix(rotation),
-1 * np.array(center_array, dtype=np.float))
pose = kapture.PoseTransform(r=rotation, t=translation)
trajectories[(timestamp, camera_id)] = pose
# if no trajectories were added (query), prefer None
if not trajectories:
trajectories = None
return records_camera, trajectories
def test_timestamps_length(self):
trajectories = kapture.Trajectories()
self.assertEqual(trajectories.timestamp_length(), -1)
trajectories[1614362592378, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20])
trajectories[1614362592634, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0])
trajectories[1614362592378, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20])
trajectories[1614362593123, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 30])
self.assertEqual(trajectories.timestamp_length(), 13)
# Check that if we have timestamps of different precision, we can not compute a common length
trajectories[1614362594, 'lidar0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0])
self.assertEqual(trajectories.timestamp_length(), -1)
def test_pose_interpolation(self):
trajectories = kapture.Trajectories()
trajectories[1614362592000,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 0])
trajectories[1614362592000,
'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 10, 0])
trajectories[1614362592500,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 10])
trajectories[1614362593000,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 20])
trajectories[1614362593000,
'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 10, 20])
trajectories[1614362593500,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 30])
trajectories[1614362594000,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 40])
trajectories[1614362594500,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 50])
trajectories[1614362595000,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 60])
trajectories[1614362595500,
'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0],
t=[0, 0, 70])
self.assertEqual(trajectories.timestamp_length(), 13)
pose = trajectories.intermediate_pose(1614362592500, 'cam2', 1000000)
self.assertIsNone(pose, "unknown device")
pose = trajectories.intermediate_pose(1614362593000, 'cam0', 1000000)
self.assertEqual(pose,
kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]),
"existing pose")
pose = trajectories.intermediate_pose(1614362500000, 'cam0', 1000000)
self.assertIsNone(pose, "time too far in past")
pose = trajectories.intermediate_pose(1614362600000, 'cam0', 1000000)
self.assertIsNone(pose, "time too far in future")
pose = trajectories.intermediate_pose(1614362595250, 'cam0', 1000000)
self.assertEqual(pose,
kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 65]))
pose = trajectories.intermediate_pose(1614362592500, 'cam1', 1000000)
self.assertEqual(pose,
kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 10, 10]))
pose = trajectories.intermediate_pose(1614362595250, 'cam1', 1000000)
self.assertIsNone(pose, "not enough pose for cam1")
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 test_remove(self):
trajectories = kapture.Trajectories()
trajectories[0, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0])
trajectories[1, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10])
trajectories[2, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20])
trajectories[2, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20])
self.assertEqual(len(trajectories), 3)
self.assertEqual(len(trajectories[2]), 2)
del trajectories[2, 'cam0']
self.assertEqual(len(trajectories), 3)
self.assertEqual(len(trajectories[2]), 1)
del trajectories[1]
self.assertEqual(len(trajectories), 2)
del trajectories[2, 'cam1']
self.assertEqual(len(trajectories), 1)
def test_timestamps_list(self):
trajectories = kapture.Trajectories()
trajectories[2, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20])
trajectories[0, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0])
trajectories[2, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20])
self.assertEqual(trajectories.timestamps_sorted_list(), [0, 2])
trajectories[1, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10])
self.assertEqual(len(trajectories), 3)
self.assertEqual(trajectories.timestamps_sorted_list(), [0, 1, 2])
del trajectories[2, 'cam0']
self.assertEqual(trajectories.timestamps_sorted_list(), [0, 1, 2])
del trajectories[1]
self.assertEqual(trajectories.timestamps_sorted_list(), [0, 2])
del trajectories[2, 'cam1']
self.assertEqual(trajectories.timestamps_sorted_list(), [0, ])
def sub_kapture_from_img_list(kdata, kdata_path, img_list, pairs):
trajectories = kapture.Trajectories()
sensors = kapture.Sensors()
records = kapture.RecordsCamera()
keypoints = kapture.Keypoints(kdata.keypoints._tname,
kdata.keypoints._dtype,
kdata.keypoints._dsize)
if kdata.descriptors != None:
descriptors = kapture.Descriptors(kdata.descriptors._tname,
kdata.descriptors._dtype,
kdata.descriptors._dsize)
else:
descriptors = None
matches = kapture.Matches()
timestamp_sensor_id_from_image_name = {
img_name: (timestamp, sensor_id)
for timestamp, sensor_id, img_name in kapture.flatten(
kdata.records_camera)
}
for img in img_list:
timestamp, sensor_id = timestamp_sensor_id_from_image_name[img]
pose = kdata.trajectories[timestamp][sensor_id]
sensors[sensor_id] = kdata.sensors[sensor_id]
records[timestamp, sensor_id] = img
trajectories[timestamp, sensor_id] = pose
keypoints.add(img)
if kdata.descriptors != None:
descriptors.add(img)
for i in pairs:
image_matches_filepath = get_matches_fullpath((i[0], i[1]), kdata_path)
if os.path.exists(image_matches_filepath):
matches.add(i[0], i[1])
matches.normalize()
return kapture.Kapture(sensors=sensors,
trajectories=trajectories,
records_camera=records,
descriptors=descriptors,
keypoints=keypoints,
matches=matches)
def merge_trajectories(
trajectories_list: List[Optional[kapture.Trajectories]]
) -> kapture.Trajectories:
"""
Merge several trajectories lists. For trajectory point with the same timestamp and sensor identifier,
keep only the first one.
:param trajectories_list: list of trajectories
:return: merged trajectories
"""
assert len(trajectories_list) > 0
merged_trajectories = kapture.Trajectories()
for trajectories in trajectories_list:
if trajectories is None:
continue
for timestamp, sensor_id, pose in kapture.flatten(trajectories):
if (timestamp, sensor_id) in merged_trajectories:
continue
merged_trajectories[(timestamp, sensor_id)] = pose
return merged_trajectories
def get_images_and_trajectories_from_database(
database: COLMAPDatabase
) -> Tuple[kapture.RecordsCamera, kapture.Trajectories]:
"""
Creates records_camera and trajectories from colmap images table
In trajectories, timestamps are made up from colmap image id.
:param database: colmap database
:return: kapture records_camera and trajectories
"""
logging.info('parsing images ...')
kapture_images = kapture.RecordsCamera()
kapture_trajectories = kapture.Trajectories()
hide_progressbar = logger.getEffectiveLevel() > logging.INFO
for image_id, name, camera_id, prior_qw, prior_qx, prior_qy, prior_qz, prior_tx, prior_ty, prior_tz \
in tqdm(database.execute('SELECT image_id, name, camera_id, '
'prior_qw, prior_qx, prior_qy, prior_qz, '
'prior_tx, prior_ty, prior_tz FROM images;'
), disable=hide_progressbar):
# images
timestamp = int(image_id)
camera_id = get_camera_kapture_id_from_colmap_id(camera_id)
kapture_images[timestamp, camera_id] = name
# trajectories
prior_q = [prior_qw, prior_qx, prior_qy, prior_qz]
prior_t = [prior_tx, prior_ty, prior_tz]
# do not register the pose part if its invalid.
is_undefined = all(v is None for v in prior_q + prior_t)
if is_undefined:
# just ignore
continue
prior_pose = kapture.PoseTransform(prior_q, prior_t)
kapture_trajectories[timestamp, camera_id] = prior_pose
if len(kapture_trajectories) == 0:
# if there is no pose at all, just don't bother.
kapture_trajectories = None
return kapture_images, kapture_trajectories
def sub_kapture_from_img_list(kdata, img_list, pairs, keypoints_type, descriptors_type):
trajectories = kapture.Trajectories()
sensors = kapture.Sensors()
records = kapture.RecordsCamera()
keypoints = kapture.Keypoints(kdata.keypoints[keypoints_type].type_name,
kdata.keypoints[keypoints_type].dtype,
kdata.keypoints[keypoints_type].dsize)
if kdata.descriptors is not None and descriptors_type in kdata.descriptors:
descriptors = kapture.Descriptors(kdata.descriptors[descriptors_type].type_name,
kdata.descriptors[descriptors_type].dtype,
kdata.descriptors[descriptors_type].dsize,
kdata.descriptors[descriptors_type].keypoints_type,
kdata.descriptors[descriptors_type].metric_type)
else:
descriptors = None
matches = kapture.Matches()
timestamp_sensor_id_from_image_name = {img_name: (timestamp, sensor_id) for timestamp, sensor_id, img_name in
kapture.flatten(kdata.records_camera)}
for img in img_list:
timestamp, sensor_id = timestamp_sensor_id_from_image_name[img]
sensors[sensor_id] = kdata.sensors[sensor_id]
records[timestamp, sensor_id] = img
if (timestamp, sensor_id) in kdata.trajectories:
pose = kdata.trajectories[timestamp][sensor_id]
trajectories[timestamp, sensor_id] = pose
keypoints.add(img)
if kdata.descriptors is not None:
descriptors.add(img)
for i in pairs:
if i in kdata.matches[keypoints_type]:
matches.add(i[0], i[1])
matches.normalize()
return kapture.Kapture(sensors=sensors, trajectories=trajectories, records_camera=records,
descriptors={descriptors_type: descriptors},
keypoints={keypoints_type: keypoints},
matches={keypoints_type: matches})
def _import_trajectories(input_json, device_identifiers, timestamp_for_pose):
trajectories = kapture.Trajectories()
if input_json.get(EXTRINSICS):
extrinsics = input_json[EXTRINSICS]
logger.info(f'Importing {len(extrinsics)} extrinsics -> trajectories')
for pose in extrinsics:
pose_id = pose[KEY]
center = pose[VALUE][CENTER]
rotation = pose[VALUE][ROTATION]
kap_translation = -1 * np.matmul(rotation, center)
kap_pose = kapture.PoseTransform(
quaternion.from_rotation_matrix(rotation), kap_translation)
timestamp = timestamp_for_pose.get(pose_id)
if timestamp is None:
logger.warning(f'Missing timestamp for extrinsic {pose_id}')
continue
device_id = device_identifiers.get(pose_id)
if device_id is None:
logger.warning(f'Missing device for extrinsic {pose_id}')
continue
trajectories[(timestamp,
device_id)] = kap_pose # tuple of int,str -> 6D pose
return trajectories
def test_init(self):
timestamp1 = 0
timestamp2 = 1
device_id = 'cam0'
pose = kapture.PoseTransform()
traj = kapture.Trajectories()
# pair assignment
traj[(timestamp1, device_id)] = pose
# dict assignment
traj[timestamp2] = {device_id: pose}
self.assertEqual(2, len(traj))
self.assertEqual(traj[timestamp1], traj[timestamp2])
self.assertEqual(traj[(timestamp1, device_id)],
traj[(timestamp2, device_id)])
# test __contains__
self.assertIn(timestamp1, traj)
self.assertIn((timestamp1, device_id), traj)
self.assertIn(timestamp2, traj)
self.assertIn((timestamp1, device_id), traj)
self.assertNotIn((timestamp1, 'cam1'), traj)
self.assertNotIn((2, device_id), traj)
def trajectories_from_file(filepath: str, device_ids: Optional[Set[str]] = None) -> kapture.Trajectories:
"""
Reads trajectories from CSV file.
:param filepath: input file path
:param device_ids: input set of valid device ids (rig or sensor).
If the trajectories contains unknown devices, they will be ignored.
If no device_ids given, everything is loaded.
:return: trajectories
"""
trajectories = kapture.Trajectories()
with open(filepath) as file:
table = table_from_file(file)
# timestamp, device_id, qw, qx, qy, qz, tx, ty, tz
for timestamp, device_id, qw, qx, qy, qz, tx, ty, tz in table:
if device_ids is not None and device_id not in device_ids:
# just ignore
continue
rotation = float_array_or_none([qw, qx, qy, qz])
trans = float_array_or_none([tx, ty, tz])
pose = kapture.PoseTransform(rotation, trans)
trajectories[(int(timestamp), str(device_id))] = pose
return trajectories
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_robotcar_colmap_location(
robotcar_path: str, colmap_reconstruction_fullpath: path,
kapture_path: str, rigs: kapture.Rigs,
skip_reconstruction: bool) -> kapture.Kapture:
"""
Import robotcar data for one location from colmap reconstruction
:param robotcar_path: path to the robotcar top directory
:param colmap_reconstruction_fullpath: path to the colmap reconstruction directory
:param kapture_path: path to the kapture top directory
:param rigs: kapture rigs to modify
:param skip_reconstruction: if True, will not add the reconstruction
:return: a kapture object
"""
# First, import Colmap reconstruction for given location
kapture_data = import_colmap(
kapture_dirpath=kapture_path,
colmap_reconstruction_dirpath=colmap_reconstruction_fullpath,
colmap_images_dirpath=path.join(robotcar_path, "images"),
skip_reconstruction=skip_reconstruction,
images_import_strategy=TransferAction.skip
) # since filenames are incorrect
# Post processing:
# - use correct names for cameras
# - model was built with PNG files, but we have JPG
# - recover proper timestamps
# - recover rig
# Fix sensors.txt
camera_mapping = {
'cam_00001': 'left',
'cam_00002': 'rear',
'cam_00003': 'right'
}
new_cameras = kapture.Sensors()
for cam_id in kapture_data.sensors:
new_cameras[camera_mapping[cam_id]] = kapture_data.sensors[cam_id]
kapture_data.sensors = new_cameras
if not skip_reconstruction:
# Fix keypoints
# Need to rename .png.kpt to .jpg.kpt files and that's all
for root, dirs, files in os.walk(kapture_path):
for file in files:
if file.endswith('.png.kpt'):
os.rename(
path.join(root, file),
path.join(root, file.replace(".png.kpt", ".jpg.kpt")))
# observations.txt: png -> jpg
new_observations = kapture.Observations()
for point3d_idx in kapture_data.observations:
for image_path, keypoint_id in kapture_data.observations[
point3d_idx]:
new_observations.add(point3d_idx,
image_path.replace(".png", ".jpg"),
int(keypoint_id))
kapture_data.observations = new_observations
# records_camera.txt
# timestamps, png->jpg
new_records_camera = kapture.RecordsCamera()
records_camera_pattern = re.compile(r'.*/(?P<timestamp>\d+)\.png')
ts_mapping = {}
for ts, shot in kapture_data.records_camera.items():
for cam_id, image_path in shot.items():
matches = records_camera_pattern.match(image_path)
if not matches:
continue
matches = matches.groupdict()
timestamp = int(matches['timestamp'])
ts_mapping[ts] = timestamp
new_path = image_path.replace(".png", ".jpg")
new_records_camera[timestamp, camera_mapping[cam_id]] = new_path
kapture_data.records_camera = new_records_camera
# trajectories.txt
new_trajectories = kapture.Trajectories()
# First recover timestamps and camera names
for ts, sensor_id in sorted(kapture_data.trajectories.key_pairs()):
new_trajectories[
ts_mapping[ts],
camera_mapping[sensor_id]] = kapture_data.trajectories[ts,
sensor_id]
kapture_data.trajectories = new_trajectories
kapture_data.rigs = rigs
return 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
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 colmap_localize_from_loaded_data(kapture_data: kapture.Kapture,
kapture_path: str,
tar_handlers: Optional[TarCollection],
colmap_path: str,
input_database_path: str,
input_reconstruction_path: str,
colmap_binary: str,
keypoints_type: Optional[str],
use_colmap_matches_importer: bool,
image_registrator_options: List[str],
skip_list: List[str],
force: bool) -> None:
"""
Localize images on a colmap model with the kapture data.
:param kapture_data: kapture data to use
:param kapture_path: path to the kapture to use
:param tar_handler: collection of preloaded tar archives
:param colmap_path: path to the colmap build
:param input_database_path: path to the map colmap.db
:param input_database_path: path to the map colmap.db
:param input_reconstruction_path: path to the map reconstruction folder
:param colmap_binary: path to the colmap binary executable
:param keypoints_type: type of keypoints, name of the keypoints subfolder
:param use_colmap_matches_importer: bool,
:param image_registrator_options: options for the image registrator
:param skip_list: list of steps to skip
:param force: Silently overwrite kapture files if already exists.
"""
os.makedirs(colmap_path, exist_ok=True)
if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches):
raise ValueError('records_camera, sensors, keypoints, matches are mandatory')
if kapture_data.trajectories:
logger.warning("Input data contains trajectories: they will be ignored")
kapture_data.trajectories.clear()
else:
kapture_data.trajectories = kapture.Trajectories()
# COLMAP does not fully support rigs.
if kapture_data.rigs is not None and kapture_data.trajectories is not None:
# make sure, rigs are not used in trajectories.
logger.info('remove rigs notation.')
rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs)
kapture_data.rigs.clear()
# Prepare output
# Set fixed name for COLMAP database
colmap_db_path = path.join(colmap_path, 'colmap.db')
image_list_path = path.join(colmap_path, 'images.list')
reconstruction_path = path.join(colmap_path, "reconstruction")
if 'delete_existing' not in skip_list:
safe_remove_file(colmap_db_path, force)
safe_remove_file(image_list_path, force)
safe_remove_any_path(reconstruction_path, force)
os.makedirs(reconstruction_path, exist_ok=True)
# Copy colmap db to output
if not os.path.exists(colmap_db_path):
shutil.copy(input_database_path, colmap_db_path)
# find correspondences between the colmap db and the kapture data
images_all = {image_path: (ts, cam_id)
for ts, shot in kapture_data.records_camera.items()
for cam_id, image_path in shot.items()}
colmap_db = COLMAPDatabase.connect(colmap_db_path)
colmap_image_ids = database_extra.get_colmap_image_ids_from_db(colmap_db)
colmap_images = database_extra.get_images_from_database(colmap_db)
colmap_db.close()
# dict ( kapture_camera -> colmap_camera_id )
colmap_camera_ids = {images_all[image_path][1]: colmap_cam_id
for image_path, colmap_cam_id in colmap_images if image_path in images_all}
images_to_add = {image_path: value
for image_path, value in images_all.items()
if image_path not in colmap_image_ids}
flatten_images_to_add = [(ts, kapture_cam_id, image_path)
for image_path, (ts, kapture_cam_id) in images_to_add.items()]
if 'import_to_db' not in skip_list:
logger.info("Step 1: Add precomputed keypoints and matches to colmap db")
if keypoints_type is None:
keypoints_type = try_get_only_key_from_collection(kapture_data.keypoints)
assert keypoints_type is not None
assert keypoints_type in kapture_data.keypoints
assert keypoints_type in kapture_data.matches
cameras_to_add = kapture.Sensors()
for _, (_, kapture_cam_id) in images_to_add.items():
if kapture_cam_id not in colmap_camera_ids:
kapture_cam = kapture_data.sensors[kapture_cam_id]
cameras_to_add[kapture_cam_id] = kapture_cam
colmap_db = COLMAPDatabase.connect(colmap_db_path)
colmap_added_camera_ids = database_extra.add_cameras_to_database(cameras_to_add, colmap_db)
colmap_camera_ids.update(colmap_added_camera_ids)
colmap_added_image_ids = database_extra.add_images_to_database_from_flatten(
colmap_db, flatten_images_to_add, kapture_data.trajectories, colmap_camera_ids)
colmap_image_ids.update(colmap_added_image_ids)
colmap_image_ids_reversed = {v: k for k, v in colmap_image_ids.items()} # colmap_id : name
# add new features
colmap_keypoints = database_extra.get_keypoints_set_from_database(colmap_db, colmap_image_ids_reversed)
keypoints_all = kapture_data.keypoints[keypoints_type]
keypoints_to_add = {name for name in keypoints_all if name not in colmap_keypoints}
keypoints_to_add = kapture.Keypoints(keypoints_all.type_name, keypoints_all.dtype, keypoints_all.dsize,
keypoints_to_add)
database_extra.add_keypoints_to_database(colmap_db, keypoints_to_add,
keypoints_type, kapture_path,
tar_handlers,
colmap_image_ids)
# add new matches
colmap_matches = kapture.Matches(database_extra.get_matches_set_from_database(colmap_db,
colmap_image_ids_reversed))
colmap_matches.normalize()
matches_all = kapture_data.matches[keypoints_type]
matches_to_add = kapture.Matches({pair for pair in matches_all if pair not in colmap_matches})
# print(list(matches_to_add))
database_extra.add_matches_to_database(colmap_db, matches_to_add,
keypoints_type, kapture_path,
tar_handlers,
colmap_image_ids,
export_two_view_geometry=not use_colmap_matches_importer)
colmap_db.close()
if use_colmap_matches_importer:
logger.info('Step 2: Run geometric verification')
logger.debug('running colmap matches_importer...')
if keypoints_type is None:
keypoints_type = try_get_only_key_from_collection(kapture_data.matches)
assert keypoints_type is not None
assert keypoints_type in kapture_data.matches
# compute two view geometry
colmap_lib.run_matches_importer_from_kapture_matches(
colmap_binary,
colmap_use_cpu=True,
colmap_gpu_index=None,
colmap_db_path=colmap_db_path,
kapture_matches=kapture_data.matches[keypoints_type],
force=force)
else:
logger.info('Step 2: Run geometric verification - skipped')
if 'image_registrator' not in skip_list:
logger.info("Step 3: Run image_registrator")
# run image_registrator
colmap_lib.run_image_registrator(
colmap_binary,
colmap_db_path,
input_reconstruction_path,
reconstruction_path,
image_registrator_options
)
# run model_converter
if 'model_converter' not in skip_list:
logger.info("Step 4: Export reconstruction results to txt")
colmap_lib.run_model_converter(
colmap_binary,
reconstruction_path,
reconstruction_path
)
def import_colmap_rig_json(
rigs_colmap: list,
images: Optional[kapture.RecordsCamera] = None,
trajectories: Optional[kapture.Trajectories] = None
) -> Tuple[kapture.Rigs, kapture.RecordsCamera,
Optional[kapture.Trajectories]]:
"""
Build a kapture rig from colmap json file.
:param rigs_colmap: colmap data describing the rig.
:param images: input/output camera recordings: timestamps are modified to match
:param trajectories: input/output trajectories: timestamps are modified to match
:return: rigs, images and trajectories
"""
rigs_kapture = kapture.Rigs()
# camera_id (kapture) -> file prefix
camera_prefixes = {}
""" rigs_colmap
[{
"cameras": [
{"camera_id": 1, "image_prefix": "leftraw/"},
{"camera_id": 2, "image_prefix": "rightraw/"}
],
"ref_camera_id": 1
}]
"""
for rig_idx_colmap, rig_colmap in enumerate(rigs_colmap):
rig_id_kapture = f'rig{rig_idx_colmap}' # make up a rig ID from its idx in colmap.
for cam_colmap in rig_colmap['cameras']:
# colmap_cam_id -> kapture_cam_id
camera_id_colmap = cam_colmap['camera_id']
camera_id_kapture = get_camera_kapture_id_from_colmap_id(
camera_id_colmap)
camera_prefixes[camera_id_kapture] = cam_colmap['image_prefix']
# colmap does not store rig geometry, but only the fact there is one.
pose_unknown = kapture.PoseTransform(r=None, t=None)
rigs_kapture[rig_id_kapture, camera_id_kapture] = pose_unknown
reconstructed_images = None
reconstructed_trajectories = None
if images:
# image_filepath => (prefix, suffix)
filepath_to_split_fix = {}
# if there are images, modify their timestamps to match
# first pass: gather actual timestamps from suffix
# camera_suffixes = set()
for timestamp, camera_id_kapture, image_filepath in kapture.flatten(
images):
if camera_id_kapture not in camera_prefixes:
raise KeyError(
'unknown camera_id {}'.format(camera_id_kapture))
camera_prefix = camera_prefixes[camera_id_kapture]
if not image_filepath.startswith(camera_prefix):
raise ValueError('inconsistent camera name')
filepath_to_split_fix[image_filepath] = (
image_filepath[0:len(camera_prefix)],
image_filepath[len(camera_prefix):])
suffixes = sorted(set(suf
for _, suf in filepath_to_split_fix.values()))
suffix_to_timestamp = {
suffix: idx
for idx, suffix in enumerate(suffixes)
}
idx_to_timestamp = {
colmap_idx: suffix_to_timestamp[filepath_to_split_fix[filepath][1]]
for colmap_idx, _, filepath in kapture.flatten(images)
}
# second pass: reconstruct images with timestamp (frame number) instead of colmap idx
reconstructed_images = kapture.RecordsCamera()
for colmap_idx, camera_id_kapture, image_filepath in kapture.flatten(
images):
timestamp = idx_to_timestamp[colmap_idx]
reconstructed_images[timestamp, camera_id_kapture] = image_filepath
# third pass: [optional] reconstruct trajectories
if trajectories:
reconstructed_trajectories = kapture.Trajectories()
for colmap_idx, camera_id_kapture, pose in kapture.flatten(
trajectories):
timestamp = idx_to_timestamp[colmap_idx]
reconstructed_trajectories[timestamp, camera_id_kapture] = pose
return rigs_kapture, reconstructed_images, reconstructed_trajectories
def test_evaluation(self):
position = [1.658, 0, 0]
position_a = [2.658, 0, 0]
position_b = [1.758, 0, 0]
position_c = [10.1, 0, 0]
position_d = [2., 0, 0]
position_e = [6.658, 0, 0]
rotation = quaternion.from_euler_angles(np.deg2rad(110.0), 0, 0)
rotation_a = quaternion.from_euler_angles(np.deg2rad(111.0), 0, 0)
rotation_b = quaternion.from_euler_angles(np.deg2rad(108.0), 0, 0)
rotation_c = quaternion.from_euler_angles(np.deg2rad(10.0), 0, 0)
rotation_d = quaternion.from_euler_angles(np.deg2rad(110.0), 0, 0)
pose_gt = kapture.PoseTransform(r=rotation, t=position).inverse()
pose_a = kapture.PoseTransform(r=rotation_a, t=position_a).inverse()
pose_b = kapture.PoseTransform(r=rotation_b, t=position_b).inverse()
pose_c = kapture.PoseTransform(r=rotation_c, t=position_c).inverse()
pose_d = kapture.PoseTransform(r=rotation_d, t=position_d).inverse()
pose_e = kapture.PoseTransform(r=None, t=[-x for x in position_e])
kdata = kapture.Kapture(sensors=kapture.Sensors(),
records_camera=kapture.RecordsCamera(),
trajectories=kapture.Trajectories())
kdata.sensors['cam0'] = kapture.Camera(
kapture.CameraType.UNKNOWN_CAMERA, [25, 13])
kdata.records_camera[(0, 'cam0')] = 'a'
kdata.records_camera[(1, 'cam0')] = 'b'
kdata.records_camera[(2, 'cam0')] = 'c'
kdata.records_camera[(3, 'cam0')] = 'd'
kdata.records_camera[(4, 'cam0')] = 'e'
kdata.trajectories[(0, 'cam0')] = pose_a
kdata.trajectories[(1, 'cam0')] = pose_b
kdata.trajectories[(2, 'cam0')] = pose_c
kdata.trajectories[(3, 'cam0')] = pose_d
kdata2 = copy.deepcopy(kdata)
kdata2.trajectories[(4, 'cam0')] = pose_e
kdata2.records_camera[(5, 'cam0')] = 'f'
kdata_gt = copy.deepcopy(kdata2)
kdata_gt.trajectories[(0, 'cam0')] = pose_gt
kdata_gt.trajectories[(1, 'cam0')] = pose_gt
kdata_gt.trajectories[(2, 'cam0')] = pose_gt
kdata_gt.trajectories[(3, 'cam0')] = pose_gt
kdata_gt.trajectories[(4, 'cam0')] = pose_gt
kdata_gt.trajectories[(5, 'cam0')] = pose_gt
kdata_list = [kdata, kdata2, kdata_gt]
intersection = {'a', 'b', 'c', 'd', 'e'}
result1 = evaluate(kdata, kdata_gt, intersection)
self.assertEqual(len(result1), 5)
self.assertEqual(result1[0][0], 'a')
self.assertAlmostEqual(result1[0][1], 1.0)
self.assertAlmostEqual(result1[0][2], 1.0)
self.assertEqual(result1[1][0], 'b')
self.assertAlmostEqual(result1[1][1], 0.1)
self.assertAlmostEqual(result1[1][2], 2.0)
self.assertEqual(result1[2][0], 'c')
self.assertAlmostEqual(result1[2][1], 8.442)
self.assertAlmostEqual(result1[2][2], 100.0)
self.assertEqual(result1[3][0], 'd')
self.assertAlmostEqual(result1[3][1], 0.342)
self.assertAlmostEqual(result1[3][2], 0.0)
self.assertEqual(result1[4][0], 'e')
self.assertTrue(math.isnan(result1[4][1]))
self.assertTrue(math.isnan(result1[4][2]))
result2 = evaluate(kdata2, kdata_gt, intersection)
self.assertEqual(len(result2), 5)
self.assertEqual(result2[0][0], 'a')
self.assertAlmostEqual(result2[0][1], 1.0)
self.assertAlmostEqual(result2[0][2], 1.0)
self.assertEqual(result2[1][0], 'b')
self.assertAlmostEqual(result2[1][1], 0.1)
self.assertAlmostEqual(result2[1][2], 2.0)
self.assertEqual(result2[2][0], 'c')
self.assertAlmostEqual(result2[2][1], 8.442)
self.assertAlmostEqual(result2[2][2], 100.0)
self.assertEqual(result2[3][0], 'd')
self.assertAlmostEqual(result2[3][1], 0.342)
self.assertAlmostEqual(result2[3][2], 0.0)
self.assertEqual(result2[4][0], 'e')
self.assertAlmostEqual(result2[4][1], 5.0)
self.assertTrue(math.isnan(result2[4][2]))
bins1 = fill_bins(result1, [(0.9, 5), (10, 105)])
self.assertEqual(len(bins1), 2)
self.assertEqual(bins1[0][0], 0.9)
self.assertEqual(bins1[0][1], 5)
self.assertEqual(bins1[0][2], 2)
self.assertEqual(bins1[1][0], 10)
self.assertEqual(bins1[1][1], 105)
self.assertEqual(bins1[1][2], 4)
bins2 = fill_bins(result1, [(0.9, 5), (10, 105)])
self.assertEqual(len(bins2), 2)
self.assertEqual(bins2[0][0], 0.9)
self.assertEqual(bins2[0][1], 5)
self.assertEqual(bins2[0][2], 2)
self.assertEqual(bins2[1][0], 10)
self.assertEqual(bins2[1][1], 105)
self.assertEqual(bins2[1][2], 4)
bins3 = fill_bins(result2, [(0.9, math.nan), (10, math.nan)])
self.assertEqual(len(bins3), 2)
self.assertEqual(bins3[0][0], 0.9)
self.assertTrue(math.isnan(bins3[0][1]))
self.assertEqual(bins3[0][2], 2)
self.assertEqual(bins3[1][0], 10)
self.assertTrue(math.isnan(bins3[1][1]))
self.assertEqual(bins3[1][2], 5)
bins4 = fill_bins(result2, [(0.9, -1), (10, -1)])
self.assertEqual(len(bins4), 2)
self.assertEqual(bins4[0][0], 0.9)
self.assertEqual(bins4[0][1], -1)
self.assertEqual(bins4[0][2], 2)
self.assertEqual(bins4[1][0], 10)
self.assertEqual(bins4[1][1], -1)
self.assertEqual(bins4[1][2], 5)
