def test_nonconsecutive_indices(self):
"""Run rotation averaging on a graph with indices that are not ordered as [0,...,N-1].
Note: Test would fail if Shonan keys were not temporarily re-ordered inside the implementation.
See https://github.com/borglab/gtsam/issues/784
"""
num_images = 4
# assume pose 0 is orphaned in the visibility graph
# Let wTi0's (R,t) be parameterized as identity Rot3(), and t = [1,1,0]
wTi1 = Pose3(Rot3(), np.array([3, 1, 0]))
wTi2 = Pose3(Rot3(), np.array([3, 3, 0]))
wTi3 = Pose3(Rot3(), np.array([1, 3, 0]))
# generate i2Ri1 rotations
# (i1,i2) -> i2Ri1
i2Ri1_input = {
(1, 2): wTi2.between(wTi1).rotation(),
(2, 3): wTi3.between(wTi2).rotation(),
(1, 3): wTi3.between(wTi1).rotation(),
}
i2Ri1_priors = {edge: None for edge in i2Ri1_input.keys()}
wRi_computed = self.obj.run(num_images, i2Ri1_input, i2Ri1_priors)
wRi_expected = [
None, wTi1.rotation(),
wTi2.rotation(),
wTi3.rotation()
]
self.assertTrue(
geometry_comparisons.compare_rotations(
wRi_computed,
wRi_expected,
angular_error_threshold_degrees=0.1))
def test_computation_graph(self):
"""Test the dask computation graph execution using a valid collection of relative poses."""
num_poses = 3
i2Ri1_dict = {
(0, 1): Rot3.RzRyRx(0, np.deg2rad(30), 0),
(1, 2): Rot3.RzRyRx(0, 0, np.deg2rad(20)),
}
i2Ri1_graph = dask.delayed(i2Ri1_dict)
# use the GTSAM API directly (without dask) for rotation averaging
expected_wRi_list = self.obj.run(num_poses, i2Ri1_dict)
# use dask's computation graph
computation_graph = self.obj.create_computation_graph(
num_poses, i2Ri1_graph)
with dask.config.set(scheduler="single-threaded"):
wRi_list = dask.compute(computation_graph)[0]
# compare the two results
self.assertTrue(
geometry_comparisons.compare_rotations(
wRi_list, expected_wRi_list,
ROTATION_ANGLE_ERROR_THRESHOLD_DEG))
def __execute_test(self, i2Ri1_input: Dict[Tuple[int, int], Rot3],
wRi_expected: List[Rot3]) -> None:
"""Helper function to run the averagaing and assert w/ expected.
Args:
i2Ri1_input: relative rotations, which are input to the algorithm.
wRi_expected: expected global rotations.
"""
wRi_computed = self.obj.run(len(wRi_expected), i2Ri1_input)
self.assertTrue(
geometry_comparisons.compare_rotations(
wRi_computed, wRi_expected,
ROTATION_ANGLE_ERROR_THRESHOLD_DEG))
def test_compare_rotations_with_nones_at_different_indices(
self, aligned_rotations_mocked):
"""Tests the comparison results on list of rotations."""
list1 = [
Rot3.RzRyRx(0, np.deg2rad(25), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-20)),
None,
]
list2 = [
Rot3.RzRyRx(0, np.deg2rad(31), 0),
None,
Rot3.RzRyRx(0, 0, np.deg2rad(-22)),
]
# test with threshold of 10 degrees, which satisfies all the rotations.
self.assertFalse(
geometry_comparisons.compare_rotations(list1, list2, 10))
aligned_rotations_mocked.assert_not_called()
def test_compare_rotations_with_all_valid_rot3s_failure(
self, align_rotations_mocked):
"""Tests the comparison results on list of rotations."""
aRi_list = [
Rot3.RzRyRx(0, np.deg2rad(25), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-20)),
Rot3.RzRyRx(0, 0, np.deg2rad(80)),
]
bRi_list = [
Rot3.RzRyRx(0, np.deg2rad(31), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-22)),
Rot3.RzRyRx(0, 0, np.deg2rad(77.5)),
] # meaningless as align function is mocked
# test with threshold of 5 degrees, which fails one rotation and hence the overall comparison
self.assertFalse(
geometry_comparisons.compare_rotations(aRi_list, bRi_list, 5))
align_rotations_mocked.assert_called_once()
def test_compare_rotations_with_all_valid_rot3s_success(
self, align_rotations_mocked):
"""Tests the comparison results on list of rotations."""
aRi_list = [
Rot3.RzRyRx(0, np.deg2rad(25), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-20)),
Rot3.RzRyRx(0, 0, np.deg2rad(80)),
]
bRi_list = [
Rot3.RzRyRx(0, np.deg2rad(31), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-22)),
Rot3.RzRyRx(0, 0, np.deg2rad(77.5)),
] # meaningless as align function is mocked
# test with threshold of 10 degrees, which satisfies all the rotations.
self.assertTrue(
geometry_comparisons.compare_rotations(aRi_list, bRi_list, 10))
align_rotations_mocked.assert_called_once()
def test_compare_rotations_with_nones_at_same_indices(
self, align_rotations_mocked):
"""Tests the comparison results on list of rotations."""
list1 = [
Rot3.RzRyRx(0, np.deg2rad(25), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-20)),
None,
]
list2 = [
Rot3.RzRyRx(0, np.deg2rad(31), 0),
Rot3.RzRyRx(0, 0, np.deg2rad(-22)),
None,
]
threshold_degrees = 10
# test with threshold of 10 degrees, which satisfies all the rotations.
self.assertTrue(
geometry_comparisons.compare_rotations(list1, list2,
threshold_degrees))
align_rotations_mocked.assert_called_once()
def assert_results(self, results_a: List[Optional[Rot3]],
results_b: List[Optional[Rot3]]) -> None:
self.assertTrue(
geometry_comparisons.compare_rotations(
results_a, results_b, ROT3_DIFF_ANGLE_THRESHOLD_DEG))
