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_epipolar_distances(normalized_coords_i1: np.ndarray,
normalized_coords_i2: np.ndarray,
i2Ei1: EssentialMatrix) -> Optional[np.ndarray]:
"""Compute symmetric point-line epipolar distances between normalized coordinates of correspondences.
Args:
normalized_coords_i1: normalized coordinates in image i1, of shape Nx2.
normalized_coords_i2: corr. normalized coordinates in image i2, of shape Nx2.
i2Ei1: essential matrix between two images
Returns:
Symmetric epipolar distances for each row of the input, of shape N.
"""
if (normalized_coords_i1 is None or normalized_coords_i1.size == 0
or normalized_coords_i2 is None or normalized_coords_i2.size == 0):
return None
# construct the essential matrix in the opposite directin
i2Ti1 = Pose3(i2Ei1.rotation(), i2Ei1.direction().point3())
i1Ti2 = i2Ti1.inverse()
i1Ei2 = EssentialMatrix(i1Ti2.rotation(), Unit3(i1Ti2.translation()))
# get lines in i2 and i1
epipolar_lines_i2 = feature_utils.convert_to_epipolar_lines(
normalized_coords_i1, i2Ei1)
epipolar_lines_i1 = feature_utils.convert_to_epipolar_lines(
normalized_coords_i2, i1Ei2)
# compute two distances and average them
return 0.5 * (feature_utils.compute_point_line_distances(
normalized_coords_i1, epipolar_lines_i1) +
feature_utils.compute_point_line_distances(
normalized_coords_i2, epipolar_lines_i2))