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 test_data_association_with_missing_camera(self): """Tests the data association with input tracks which use a camera index for which the camera doesn't exist.""" triangulation_options = TriangulationOptions( reproj_error_threshold=5, mode=TriangulationSamplingMode.NO_RANSAC, min_num_hypotheses=20) da = DataAssociation(min_track_len=3, triangulation_options=triangulation_options) # add cameras 0 and 2 cameras = { 0: PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, np.deg2rad(20), 0), np.zeros((3, 1)))), 2: PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, 0, 0), np.array([10, 0, 0]))), } # just have one track, chaining cams 0->1 , and cams 1->2 corr_idxs_dict = { (0, 1): np.array([[0, 0]], dtype=np.int32), (1, 2): np.array([[0, 0]], dtype=np.int32) } keypoints_shared = Keypoints(coordinates=np.array([[20.0, 10.0]])) # will lead to a cheirality exception because keypoints are identical in two cameras # no track will be formed, and thus connected component will be empty sfm_data, _ = da.run( num_images=3, cameras=cameras, corr_idxs_dict=corr_idxs_dict, keypoints_list=[keypoints_shared] * 3, cameras_gt=[None] * 3, relative_pose_priors={}, ) self.assertEqual(len(sfm_data.get_valid_camera_indices()), 0) self.assertEqual(sfm_data.number_tracks(), 0)
def test_VisualISAMExample(self): # Create a factor poseKey1 = symbol('x', 1) poseKey2 = symbol('x', 2) trueRotation = Rot3.RzRyRx(0.15, 0.15, -0.20) trueTranslation = Point3(+0.5, -1.0, +1.0) trueDirection = Unit3(trueTranslation) E = EssentialMatrix(trueRotation, trueDirection) model = gtsam.noiseModel.Isotropic.Sigma(5, 0.25) factor = EssentialMatrixConstraint(poseKey1, poseKey2, E, model) # Create a linearization point at the zero-error point pose1 = Pose3(Rot3.RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)) pose2 = Pose3( Rot3.RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), Point3(-3.37493895, 6.14660244, -8.93650986)) expected = np.zeros((5, 1)) actual = factor.evaluateError(pose1, pose2) self.gtsamAssertEquals(actual, expected, 1e-8)
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_convert_to_epipolar_lines_valid_input(self): """Test conversion of valid 2D points to epipolar lines using the fundamental matrix, against manual computation and with OpenCV's output.""" points = np.array([[10.0, -5.0], [3.5, 20.0],]) # 2d points in homogenous coordinates E_matrix = EssentialMatrix(Rot3.RzRyRx(0, np.deg2rad(45), 0), Unit3(np.array([-5, 2, 0]))) F_matrix = E_matrix.matrix() # using identity intrinsics expected_opencv = cv.computeCorrespondEpilines(points.reshape(-1, 1, 2), 1, F_matrix) expected_opencv = np.squeeze(expected_opencv) # converting to 2D array as opencv adds a singleton dimension computed = feature_utils.convert_to_epipolar_lines(points, F_matrix) computed_normalized = computed / np.linalg.norm(computed[:, :2], axis=1, keepdims=True) np.testing.assert_allclose(computed_normalized, expected_opencv)
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))
def test_compute_epipolar_distances(self): """Test for epipolar distance computation using 2 sets of points and the essential matrix.""" normalized_pts_i1 = np.array([[1.0, 3.5], [-2.0, 2.0]]) normalized_pts_i2 = np.array([[2.0, -1.0], [1.0, 0.0]]) i2Ri1 = Rot3.RzRyRx(0, np.deg2rad(30), np.deg2rad(10)) i2ti1 = np.array([-0.5, 2.0, 0]) i2Ei1 = EssentialMatrix(i2Ri1, Unit3(i2ti1)) expected = np.array([1.637, 1.850]) computed = verification_utils.compute_epipolar_distances( normalized_pts_i1, normalized_pts_i2, i2Ei1) np.testing.assert_allclose(computed, expected, rtol=1e-3)
def test_compute_translation_to_direction_angle_is_nonzero(self): rz = np.deg2rad(90) wRi2 = Rot3.RzRyRx(0, 0, rz) # x-axis of i2 points along y in world frame wTi2_estimated = Pose3(wRi2, Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3( -1, 0, 0)) # At (0, 1, 0) in i2 frame, rotation of i1 is irrelevant here. i2Ui1_measured = Unit3(Point3(1, 0, 0)) # Estimated relative translation of i1 in i2 frame is (0, 1, 0), and the measurement in i2 frame is (1, 0, 0). # Expected angle between the two is 90 degrees. self.assertTrue( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 90.0, )
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 test_convert_to_epipolar_lines_valid_input(self): """Test conversion of valid 2D points to epipolar lines using the essential matrix.""" points = np.array([ [10.0, -5.0], [3.5, 20.0], ]) # 2d points in homogenous coordinates essential_mat = EssentialMatrix(Rot3.RzRyRx(0, np.deg2rad(45), 0), Unit3(np.array([-5, 2, 0]))) computed = feature_utils.convert_to_epipolar_lines( points, essential_mat) expected = np.array([ [-2.36351579, -5.90878948, 1.75364193], [-0.65653216, -1.64133041, -19.75129171], ]) np.testing.assert_allclose(computed, expected)
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_align_rotations(self): """Tests the alignment of rotations.""" # using rotation along just the Y-axis so that angles can be linearly added. input_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-10), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(30), 0), ] ref_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(80), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-40), 0), ] computed = geometry_comparisons.align_rotations(input_list, ref_list) expected = [ Rot3.RzRyRx(0, np.deg2rad(80), 0), Rot3.RzRyRx(0, np.deg2rad(120), 0), ] self.__assert_equality_on_rot3s(computed, expected)
def test_rot3_roundtrip(self): """Test the round-trip on Rot3 object.""" expected = Rot3.RzRyRx(0, 0.05, 0.1) header, frames = serialize(expected) recovered = deserialize(header, frames) self.assertTrue(expected.equals(recovered, 1e-5))
class TestGeometryComparisons(unittest.TestCase): """Unit tests for comparison functions for geometry types.""" def __assert_equality_on_rot3s(self, computed: List[Rot3], expected: List[Rot3]) -> None: self.assertEqual(len(computed), len(expected)) for R, R_ in zip(computed, expected): self.assertEqual(R, R_) def __assert_equality_on_point3s(self, computed: List[Point3], expected: List[Point3]) -> None: self.assertEqual(len(computed), len(expected)) for t, t_ in zip(computed, expected): np.testing.assert_allclose(t, t_, rtol=POINT3_RELATIVE_ERROR_THRESH, atol=POINT3_ABS_ERROR_THRESH) def __assert_equality_on_pose3s(self, computed: List[Pose3], expected: List[Pose3]) -> None: self.assertEqual(len(computed), len(expected)) computed_rot3s = [x.rotation() for x in computed] computed_point3s = [x.translation() for x in computed] expected_rot3s = [x.rotation() for x in expected] expected_point3s = [x.translation() for x in expected] self.__assert_equality_on_rot3s(computed_rot3s, expected_rot3s) self.__assert_equality_on_point3s(computed_point3s, expected_point3s) def setUp(self): super().setUp() self.addTypeEqualityFunc(Rot3, rot3_compare) self.addTypeEqualityFunc(Point3, point3_compare) def test_align_rotations(self): """Tests the alignment of rotations.""" # using rotation along just the Y-axis so that angles can be linearly added. input_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-10), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(30), 0), ] ref_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(80), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-40), 0), ] computed = geometry_comparisons.align_rotations(input_list, ref_list) expected = [ Rot3.RzRyRx(0, np.deg2rad(80), 0), Rot3.RzRyRx(0, np.deg2rad(120), 0), ] self.__assert_equality_on_rot3s(computed, expected) 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, aSb = geometry_comparisons.align_poses_sim3( sample_poses.CIRCLE_TWO_EDGES_GLOBAL_POSES, ref_list) assert isinstance(aSb, Similarity3) self.__assert_equality_on_pose3s( computed_poses, sample_poses.CIRCLE_TWO_EDGES_GLOBAL_POSES) 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_, aSb = geometry_comparisons.align_poses_sim3( aTi_list, bTi_list) assert isinstance(aSb, Similarity3) self.__assert_equality_on_pose3s(aTi_list_, aTi_list) @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(32), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-22)), Rot3.RzRyRx(0, 0, np.deg2rad(83)), ], # compared with aRi_list ) 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() @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(32), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-22)), Rot3.RzRyRx(0, 0, np.deg2rad(83)), ], # compared with aRi_list ) 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() @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(25), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-20)) ], # compared with aRi_list ) 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() @patch("gtsfm.utils.geometry_comparisons.align_rotations", return_value=None) 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_compute_relative_rotation_angle(self): """Tests the relative angle between two rotations.""" R_1 = Rot3.RzRyRx(0, np.deg2rad(45), np.deg2rad(22.5)) R_2 = Rot3.RzRyRx(0, np.deg2rad(90), np.deg2rad(22.5)) # returns angle in degrees computed_deg = geometry_comparisons.compute_relative_rotation_angle( R_1, R_2) expected_deg = 45 np.testing.assert_allclose(computed_deg, expected_deg, rtol=1e-3, atol=1e-3) def test_compute_relative_rotation_angle2(self) -> None: """Tests the relative angle between two rotations Currently compute_relative_rotation_angle() uses Scipy, so this test compares with GTSAM. TODO(johnwlambert): replace this test with Scipy function calls once we fix the GTSAM's .axisAngle() code. """ num_trials = 1000 np.random.seed(0) def random_rotation() -> Rot3: """Sample a random rotation by generating a sample from the 4d unit sphere.""" q = np.random.randn(4) # make unit-length quaternion q /= np.linalg.norm(q) qw, qx, qy, qz = q R = Rot3(qw, qx, qy, qz) return R for _ in range(num_trials): # generate 2 random rotations wR1 = random_rotation() wR2 = random_rotation() computed_deg = geometry_comparisons.compute_relative_rotation_angle( wR1, wR2) i2Ri1 = wR2.between(wR1) _, expected_rad = i2Ri1.axisAngle() expected_deg = np.rad2deg(expected_rad) np.testing.assert_allclose(computed_deg, expected_deg, rtol=1e-3, atol=1e-3) def test_compute_relative_unit_translation_angle(self): """Tests the relative angle between two unit-translations.""" U_1 = Unit3(np.array([1, 0, 0])) U_2 = Unit3(np.array([0.5, 0.5, 0])) # returns angle in degrees computed_deg = geometry_comparisons.compute_relative_unit_translation_angle( U_1, U_2) expected_deg = 45 self.assertAlmostEqual(computed_deg, expected_deg, places=3) def test_compute_translation_to_direction_angle_is_zero(self): i2Ui1_measured = Unit3(Point3(1, 0, 0)) wTi2_estimated = Pose3(Rot3(), Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3(2, 0, 0)) self.assertEqual( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 0.0, ) def test_compute_translation_to_direction_angle_is_nonzero(self): rz = np.deg2rad(90) wRi2 = Rot3.RzRyRx(0, 0, rz) # x-axis of i2 points along y in world frame wTi2_estimated = Pose3(wRi2, Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3( -1, 0, 0)) # At (0, 1, 0) in i2 frame, rotation of i1 is irrelevant here. i2Ui1_measured = Unit3(Point3(1, 0, 0)) # Estimated relative translation of i1 in i2 frame is (0, 1, 0), and the measurement in i2 frame is (1, 0, 0). # Expected angle between the two is 90 degrees. self.assertTrue( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 90.0, ) def test_compute_points_distance_l2_is_zero(self): self.assertEqual( geometry_comparisons.compute_points_distance_l2( wti1=Point3(1, -2, 3), wti2=Point3(1, -2, 3)), 0.0) def test_compute_points_distance_l2_is_none(self): self.assertEqual( geometry_comparisons.compute_points_distance_l2(wti1=Point3( 0, 0, 0), wti2=None), None) def test_compute_points_distance_l2_is_nonzero(self): wti1 = Point3(1, 1, 1) wti2 = Point3(1, 1, -1) self.assertEqual( geometry_comparisons.compute_points_distance_l2(wti1, wti2), 2) def test_align_poses_sim3_ignore_missing(self): """Consider a simple cases with 4 poses in a line. Suppose SfM only recovers 2 of the 4 poses.""" wT0 = Pose3(Rot3(np.eye(3)), np.zeros(3)) wT1 = Pose3(Rot3(np.eye(3)), np.ones(3)) wT2 = Pose3(Rot3(np.eye(3)), np.ones(3) * 2) wT3 = Pose3(Rot3(np.eye(3)), np.ones(3) * 3) # `a` frame is the target/reference frame aTi_list = [wT0, wT1, wT2, wT3] # `b` frame contains the estimates bTi_list = [None, wT1, None, wT3] aTi_list_, _ = geometry_comparisons.align_poses_sim3_ignore_missing( aTi_list, bTi_list) # indices 0 and 2 should still have no estimated pose, even after alignment assert aTi_list_[0] is None assert aTi_list_[2] is None # identity alignment should preserve poses, should still match GT/targets at indices 1 and 3 self.__assert_equality_on_pose3s(computed=[aTi_list_[1], aTi_list_[3]], expected=[aTi_list[1], aTi_list[3]])
def simulate_two_planes_scene( M: int, N: int) -> Tuple[Keypoints, Keypoints, EssentialMatrix]: """Generate a scene where 3D points are on two planes, and projects the points to the 2 cameras. There are M points on plane 1, and N points on plane 2. The two planes in this test are: 1. -10x -y -20z +150 = 0 2. 15x -2y -35z +200 = 0 Args: M: number of points on 1st plane. N: number of points on 2nd plane. Returns: keypoints for image i1, of length (M+N). keypoints for image i2, of length (M+N). Essential matrix i2Ei1. """ # range of 3D points range_x_coordinate = (-5, 7) range_y_coordinate = (-10, 10) # define the plane equation plane1_coeffs = (-10, -1, -20, 150) plane2_coeffs = (15, -2, -35, 200) # sample the points from planes plane1_points = sample_points_on_plane(plane1_coeffs, range_x_coordinate, range_y_coordinate, M) plane2_points = sample_points_on_plane(plane2_coeffs, range_x_coordinate, range_y_coordinate, N) points_3d = np.vstack((plane1_points, plane2_points)) # define the camera poses and compute the essential matrix wti1 = np.array([0.1, 0, -20]) wti2 = np.array([1, -2, -20.4]) wRi1 = Rot3.RzRyRx(np.pi / 20, 0, 0.0) wRi2 = Rot3.RzRyRx(0.0, np.pi / 6, 0.0) wTi1 = Pose3(wRi1, wti1) wTi2 = Pose3(wRi2, wti2) i2Ti1 = wTi2.between(wTi1) i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation())) # project 3D points to 2D image measurements intrinsics = Cal3Bundler() camera_i1 = PinholeCameraCal3Bundler(wTi1, intrinsics) camera_i2 = PinholeCameraCal3Bundler(wTi2, intrinsics) uv_im1 = [] uv_im2 = [] for point in points_3d: uv_im1.append(camera_i1.project(point)) uv_im2.append(camera_i2.project(point)) uv_im1 = np.vstack(uv_im1) uv_im2 = np.vstack(uv_im2) # return the points as keypoints and the essential matrix return Keypoints(coordinates=uv_im1), Keypoints(coordinates=uv_im2), i2Ei1
def test_rot3_roundtrip(self): obj = Rot3.RzRyRx(0, 0.05, 0.1) self.assertEqualityOnPickleRoundtrip(obj)
def test_pinholeCameraCal3Bundler_roundtrip(self): obj = PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)), Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70), ) self.assertEqualityOnPickleRoundtrip(obj)
"""Sample poses for testing the averaging algorithms. The visualizations of this poses are stored in the folder: tests/data/viz_sample_poses Authors: Ayush Baid """ import copy from typing import Dict, List, Tuple import numpy as np from gtsam import Cal3_S2, Point3, Pose3, Rot3, Unit3 from gtsam.examples import SFMdata DEFAULT_ROTATION = Rot3.RzRyRx(0, np.deg2rad(10), 0) DEFAULT_TRANSLATION = np.array([0, 2, 0]) def generate_relative_from_global( wTi_list: List[Pose3], pair_indices: List[Tuple[int, int]]) -> Dict[Tuple[int, int], Pose3]: """Generate relative poses from global poses. Args: wTi_list: global poses. pair_indices: pairs (i1, i2) to construct relative poses for. Returns: Dictionary (i1, i2) -> i2Ti1 for all requested pairs. """ return {(i1, i2): wTi_list[i2].between(wTi_list[i1]) for i1, i2 in pair_indices}
def generate_random_essential_matrix() -> EssentialMatrix: rotation_angles = np.random.uniform(low=0.0, high=2 * np.pi, size=(3, )) R = Rot3.RzRyRx(rotation_angles[0], rotation_angles[1], rotation_angles[2]) t = np.random.uniform(low=-1.0, high=1.0, size=(3, )) return EssentialMatrix(R, Unit3(t))
def test_pose3_roundtrip(self): """Test the round-trip on Point3 object.""" expected = Pose3(Rot3.RzRyRx(0, 0.1, 0.2), np.random.randn(3)) header, frames = serialize(expected) recovered = deserialize(header, frames) self.assertTrue(recovered.equals(expected, 1e-5))
class TestGeometryComparisons(unittest.TestCase): """Unit tests for comparison functions for geometry types.""" def __assert_equality_on_rot3s(self, computed: List[Rot3], expected: List[Rot3]) -> None: self.assertEqual(len(computed), len(expected)) for R, R_ in zip(computed, expected): self.assertEqual(R, R_) def __assert_equality_on_point3s(self, computed: List[Point3], expected: List[Point3]) -> None: self.assertEqual(len(computed), len(expected)) for t, t_ in zip(computed, expected): np.testing.assert_allclose(t, t_, rtol=POINT3_RELATIVE_ERROR_THRESH, atol=POINT3_ABS_ERROR_THRESH) def __assert_equality_on_pose3s(self, computed: List[Pose3], expected: List[Pose3]) -> None: self.assertEqual(len(computed), len(expected)) computed_rot3s = [x.rotation() for x in computed] computed_point3s = [x.translation() for x in computed] expected_rot3s = [x.rotation() for x in expected] expected_point3s = [x.translation() for x in expected] self.__assert_equality_on_rot3s(computed_rot3s, expected_rot3s) self.__assert_equality_on_point3s(computed_point3s, expected_point3s) def setUp(self): super().setUp() self.addTypeEqualityFunc(Rot3, rot3_compare) self.addTypeEqualityFunc(Point3, point3_compare) def test_align_rotations(self): """Tests the alignment of rotations.""" # using rotation along just the Y-axis so that angles can be linearly added. input_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-10), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(30), 0), ] ref_list = [ Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(80), 0), Rot3.RzRyRx(np.deg2rad(0), np.deg2rad(-40), 0), ] computed = geometry_comparisons.align_rotations(input_list, ref_list) expected = [ Rot3.RzRyRx(0, np.deg2rad(80), 0), Rot3.RzRyRx(0, np.deg2rad(120), 0), ] self.__assert_equality_on_rot3s(computed, expected) 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 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) @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(32), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-22)), Rot3.RzRyRx(0, 0, np.deg2rad(83)), ], # compared with aRi_list ) 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() @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(32), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-22)), Rot3.RzRyRx(0, 0, np.deg2rad(83)), ], # compared with aRi_list ) 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() @patch( "gtsfm.utils.geometry_comparisons.align_rotations", return_value=[ Rot3.RzRyRx(0, np.deg2rad(25), 0), Rot3.RzRyRx(0, 0, np.deg2rad(-20)), ], # compared with aRi_list ) 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() @patch("gtsfm.utils.geometry_comparisons.align_rotations", return_value=None) 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_compute_relative_rotation_angle(self): """Tests the relative angle between two rotations.""" R_1 = Rot3.RzRyRx(0, np.deg2rad(45), np.deg2rad(22.5)) R_2 = Rot3.RzRyRx(0, np.deg2rad(90), np.deg2rad(22.5)) # returns angle in degrees computed_deg = geometry_comparisons.compute_relative_rotation_angle( R_1, R_2) expected_deg = 45 np.testing.assert_allclose(computed_deg, expected_deg, rtol=1e-3, atol=1e-3) def test_compute_relative_unit_translation_angle(self): """Tests the relative angle between two unit-translations.""" U_1 = Unit3(np.array([1, 0, 0])) U_2 = Unit3(np.array([0.5, 0.5, 0])) # returns angle in degrees computed_deg = geometry_comparisons.compute_relative_unit_translation_angle( U_1, U_2) expected_deg = 45 self.assertAlmostEqual(computed_deg, expected_deg, places=3) def test_compute_translation_to_direction_angle_is_zero(self): i2Ui1_measured = Unit3(Point3(1, 0, 0)) wTi2_estimated = Pose3(Rot3(), Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3(2, 0, 0)) self.assertEqual( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 0.0, ) def test_compute_translation_to_direction_angle_is_nonzero(self): rz = np.deg2rad(90) wRi2 = Rot3.RzRyRx(0, 0, rz) # x-axis of i2 points along y in world frame wTi2_estimated = Pose3(wRi2, Point3(0, 0, 0)) wTi1_estimated = Pose3(Rot3(), Point3( -1, 0, 0)) # At (0, 1, 0) in i2 frame, rotation of i1 is irrelevant here. i2Ui1_measured = Unit3(Point3(1, 0, 0)) # Estimated relative translation of i1 in i2 frame is (0, 1, 0), and the measurement in i2 frame is (1, 0, 0). # Expected angle between the two is 90 degrees. self.assertTrue( geometry_comparisons.compute_translation_to_direction_angle( i2Ui1_measured, wTi2_estimated, wTi1_estimated), 90.0, ) def test_compute_points_distance_l2_is_zero(self): self.assertEqual( geometry_comparisons.compute_points_distance_l2( Point3(1, -2, 3), Point3(1, -2, 3)), 0.0, ) def test_compute_points_distance_l2_is_none(self): self.assertEqual( geometry_comparisons.compute_points_distance_l2( Point3(0, 0, 0), None), None, ) def test_compute_points_distance_l2_is_nonzero(self): wti1 = Point3(1, 1, 1) wti2 = Point3(1, 1, -1) self.assertEqual( geometry_comparisons.compute_points_distance_l2(wti1, wti2), 2)