def test_bundle_adjust(self):
"""Tests the bundle adjustment for relative pose on a simulated scene."""
two_view_estimator = TwoViewEstimator(matcher=None,
verifier=None,
inlier_support_processor=None,
bundle_adjust_2view=True,
eval_threshold_px=4)
# Extract example poses.
i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation()
i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation()
i2Ti1 = Pose3(i1Ri2, i1ti2).inverse()
i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation()))
# Perform bundle adjustment.
i2Ri1_optimized, i2Ui1_optimized, corr_idxs = two_view_estimator.bundle_adjust(
keypoints_i1=self.keypoints_i1,
keypoints_i2=self.keypoints_i2,
verified_corr_idxs=self.corr_idxs,
camera_intrinsics_i1=Cal3Bundler(),
camera_intrinsics_i2=Cal3Bundler(),
i2Ri1_initial=i2Ei1.rotation(),
i2Ui1_initial=i2Ei1.direction(),
i2Ti1_prior=None,
)
# Assert
rotation_angular_error = comp_utils.compute_relative_rotation_angle(
i2Ri1_optimized, i2Ei1.rotation())
translation_angular_error = comp_utils.compute_relative_unit_translation_angle(
i2Ui1_optimized, i2Ei1.direction())
self.assertLessEqual(rotation_angular_error, 1)
self.assertLessEqual(translation_angular_error, 1)
np.testing.assert_allclose(corr_idxs, self.corr_idxs)
def compute_relative_pose_metrics(
i2Ri1_computed: Optional[Rot3],
i2Ui1_computed: Optional[Unit3],
wTi1_expected: Optional[Pose3],
wTi2_expected: Optional[Pose3],
) -> Tuple[Optional[float], Optional[float]]:
"""Compute the metrics on relative camera pose.
Args:
i2Ri1_computed: computed relative rotation.
i2Ui1_computed: computed relative translation direction.
i2Ti1_expected: expected relative pose.
Returns:
Rotation error, in degrees
Unit translation error, in degrees
"""
if wTi1_expected is not None and wTi2_expected is not None:
i2Ti1_expected = wTi2_expected.between(wTi1_expected)
R_error_deg = comp_utils.compute_relative_rotation_angle(
i2Ri1_computed, i2Ti1_expected.rotation())
U_error_deg = comp_utils.compute_relative_unit_translation_angle(
i2Ui1_computed, Unit3(i2Ti1_expected.translation()))
else:
return (None, None)
return (R_error_deg, U_error_deg)
def assert_results(self, results_a: VERIFIER_RESULT_TYPE, results_b: VERIFIER_RESULT_TYPE) -> None:
print(results_a[0])
print(results_b[0])
self.assertLessEqual(
geometry_comparisons.compute_relative_rotation_angle(results_a[0], results_b[0]),
ROT3_DIFF_ANGLE_THRESHOLD_DEG,
)
self.assertLessEqual(
geometry_comparisons.compute_relative_unit_translation_angle(results_a[1], results_b[1]),
UNIT3_DIFF_ANGLE_THRESHOLD_DEG,
)
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 compute_rotation_angle_metrics(
wRi_list: List[Optional[Rot3]],
gt_wRi_list: List[Optional[Pose3]]) -> StatsDict:
"""Computes statistics for the angle between estimated and GT rotations.
Assumes that the estimated and GT rotations have been aligned and do not
have a gauge freedom.
Args:
wRi_list: List of estimated camera rotations.
gt_wRi_list: List of ground truth camera rotations.
Returns:
A statistics dict of the metrics errors in degrees.
"""
errors = []
for (wRi, gt_wRi) in zip(wRi_list, gt_wRi_list):
errors.append(comp_utils.compute_relative_rotation_angle(wRi, gt_wRi))
return compute_errors_statistics(errors)
def __execute_verifier_test(
self,
keypoints_i1: Keypoints,
keypoints_i2: Keypoints,
match_indices: np.ndarray,
camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler,
i2Ri1_expected: Rot3,
i2Ui1_expected: Unit3,
verified_indices_expected: np.ndarray,
) -> None:
"""Execute the verification and compute results."""
i2Ri1_computed, i2Ui1_computed, verified_indices_computed, inlier_ratio_est_model = self.verifier.verify(
keypoints_i1,
keypoints_i2,
match_indices,
camera_intrinsics_i1,
camera_intrinsics_i2,
)
if i2Ri1_expected is None:
self.assertIsNone(i2Ri1_computed)
else:
angular_err = geom_comp_utils.compute_relative_rotation_angle(
i2Ri1_expected, i2Ri1_computed)
self.assertLess(
angular_err,
ROTATION_ANGULAR_ERROR_DEG_THRESHOLD,
msg=
f"Angular error {angular_err:.1f} vs. tol. {ROTATION_ANGULAR_ERROR_DEG_THRESHOLD:.1f}",
)
if i2Ui1_expected is None:
self.assertIsNone(i2Ui1_computed)
else:
self.assertLess(
geom_comp_utils.compute_relative_unit_translation_angle(
i2Ui1_expected, i2Ui1_computed),
DIRECTION_ANGULAR_ERROR_DEG_THRESHOLD,
)
np.testing.assert_array_equal(verified_indices_computed,
verified_indices_expected)
def compute_rotation_angle_metric(
wRi_list: List[Optional[Rot3]],
gt_wRi_list: List[Optional[Pose3]]) -> GtsfmMetric:
"""Computes statistics for the angle between estimated and GT rotations.
Assumes that the estimated and GT rotations have been aligned and do not
have a gauge freedom.
Args:
wRi_list: List of estimated camera rotations.
gt_wRi_list: List of ground truth camera rotations.
Returns:
A GtsfmMetric for the rotation angle errors, in degrees.
"""
errors = []
for (wRi, gt_wRi) in zip(wRi_list, gt_wRi_list):
if wRi is not None and gt_wRi is not None:
errors.append(
comp_utils.compute_relative_rotation_angle(wRi, gt_wRi))
return GtsfmMetric("rotation_angle_error_deg", errors)
def compute_relative_pose_metrics(
i2Ri1_computed: Optional[Rot3],
i2Ui1_computed: Optional[Unit3],
i2Ti1_expected: Pose3,
) -> Tuple[Optional[float], Optional[float]]:
"""Compute the metrics on relative camera pose.
Args:
i2Ri1_computed: computed relative rotation.
i2Ui1_computed: computed relative translation direction.
i2Ti1_expected: expected relative pose.
Returns:
Rotation error, in degrees
Unit translation error, in degrees
"""
R_error_deg = comp_utils.compute_relative_rotation_angle(
i2Ri1_computed, i2Ti1_expected.rotation())
U_error_deg = comp_utils.compute_relative_unit_translation_angle(
i2Ui1_computed, Unit3(i2Ti1_expected.translation()))
return (R_error_deg, U_error_deg)
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)