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)
