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 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 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