def test_compare_global_poses_scaled_squares(self):
"""Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
"""
R0 = Rotation.from_euler("z", 0, degrees=True).as_matrix()
R90 = Rotation.from_euler("z", 90, degrees=True).as_matrix()
R180 = Rotation.from_euler("z", 180, degrees=True).as_matrix()
R270 = Rotation.from_euler("z", 270, degrees=True).as_matrix()
wTu0 = Pose3(Rot3(R0), np.array([2, 3, 0]))
wTu1 = Pose3(Rot3(R90), np.array([12, 3, 0]))
wTu2 = Pose3(Rot3(R180), np.array([12, 13, 0]))
wTu3 = Pose3(Rot3(R270), np.array([2, 13, 0]))
wTi_list = [wTu0, wTu1, wTu2, wTu3]
wTv0 = Pose3(Rot3(R0), np.array([4, 3, 0]))
wTv1 = Pose3(Rot3(R90), np.array([8, 3, 0]))
wTv2 = Pose3(Rot3(R180), np.array([8, 7, 0]))
wTv3 = Pose3(Rot3(R270), np.array([4, 7, 0]))
wTi_list_ = [wTv0, wTv1, wTv2, wTv3]
pose_graphs_equal = geometry_comparisons.compare_global_poses(
wTi_list, wTi_list_)
self.assertTrue(pose_graphs_equal)
def test_create_computation_graph(self):
"""Will test Dask multi-processing capabilities and ability to serialize all objects."""
use_intrinsics_in_verification = False
with initialize_config_module(config_module="gtsfm.configs"):
# config is relative to the gtsfm module
cfg = compose(config_name="scene_optimizer_unit_test_config.yaml")
self.obj: SceneOptimizer = instantiate(cfg.SceneOptimizer)
# generate the dask computation graph
sfm_result_graph = self.obj.create_computation_graph(
len(self.loader),
self.loader.get_valid_pairs(),
self.loader.create_computation_graph_for_images(),
self.loader.create_computation_graph_for_intrinsics(),
use_intrinsics_in_verification=use_intrinsics_in_verification,
)
# create dask client
cluster = LocalCluster(n_workers=1, threads_per_worker=4)
with Client(cluster):
sfm_result = dask.compute(sfm_result_graph)[0]
self.assertIsInstance(sfm_result, SfmResult)
# compare the camera poses
poses = sfm_result.get_camera_poses()
expected_poses = [self.loader.get_camera_pose(i) for i in range(len(self.loader))]
self.assertTrue(comp_utils.compare_global_poses(poses, expected_poses))
def test_lund_door(self):
loader = FolderLoader(str(DATA_ROOT_PATH / "set1_lund_door"),
image_extension="JPG")
expected_wTi_list = [
loader.get_camera_pose(x) for x in range(len(loader))
]
wRi_list = [x.rotation() for x in expected_wTi_list]
i2Ui1_dict = dict()
for (i1, i2) in loader.get_valid_pairs():
i2Ti1 = expected_wTi_list[i2].between(expected_wTi_list[i1])
i2Ui1_dict[(i1, i2)] = Unit3((i2Ti1.translation()))
wti_list = self.obj.run(len(loader), i2Ui1_dict, wRi_list)
wTi_list = [
Pose3(wRi, wti) if wti is not None else None
for (wRi, wti) in zip(wRi_list, wti_list)
]
# TODO: using a v high value for translation relative threshold. Fix it
self.assertTrue(
geometry_comparisons.compare_global_poses(wTi_list,
expected_wTi_list,
trans_err_thresh=2e1))
def test_compare_different_poses(self):
"""Compare pose comparison with different inputs."""
pose_list = [POSE_LIST[1], POSE_LIST[2], POSE_LIST[3]]
pose_list_ = [POSE_LIST[2], POSE_LIST[3], POSE_LIST[1]]
self.assertFalse(
geometry_comparisons.compare_global_poses(pose_list, pose_list_))
def test_compare_poses_with_origin_shift(self):
"""Check pose comparison with a shift in the global origin."""
new_origin = Pose3(Rot3.RzRyRx(0.3, 0.1, -0.27),
np.array([-20.0, +19.0, 3.5]))
pose_list_ = [new_origin.between(x) for x in POSE_LIST]
self.assertTrue(
geometry_comparisons.compare_global_poses(POSE_LIST, pose_list_))
def test_compare_poses_with_uniform_scaled_translations(self):
"""Check pose comparison with all translations in input #2 scaled by
the same scalar factor."""
scale_factor = 1.2
pose_list_ = [
Pose3(x.rotation(),
x.translation() * scale_factor) for x in POSE_LIST
]
self.assertTrue(
geometry_comparisons.compare_global_poses(POSE_LIST, pose_list_))
def test_compare_poses_with_nonuniform_scaled_translations(self):
"""Check pose comparison with all translations in input #2 scaled by
significantly different scalar factors."""
scale_factors = [0.3, 0.7, 0.9, 1.0, 1.0, 0.99, 1.01, 1.10]
pose_list_ = [
Pose3(x.rotation(),
x.translation() * scale_factors[idx])
for idx, x in enumerate(POSE_LIST)
]
self.assertFalse(
geometry_comparisons.compare_global_poses(POSE_LIST, pose_list_))
def test_computation_graph(self):
"""Test the dask computation graph execution using a valid collection of relative unit-translations."""
"""Test a simple case with 8 camera poses.
The camera poses are arranged on the circle and point towards the center
of the circle. The poses of 8 cameras are obtained from SFMdata and the
unit translations directions between some camera pairs are computed from their global translations.
This test is copied from GTSAM's TranslationAveragingExample.
"""
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
expected_wTi_list = SFMdata.createPoses(Cal3_S2(fx, fy, s, u0, v0))
wRi_list = [x.rotation() for x in expected_wTi_list]
# create relative translation directions between a pose index and the
# next two poses
i2Ui1_dict = {}
for i1 in range(len(expected_wTi_list) - 1):
for i2 in range(i1 + 1, min(len(expected_wTi_list), i1 + 3)):
# create relative translations using global R and T.
i2Ui1_dict[(i1, i2)] = Unit3(expected_wTi_list[i2].between(
expected_wTi_list[i1]).translation())
# use the `run` API to get expected results
expected_wti_list = self.obj.run(len(wRi_list), i2Ui1_dict, wRi_list)
expected_wTi_list = [
Pose3(wRi, wti) if wti is not None else None
for (wRi, wti) in zip(wRi_list, expected_wti_list)
]
# form computation graph and execute
i2Ui1_graph = dask.delayed(i2Ui1_dict)
wRi_graph = dask.delayed(wRi_list)
computation_graph = self.obj.create_computation_graph(
len(wRi_list), i2Ui1_graph, wRi_graph)
with dask.config.set(scheduler="single-threaded"):
wti_list = dask.compute(computation_graph)[0]
wTi_list = [
Pose3(wRi, wti) if wti is not None else None
for (wRi, wti) in zip(wRi_list, wti_list)
]
# compare the entries
self.assertTrue(
geometry_comparisons.compare_global_poses(wTi_list,
expected_wTi_list))
def test_create_computation_graph(self):
"""Will test Dask multi-processing capabilities and ability to serialize all objects."""
self.loader = OlssonLoader(str(DATA_ROOT_PATH / "set1_lund_door"),
image_extension="JPG")
with hydra.initialize_config_module(config_module="gtsfm.configs"):
# config is relative to the gtsfm module
cfg = hydra.compose(
config_name="scene_optimizer_unit_test_config.yaml")
scene_optimizer: SceneOptimizer = instantiate(cfg.SceneOptimizer)
# generate the dask computation graph
delayed_sfm_result, delayed_io = scene_optimizer.create_computation_graph(
num_images=len(self.loader),
image_pair_indices=self.loader.get_valid_pairs(),
image_graph=self.loader.create_computation_graph_for_images(),
all_intrinsics=self.loader.get_all_intrinsics(),
image_shapes=self.loader.get_image_shapes(),
absolute_pose_priors=self.loader.get_absolute_pose_priors(),
relative_pose_priors=self.loader.get_relative_pose_priors(
self.loader.get_valid_pairs()),
cameras_gt=self.loader.get_gt_cameras(),
gt_wTi_list=self.loader.get_gt_poses(),
)
# create dask client
cluster = LocalCluster(n_workers=1, threads_per_worker=4)
with Client(cluster):
sfm_result, *io = dask.compute(delayed_sfm_result, *delayed_io)
self.assertIsInstance(sfm_result, GtsfmData)
# compare the camera poses
computed_poses = sfm_result.get_camera_poses()
# get active cameras from largest connected component, may be <len(self.loader)
connected_camera_idxs = sfm_result.get_valid_camera_indices()
expected_poses = [
self.loader.get_camera_pose(i) for i in connected_camera_idxs
]
self.assertTrue(
comp_utils.compare_global_poses(computed_poses,
expected_poses,
trans_err_atol=1.0,
trans_err_rtol=0.1))
def __execute_test(self, i2Ui1_input: Dict[Tuple[int, int],
Unit3], wRi_input: List[Rot3],
wti_expected: List[Point3]) -> None:
"""Helper function to run the averagaing and assert w/ expected."""
wti_computed, _ = self.obj.run(len(wRi_input), i2Ui1_input, wRi_input)
wTi_computed = [
Pose3(wRi, wti) for wRi, wti in zip(wRi_input, wti_computed)
]
wTi_expected = [
Pose3(wRi, wti) for wRi, wti in zip(wRi_input, wti_expected)
]
self.assertTrue(
geometry_comparisons.compare_global_poses(
wTi_computed, wTi_expected, RELATIVE_ERROR_THRESHOLD,
ABSOLUTE_ERROR_THRESHOLD))
def test_create_computation_graph(self):
"""Will test Dask multi-processing capabilities and ability to serialize all objects."""
self.loader = OlssonLoader(str(DATA_ROOT_PATH / "set1_lund_door"),
image_extension="JPG")
with initialize_config_module(config_module="gtsfm.configs"):
# config is relative to the gtsfm module
cfg = compose(config_name="scene_optimizer_unit_test_config.yaml")
obj: SceneOptimizer = instantiate(cfg.SceneOptimizer)
# generate the dask computation graph
sfm_result_graph = obj.create_computation_graph(
len(self.loader),
self.loader.get_valid_pairs(),
self.loader.create_computation_graph_for_images(),
self.loader.create_computation_graph_for_intrinsics(),
gt_pose_graph=self.loader.create_computation_graph_for_poses(),
)
# create dask client
cluster = LocalCluster(n_workers=1, threads_per_worker=4)
with Client(cluster):
sfm_result = dask.compute(sfm_result_graph)[0]
self.assertIsInstance(sfm_result, GtsfmData)
# compare the camera poses
computed_poses = sfm_result.get_camera_poses()
computed_rotations = [x.rotation() for x in computed_poses]
computed_translations = [x.translation() for x in computed_poses]
# get active cameras from largest connected component, may be <len(self.loader)
connected_camera_idxs = sfm_result.get_valid_camera_indices()
expected_poses = [
self.loader.get_camera_pose(i) for i in connected_camera_idxs
]
self.assertTrue(
comp_utils.compare_global_poses(expected_poses,
expected_poses))
def assert_results(self, results_a: List[Optional[Point3]], results_b: List[Optional[Point3]]) -> None:
poses_a = [Pose3(R, t) if t is not None else None for R, t in zip(self._global_rotations_input, results_a)]
poses_b = [Pose3(R, t) if t is not None else None for R, t in zip(self._global_rotations_input, results_b)]
self.assertTrue(geometry_comparisons.compare_global_poses(poses_a, poses_b))
def test_compare_poses_exact(self):
"""Check pose comparison with exactly same inputs."""
self.assertTrue(
geometry_comparisons.compare_global_poses(POSE_LIST, POSE_LIST))