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 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 _get_measurement_angle_errors(
i1_i2_pairs: Set[Tuple[int, int]],
i2Ui1_measurements: Dict[Tuple[int, int], Unit3],
gt_i2Ui1_measurements: Dict[Tuple[int, int], Unit3],
) -> List[Optional[float]]:
"""Returns a list of the angle between i2Ui1_measurements and gt_i2Ui1_measurements for every
(i1, i2) in i1_i2_pairs.
Args:
i1_i2_pairs: List of (i1, i2) tuples for which the angles must be computed.
i2Ui1_measurements: Measured translation direction of i1 WRT i2.
gt_i2Ui1_measurements: Ground truth translation direction of i1 WRT i2.
Returns:
List of angles between the measured and ground truth translation directions.
"""
errors: List[Optional[float]] = []
for (i1, i2) in i1_i2_pairs:
if (i1, i2) in i2Ui1_measurements and (i1, i2) in gt_i2Ui1_measurements:
errors.append(
comp_utils.compute_relative_unit_translation_angle(
i2Ui1_measurements[(i1, i2)], gt_i2Ui1_measurements[(i1, i2)]
)
)
return errors
def calculate_triangulation_angle_in_degrees(
camera_1: PinholeCameraCal3Bundler, camera_2: PinholeCameraCal3Bundler,
point_3d: np.ndarray) -> float:
"""Calculates the angle formed at the 3D point by the rays backprojected from 2 cameras.
In the setup with X (point_3d) and two cameras C1 and C2, the triangulation angle is the angle between rays C1-X
and C2-X, i.e. the angle subtendted at the 3d point.
X
/ \
/ \
/ \
C1 C2
References:
- https://github.com/colmap/colmap/blob/dev/src/base/triangulation.cc#L122
Args:
camera_1: the first camera.
camera_2: the second camera.
point_3d: the 3d point which is imaged by the two camera centers, and where the angle between the light rays
associated with the measurements are computed.
Returns:
the angle formed at the 3d point, in degrees.
"""
camera_center_1: np.ndarray = camera_1.pose().translation()
camera_center_2: np.ndarray = camera_2.pose().translation()
# compute the two rays
ray_1 = point_3d - camera_center_1
ray_2 = point_3d - camera_center_2
return geometry_utils.compute_relative_unit_translation_angle(
Unit3(ray_1), Unit3(ray_2))
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_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 __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_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)
