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))
def compute_epipolar_distances_sq_sampson(
coordinates_i1: np.ndarray, coordinates_i2: np.ndarray,
i2Fi1: np.ndarray) -> Optional[np.ndarray]:
"""Compute the sampson distance between corresponding coordinates in two images. Sampson distance is the first
order approximation of the reprojection error, and well-estimates the gold standard reprojection error at low
values of the reprojection error.
The squared Sampson distance is computed in OpenCV (Ref 3) as well COLMAP (Ref 4). OpenCV uses the squared sampson
error to find inliers in RANSAC (Ref 5).
References:
1 "Fathy et al., Fundamental Matrix Estimation: A Study of Error Criteria", https://arxiv.org/abs/1706.07886
2 "Hartley, R.~I. et al. Multiple View Geometry in Computer Vision.. Cambridge University Press, Pg 288"
3 https://github.com/opencv/opencv/blob/29fb4f98b10767008d0751dd064023727d9d3e5d/modules/calib3d/src/five-point.cpp#L375 # noqa: E501
4 https://github.com/colmap/colmap/blob/9f3a75ae9c72188244f2403eb085e51ecf4397a8/src/estimators/utils.cc#L87
5 https://github.com/opencv/opencv/blob/29fb4f98b10767008d0751dd064023727d9d3e5d/modules/calib3d/src/ptsetreg.cpp#L85 # noqa: E731
Algorithm:
- l2 = i2Fi1 @ x1
- l1 = x2.T @ i2Fi1 = [a1, b1, c2]
- n1 = EuclideanNorm(Normal(l1)) = sqrt(a1^2 + b1^2)
- n2 = EuclideanNorm(Normal(l2))
- Sampson^2 = ( l1 @ x1 )^2 / (n1^2 + n2^2)
Args:
coordinates_i1: coordinates in image i1, of shape Nx2.
coordinates_i2: corr. coordinates in image i2, of shape Nx2.
i2Fi1: fundamental matrix between two images.
Returns:
Sampson distance for each row of the input, of shape N.
"""
if coordinates_i1 is None or coordinates_i1.size == 0 or coordinates_i2 is None or coordinates_i2.size == 0:
return None
epipolar_lines_i2 = feature_utils.convert_to_epipolar_lines(
coordinates_i1, i2Fi1) # Ex1
epipolar_lines_i1 = feature_utils.convert_to_epipolar_lines(
coordinates_i2, i2Fi1.T) # Etx2
line_sq_norms_i1 = np.sum(np.square(epipolar_lines_i1[:, :2]), axis=1)
line_sq_norms_i2 = np.sum(np.square(epipolar_lines_i2[:, :2]), axis=1)
numerator = np.square(
feature_utils.point_line_dotproduct(coordinates_i1, epipolar_lines_i1))
denominator = line_sq_norms_i1 + line_sq_norms_i2
return numerator / denominator
def test_convert_to_epipolar_lines_empty_input(self):
"""Test conversion of 0 2D points to epipolar lines using the essential matrix."""
points = np.array([]) # 2d points in homogenous coordinates
f_matrix = EssentialMatrix(Rot3.RzRyRx(0, np.deg2rad(45), 0), Unit3(np.array([-5, 2, 0]))).matrix()
computed = feature_utils.convert_to_epipolar_lines(points, f_matrix)
self.assertIsNone(computed)
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 compute_epipolar_distances_sq_sed(
coordinates_i1: np.ndarray, coordinates_i2: np.ndarray,
i2Fi1: np.ndarray) -> Optional[np.ndarray]:
"""Compute symmetric point-line epipolar squared distance between corresponding coordinates in two images. The
squared SED is the geometric point-line distance and is an over-estimate of the gold-standard reprojection error.
The over-estimate can reject correspondences which are actually inliers.
References:
1 "Fathy et al., Fundamental Matrix Estimation: A Study of Error Criteria", https://arxiv.org/abs/1706.07886
2 "Hartley, R.~I. et al. Multiple View Geometry in Computer Vision.. Cambridge University Press, Pg 288"
Algorithm:
- l2 = i2Fi1 @ x1
- l1 = x2.T @ i2Fi1 = [a1, b1, c1]
- n1 = EuclideanNorm(Normal(l1)) = sqrt(a1^2 + b1^2)
- n2 = EuclideanNorm(Normal(l2))
- SED^2 = ( l1 @ x1 )^2 * (1/n1^2 + 1/n2^2)
Args:
coordinates_i1: coordinates in image i1, of shape Nx2.
coordinates_i2: corr. coordinates in image i2, of shape Nx2.
i2Fi1: fundamental matrix between two images.
Returns:
Symmetric epipolar point-line distances for each row of the input, of shape N.
"""
if coordinates_i1 is None or coordinates_i1.size == 0 or coordinates_i2 is None or coordinates_i2.size == 0:
return None
epipolar_lines_i2 = feature_utils.convert_to_epipolar_lines(
coordinates_i1, i2Fi1) # Ex1
epipolar_lines_i1 = feature_utils.convert_to_epipolar_lines(
coordinates_i2, i2Fi1.T) # Etx2
numerator = np.square(
feature_utils.point_line_dotproduct(coordinates_i1, epipolar_lines_i1))
line_sq_norms_i1 = np.sum(np.square(epipolar_lines_i1[:, :2]), axis=1)
line_sq_norms_i2 = np.sum(np.square(epipolar_lines_i2[:, :2]), axis=1)
return numerator * (1 / line_sq_norms_i1 + 1 / line_sq_norms_i2)
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 test_convert_to_epipolar_lines_valid_input(self):
"""Test conversion of valid 2D points to epipolar lines using the essential matrix."""
points = np.array([
[10.0, -5.0],
[3.5, 20.0],
]) # 2d points in homogenous coordinates
essential_mat = EssentialMatrix(Rot3.RzRyRx(0, np.deg2rad(45), 0),
Unit3(np.array([-5, 2, 0])))
computed = feature_utils.convert_to_epipolar_lines(
points, essential_mat)
expected = np.array([
[-2.36351579, -5.90878948, 1.75364193],
[-0.65653216, -1.64133041, -19.75129171],
])
np.testing.assert_allclose(computed, expected)
