def test_convert_to_epipolar_lines_none_input(self):
"""Test conversion of None to epipolar lines using the essential matrix."""
points = None
E_matrix = EssentialMatrix(Rot3.RzRyRx(0, np.deg2rad(45), 0), Unit3(np.array([-5, 2, 0])))
F_matrix = E_matrix.matrix() # using identity intrinsics
computed = feature_utils.convert_to_epipolar_lines(points, F_matrix)
self.assertIsNone(computed)
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 essential_to_fundamental_matrix(
i2Ei1: EssentialMatrix, camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler) -> np.ndarray:
"""Converts the essential matrix to fundamental matrix using camera intrinsics.
Args:
i2Ei1: essential matrix which maps points in image #i1 to lines in image #i2.
camera_intrinsics_i1: intrinsics for image #i1.
camera_intrinsics_i2: intrinsics for image #i2.
Returns:
Fundamental matrix i2Fi1 as numpy array of shape (3x3).
"""
return np.linalg.inv(
camera_intrinsics_i2.K().T) @ i2Ei1.matrix() @ np.linalg.inv(
camera_intrinsics_i1.K())
def convert_to_epipolar_lines(normalized_coordinates_i1: np.ndarray, i2Ei1: EssentialMatrix) -> Optional[np.array]:
"""Convert coordinates to epipolar lines in image i2.
The epipolar line in image i2 is given by i2Ei1 @ x_i1. A point x_i2 is on this line if x_i2^T @ i2Ei1 @ x_i1 = 0.
Args:
normalized_coordinates_i1: normalized coordinates in i1, of shape Nx2.
i2Ei1: essential matrix.
Returns:
Corr. epipolar lines in i2, of shape Nx3.
"""
if normalized_coordinates_i1 is None or normalized_coordinates_i1.size == 0:
return None
epipolar_lines = convert_to_homogenous_coordinates(normalized_coordinates_i1) @ i2Ei1.matrix().T
return epipolar_lines