diff --git a/tests/averaging/rotation/test_uncertainity_aware.py b/tests/averaging/rotation/test_uncertainity_aware.py index aecf8f5ad..f6d25cb35 100644 --- a/tests/averaging/rotation/test_uncertainity_aware.py +++ b/tests/averaging/rotation/test_uncertainity_aware.py @@ -2,6 +2,7 @@ Authors: Ayush Baid, John Lambert """ + import pickle import unittest from typing import Dict, List, Tuple @@ -13,7 +14,7 @@ import gtsfm.utils.geometry_comparisons as geometry_comparisons import tests.data.sample_poses as sample_poses from gtsfm.averaging.rotation.rotation_averaging_base import RotationAveragingBase -from gtsfm.averaging.rotation.uncertainity_aware_rotation_averaging import UncertainityAwareRotationAveraging +from gtsfm.averaging.rotation.uncertainty_aware_rotation_averaging import UncertaintyAwareRotationAveraging from gtsfm.common.pose_prior import PosePrior, PosePriorType ROTATION_ANGLE_ERROR_THRESHOLD_DEG = 2 @@ -28,7 +29,7 @@ class TestUncertainityAwareRotationAveraging(unittest.TestCase): def setUp(self): super().setUp() - self.obj: RotationAveragingBase = UncertainityAwareRotationAveraging() + self.obj: RotationAveragingBase = UncertaintyAwareRotationAveraging() 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. @@ -87,92 +88,43 @@ def test_simple(self): self.__execute_test(i2Ri1_dict, expected_wRi_list) - # def test_simple_with_prior(self): - # """Test a simple case with 1 measurement and a single pose prior.""" - # expected_wRi_list = [Rot3.RzRyRx(0, 0, 0), Rot3.RzRyRx(0, np.deg2rad(30), 0), Rot3.RzRyRx(np.deg2rad(30), 0, 0)] - - # i2Ri1_dict = { - # (1, 0): Rot3.RzRyRx(0, np.deg2rad(30), 0), - # } - - # expected_0R2 = expected_wRi_list[0].between(expected_wRi_list[2]) - # i1Ti2_priors = { - # (0, 2): PosePrior( - # value=Pose3(expected_0R2, np.zeros((3,))), - # covariance=np.eye(6) * 1e-5, - # type=PosePriorType.SOFT_CONSTRAINT, - # ) - # } - - # wRi_computed = self.obj.run_rotation_averaging(len(expected_wRi_list), i2Ri1_dict, i1Ti2_priors) - # self.assertTrue( - # geometry_comparisons.compare_rotations(wRi_computed, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) # ) - # def test_computation_graph(self): - # """Test the dask computation graph execution using a valid collection of relative poses.""" + def test_computation_graph(self): + """Test the dask computation graph execution using a valid collection of relative poses.""" - # num_poses = 3 + num_poses = 3 - # i2Ri1_dict = { - # (0, 1): Rot3.RzRyRx(0, np.deg2rad(30), 0), - # (1, 2): Rot3.RzRyRx(0, 0, np.deg2rad(20)), - # } + 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) + i2Ri1_graph = dask.delayed(i2Ri1_dict) - # # use the GTSAM API directly (without dask) for rotation averaging - # i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} - # expected_wRi_list = self.obj.run_rotation_averaging(num_poses, i2Ri1_dict, i1Ti2_priors) + # use the GTSAM API directly (without dask) for rotation averaging + i1Ti2_priors: Dict[Tuple[int, int], PosePrior] = {} + expected_wRi_list = self.obj.run_rotation_averaging(num_poses, i2Ri1_dict, i1Ti2_priors) - # # use dask's computation graph - # gt_wTi_list = [None] * len(expected_wRi_list) - # rotations_graph, _ = self.obj.create_computation_graph(num_poses, i2Ri1_graph, i1Ti2_priors, gt_wTi_list) + # use dask's computation graph + gt_wTi_list = [None] * len(expected_wRi_list) + rotations_graph, _ = self.obj.create_computation_graph(num_poses, i2Ri1_graph, i1Ti2_priors, gt_wTi_list) - # with dask.config.set(scheduler="single-threaded"): - # wRi_list = dask.compute(rotations_graph)[0] + with dask.config.set(scheduler="single-threaded"): + wRi_list = dask.compute(rotations_graph)[0] - # # compare the two results - # self.assertTrue( - # geometry_comparisons.compare_rotations(wRi_list, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) - # ) + # compare the two results + self.assertTrue( + geometry_comparisons.compare_rotations(wRi_list, expected_wRi_list, ROTATION_ANGLE_ERROR_THRESHOLD_DEG) + ) - # def test_pickleable(self): - # """Tests that the object is pickleable (required for dask).""" - - # try: - # pickle.dumps(self.obj) - # except TypeError: - # self.fail("Cannot dump rotation averaging object using pickle") - - # 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(), - # } - - # relative_pose_priors: Dict[Tuple[int, int], PosePrior] = {} - # wRi_computed = self.obj.run_rotation_averaging(num_images, i2Ri1_input, relative_pose_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_pickleable(self): + """Tests that the object is pickleable (required for dask).""" + + try: + pickle.dumps(self.obj) + except TypeError: + self.fail("Cannot dump rotation averaging object using pickle") if __name__ == "__main__":