def fundamental_to_essential_matrix(
i2Fi1: np.ndarray, camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler) -> np.ndarray:
"""Converts the fundamental matrix to essential matrix using camera intrinsics.
Args:
i2Fi1: fundamental 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:
Estimated essential matrix i2Ei1 as numpy array of shape (3x3).
"""
return camera_intrinsics_i2.K().T @ i2Fi1 @ camera_intrinsics_i1.K()
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 verify(
self,
keypoints_i1: Keypoints,
keypoints_i2: Keypoints,
match_indices: np.ndarray,
camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler,
) -> Tuple[Optional[Rot3], Optional[Unit3], np.ndarray, float]:
"""Performs verification of correspondences between two images to recover the relative pose and indices of
verified correspondences.
Args:
keypoints_i1: detected features in image #i1.
keypoints_i2: detected features in image #i2.
match_indices: matches as indices of features from both images, of shape (N3, 2), where N3 <= min(N1, N2).
camera_intrinsics_i1: intrinsics for image #i1.
camera_intrinsics_i2: intrinsics for image #i2.
Returns:
Estimated rotation i2Ri1, or None if it cannot be estimated.
Estimated unit translation i2Ui1, or None if it cannot be estimated.
Indices of verified correspondences, of shape (N, 2) with N <= N3. These are subset of match_indices.
Inlier ratio w.r.t. the estimated model, i.e. #ransac inliers / # putative matches.
"""
if match_indices.shape[0] < self._min_matches:
return self._failure_result
if self._use_intrinsics_in_verification:
uv_norm_i1 = feature_utils.normalize_coordinates(
keypoints_i1.coordinates, camera_intrinsics_i1)
uv_norm_i2 = feature_utils.normalize_coordinates(
keypoints_i2.coordinates, camera_intrinsics_i2)
# OpenCV can fail here, for some reason
if match_indices.shape[0] < 6:
return self._failure_result
if np.amax(match_indices[:, 1]) >= uv_norm_i2.shape[0]:
print("Out of bounds access w/ keypoints",
keypoints_i2.coordinates[:10])
if np.amax(match_indices[:, 0]) >= uv_norm_i1.shape[0]:
print("Out of bounds access w/ keypoints",
keypoints_i1.coordinates[:10])
# Use larger focal length, among the two choices, to yield a stricter threshold as (threshold_px / fx).
fx = max(camera_intrinsics_i1.K()[0, 0],
camera_intrinsics_i2.K()[0, 0])
i2Ei1, inlier_mask = self.estimate_E(uv_norm_i1=uv_norm_i1,
uv_norm_i2=uv_norm_i2,
match_indices=match_indices,
fx=fx)
else:
i2Fi1, inlier_mask = self.estimate_F(keypoints_i1=keypoints_i1,
keypoints_i2=keypoints_i2,
match_indices=match_indices)
i2Ei1 = verification_utils.fundamental_to_essential_matrix(
i2Fi1, camera_intrinsics_i1, camera_intrinsics_i2)
inlier_idxs = np.where(inlier_mask.ravel() == 1)[0]
v_corr_idxs = match_indices[inlier_idxs]
inlier_ratio_est_model = np.mean(inlier_mask)
(i2Ri1, i2Ui1
) = verification_utils.recover_relative_pose_from_essential_matrix(
i2Ei1,
keypoints_i1.coordinates[v_corr_idxs[:, 0]],
keypoints_i2.coordinates[v_corr_idxs[:, 1]],
camera_intrinsics_i1,
camera_intrinsics_i2,
)
return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model
