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