def rigs_calibrate_average(trajectories: Trajectories,
rigs: Rigs,
master_sensors: Optional[List[str]] = None) -> Rigs:
"""
Recomputes rig calibrations, ie. the poses of the senors inside a rig.
It uses the poses of the senors in the given trajectories, to average a average rig.
:param trajectories:
:param rigs:
:param master_sensors:
:return:
"""
# senor_id -> rig_id
sensors_to_rigs = {
sensor_id: rig_id
for rig_id, sensor_id in rigs.key_pairs()
}
show_progress_bar = logger.getEffectiveLevel() <= logging.INFO
for timestamp, poses in tqdm(trajectories.items(),
disable=not show_progress_bar):
# accumulate relative poses inside rig
for sensor_id, pose_sensor_from_world in poses.items():
pass
rigs_calibrated = Rigs()
return rigs_calibrated
def get_rigs_mapping(rigs: kapture.Rigs, offset: int = 0) -> Dict[str, str]:
"""
Creates list of rig names,identifiers
:param rigs: list of rig definitions
:param offset: optional offset for the identifier numbers
:return: mapping of rig names to identifiers
"""
return {
k: f'rig{v}'
for k, v in zip(rigs.keys(), range(offset, offset + len(rigs)))
}
def setUp(self) -> None:
self.rigs_expected = Rigs()
# kind of make a rosace around a central point
nb_cams = 6
rig_id = 'rig'
rig_radius = 10
camera_yaw_angles = np.linspace(0, 2 * np.pi, nb_cams + 1)[:-1]
t = [0, 0, -rig_radius]
for idx, cam_yaw_angle in enumerate(camera_yaw_angles):
r = quaternion.from_rotation_vector([0, cam_yaw_angle, 0])
pose_cam_from_rig = PoseTransform(t=t, r=r)
self.rigs_expected[rig_id, f'cam{idx:02d}'] = pose_cam_from_rig
# and for the trajectory, do random
self.rigs_trajectories = Trajectories()
nb_timestamps = 100
timestamps = np.arange(0, nb_timestamps)
orientations = np.random.uniform(-1, 1., size=(nb_timestamps, 4))
positions = np.random.uniform(-100., 100., size=(nb_timestamps, 3))
for ts, r, t in zip(timestamps, orientations, positions):
pose_rig_from_world = PoseTransform(t=t, r=r).inverse()
self.rigs_trajectories[int(ts), rig_id] = pose_rig_from_world
def rigs_to_file(filepath: str, rigs: kapture.Rigs) -> None:
"""
Writes rigs to CSV file.
:param filepath:
:param rigs:
"""
assert (isinstance(rigs, kapture.Rigs))
header = '# rig_id, sensor_id, qw, qx, qy, qz, tx, ty, tz'
padding = PADDINGS['device_id'] + PADDINGS['device_id'] + PADDINGS['pose']
table = ([rig_id, sensor_id] + pose_to_list(pose)
for rig_id, rig in rigs.items()
for sensor_id, pose in rig.items())
os.makedirs(path.dirname(filepath), exist_ok=True)
with open(filepath, 'w') as file:
table_to_file(file, table, header=header, padding=padding)
def test_perfect(self):
# create the trajectory with perfect transforms
cameras_trajectories = Trajectories()
for timestamp, rig_id, pose_rig_from_world in flatten(
self.rigs_trajectories):
for rig_id, cam_id, pose_cam_from_rig in flatten(
self.rigs_expected):
assert rig_id == 'rig'
pose_cam_from_world = PoseTransform.compose(
[pose_cam_from_rig, pose_rig_from_world])
cameras_trajectories[timestamp, cam_id] = pose_cam_from_world
rigs_unknown_geometry = Rigs()
for rig_id, cam_id in self.rigs_expected.key_pairs():
rigs_unknown_geometry[rig_id, cam_id] = PoseTransform()
# calibrate
rigs_recovered = rigs_calibrate_average(cameras_trajectories,
rigs_unknown_geometry)
class TestCalibrate(unittest.TestCase):
def setUp(self) -> None:
self.rigs_expected = Rigs()
# kind of make a rosace around a central point
nb_cams = 6
rig_id = 'rig'
rig_radius = 10
camera_yaw_angles = np.linspace(0, 2 * np.pi, nb_cams + 1)[:-1]
t = [0, 0, -rig_radius]
for idx, cam_yaw_angle in enumerate(camera_yaw_angles):
r = quaternion.from_rotation_vector([0, cam_yaw_angle, 0])
pose_cam_from_rig = PoseTransform(t=t, r=r)
self.rigs_expected[rig_id, f'cam{idx:02d}'] = pose_cam_from_rig
# and for the trajectory, do random
self.rigs_trajectories = Trajectories()
nb_timestamps = 100
timestamps = np.arange(0, nb_timestamps)
orientations = np.random.uniform(-1, 1., size=(nb_timestamps, 4))
positions = np.random.uniform(-100., 100., size=(nb_timestamps, 3))
for ts, r, t in zip(timestamps, orientations, positions):
pose_rig_from_world = PoseTransform(t=t, r=r).inverse()
self.rigs_trajectories[int(ts), rig_id] = pose_rig_from_world
def test_perfect(self):
# create the trajectory with perfect transforms
cameras_trajectories = Trajectories()
for timestamp, rig_id, pose_rig_from_world in flatten(
self.rigs_trajectories):
for rig_id, cam_id, pose_cam_from_rig in flatten(
self.rigs_expected):
assert rig_id == 'rig'
pose_cam_from_world = PoseTransform.compose(
[pose_cam_from_rig, pose_rig_from_world])
cameras_trajectories[timestamp, cam_id] = pose_cam_from_world
rigs_unknown_geometry = Rigs()
for rig_id, cam_id in self.rigs_expected.key_pairs():
rigs_unknown_geometry[rig_id, cam_id] = PoseTransform()
# calibrate
rigs_recovered = rigs_calibrate_average(cameras_trajectories,
rigs_unknown_geometry)
def get_top_level_rig_ids(rigs: kapture.Rigs) -> List[str]:
"""
get all top level rig ids (top level == only in the left column of rigs.txt)
"""
rig_low_level = set().union(*[list(k.keys()) for k in rigs.values()])
return [rigid for rigid in rigs.keys() if rigid not in rig_low_level]
def export_colmap_rig_json(
kapture_rigs: kapture.Rigs, records_camera: kapture.RecordsCamera,
colmap_camera_ids: Dict[str, int]) -> List[Dict[str, list]]:
"""
From colmap source code comments (colmap.cc:1401) :
// Read the configuration of the camera rigs from a JSON file. The input images
// of a camera rig must be named consistently to assign them to the appropriate
// camera rig and the respective kapture image.
//
// An example configuration of a single camera rig:
// [
// {
// "ref_camera_id": 1,
// "cameras":
// [
// {
// "camera_id": 1,
// "image_prefix": "left1_image"
// }, ...
// ]
// }
// ]
// This file specifies the configuration for a single camera rig and that you
// could potentially define multiple camera rigs.
Images with the same prefix ("left1_image/") are assigned to the same camera in the rig.
Images with the same suffix ("_frame000.png" and "/frame000.png") are
assigned to the same camera record, i.e., they are assumed to be captured at the same time.
This code, try to retrieve prefix of images.
:param kapture_rigs: kapture rigs to export
:param records_camera: used to guess camera prefix: assume a directory per camera
:param colmap_camera_ids: dict mapping sensor_id -> colmap_camera_ID
:return: The structure as expected in the colmap json file.
"""
colmap_rigs = []
for rig_kapture in kapture_rigs.values():
# rig[sensor_device_id] = <Pose>
rig_colmap = {'cameras': []}
for camera_id in rig_kapture:
if camera_id not in colmap_camera_ids:
# e.g. lidar0 and other sensors without record on disk
logger.warning(f'Camera {camera_id} not in COLMAP cameras')
continue
colmap_camera_id = colmap_camera_ids[camera_id]
# recover the image prefix :
images_dir_paths = set(
path.dirname(record_filepath) for _, sensor_id, record_filepath
in kapture.flatten(records_camera) if sensor_id == camera_id)
# check there is one, and only one image prefix (required by colmap).
if not len(images_dir_paths) == 1:
raise ValueError(
f'unable to find an image for camera {camera_id}')
the_images_dirpath = next(iter(images_dir_paths))
if not the_images_dirpath:
raise ValueError(
f'unable to find prefix for images {the_images_dirpath}.')
rig_colmap['cameras'].append({
'camera_id': colmap_camera_id,
'image_prefix': the_images_dirpath
})
rig_colmap['ref_camera_id'] = rig_colmap['cameras'][0]['camera_id']
colmap_rigs.append(rig_colmap)
return colmap_rigs