def test_align_poses_on_panorama_after_sim3_transform(self):
"""Test for alignment of poses after applying a forward motion transformation."""
translation_shift = np.array([0, 5, 0])
rotation_shift = Rot3.RzRyRx(0, 0, np.deg2rad(30))
scaling_factor = 1.0
aTi_list = sample_poses.PANORAMA_GLOBAL_POSES
bSa = Similarity3(rotation_shift, translation_shift, scaling_factor)
bTi_list = [bSa.transformFrom(x) for x in aTi_list]
aTi_list_ = geometry_comparisons.align_poses_sim3(aTi_list, bTi_list)
self.__assert_equality_on_pose3s(aTi_list_, aTi_list)
def test_align_poses_after_sim3_transform(self):
"""Test for alignment of poses after applying a SIM3 transformation."""
translation_shift = np.array([5, 10, -5])
rotation_shift = Rot3.RzRyRx(0, 0, np.deg2rad(30))
scaling_factor = 0.7
transform = Similarity3(rotation_shift, translation_shift,
scaling_factor)
ref_list = [
transform.transformFrom(x)
for x in sample_poses.CIRCLE_TWO_EDGES_GLOBAL_POSES
]
computed_poses = geometry_comparisons.align_poses_sim3(
sample_poses.CIRCLE_TWO_EDGES_GLOBAL_POSES, ref_list)
self.__assert_equality_on_pose3s(
computed_poses, sample_poses.CIRCLE_TWO_EDGES_GLOBAL_POSES)
def compute_averaging_metrics(
i2Ui1_dict: Dict[Tuple[int, int], Unit3],
wRi_list: List[Optional[Rot3]],
wti_list: List[Optional[Point3]],
gt_wTi_list: List[Optional[Pose3]],
) -> Dict[str, StatsDict]:
"""Computes statistics of multiple metrics for the averaging modules.
Specifically, computes statistics of:
- Rotation angle errors before BA,
- Translation distances before BA,
- Translation angle to direction measurements,
Estimated poses and ground truth poses are first aligned before computing metrics.
Args:
i2Ui1_dict: Dict from (i1, i2) to unit translation measurement i2Ui1.
wRi_list: List of estimated rotations.
wti_list: List of estimated translations.
gt_wTi_list: List of ground truth poses.
Returns:
Dict from metric name to a StatsDict.
Raises:
ValueError if lengths of wRi_list, wti_list and gt_wTi_list are not all same.
"""
if len(wRi_list) != len(wti_list) or len(wRi_list) != len(gt_wTi_list):
raise ValueError(
"Lengths of wRi_list, wti_list and gt_wTi_list should be the same."
)
wTi_list = []
for (wRi, wti) in zip(wRi_list, wti_list):
wTi_list.append(Pose3(wRi, wti))
wTi_aligned_list = comp_utils.align_poses_sim3(gt_wTi_list, wTi_list)
def get_rotations_translations_from_poses(
poses: List[Optional[Pose3]],
) -> Tuple[List[Optional[Rot3]], List[Optional[Point3]]]:
rotations = []
translations = []
for pose in poses:
if pose is None:
rotations.append(None)
translations.append(None)
continue
rotations.append(pose.rotation())
translations.append(pose.translation())
return rotations, translations
wRi_aligned_list, wti_aligned_list = get_rotations_translations_from_poses(
wTi_aligned_list)
gt_wRi_list, gt_wti_list = get_rotations_translations_from_poses(
gt_wTi_list)
metrics = {}
metrics["rotation_averaging_angle_deg"] = compute_rotation_angle_metrics(
wRi_aligned_list, gt_wRi_list)
metrics[
"translation_averaging_distance"] = compute_translation_distance_metrics(
wti_aligned_list, gt_wti_list)
metrics[
"translation_to_direction_angle_deg"] = compute_translation_angle_metrics(
i2Ui1_dict, wTi_aligned_list)
return metrics