def verify_pydegensac_fundam(kps1, kps2, tentatives, th=1.0, n_iter=10000):
src_pts = np.float32([kps1[m.queryIdx].pt
for m in tentatives]).reshape(-1, 2)
dst_pts = np.float32([kps2[m.trainIdx].pt
for m in tentatives]).reshape(-1, 2)
F, mask = pydegensac.findFundamentalMatrix(src_pts,
dst_pts,
th,
0.999,
n_iter,
enable_degeneracy_check=True)
print('pydegensac found {} inliers'.format(
int(deepcopy(mask).astype(np.float32).sum())))
return F, mask
def verify_with_approximate_intrinsics(
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]:
"""Estimates the essential matrix and verifies the feature matches.
Note: this function is preferred when camera intrinsics are approximate (i.e from image size/exif). The feature
coordinates are used to compute the fundamental matrix, which is then converted to the essential matrix.
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.
"""
verified_indices = np.array([], dtype=np.uint32)
# check if we don't have the minimum number of points
if match_indices.shape[0] < self.min_pts:
return None, None, verified_indices
i2Fi1, mask = pydegensac.findFundamentalMatrix(
keypoints_i1.coordinates[match_indices[:, 0]],
keypoints_i2.coordinates[match_indices[:, 1]],
)
inlier_idxes = np.where(mask.ravel() == 1)[0]
i2Ei1_matrix = verification_utils.fundamental_to_essential_matrix(
i2Fi1, camera_intrinsics_i1, camera_intrinsics_i2)
i2Ri1, i2Ui1 = verification_utils.recover_relative_pose_from_essential_matrix(
i2Ei1_matrix,
keypoints_i1.coordinates[match_indices[inlier_idxes, 0]],
keypoints_i2.coordinates[match_indices[inlier_idxes, 1]],
camera_intrinsics_i1,
camera_intrinsics_i2,
)
return i2Ri1, i2Ui1, match_indices[inlier_idxes]
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]:
"""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 of w.r.t. the estimated model, i.e. the #final RANSAC inliers/ #putatives.
"""
if match_indices.shape[0] < self._min_matches:
return self._failure_result
i2Fi1, inlier_mask = pydegensac.findFundamentalMatrix(
keypoints_i1.coordinates[match_indices[:, 0]],
keypoints_i2.coordinates[match_indices[:, 1]],
px_th=self._estimation_threshold_px,
)
inlier_idxs = np.where(inlier_mask.ravel() == 1)[0]
v_corr_idxs = match_indices[inlier_idxs]
inlier_ratio_est_model = np.mean(inlier_mask)
i2Ei1_matrix = verification_utils.fundamental_to_essential_matrix(
i2Fi1, camera_intrinsics_i1, camera_intrinsics_i2)
i2Ri1, i2Ui1 = verification_utils.recover_relative_pose_from_essential_matrix(
i2Ei1_matrix,
keypoints_i1.coordinates[match_indices[inlier_idxs, 0]],
keypoints_i2.coordinates[match_indices[inlier_idxs, 1]],
camera_intrinsics_i1,
camera_intrinsics_i2,
)
return i2Ri1, i2Ui1, v_corr_idxs, inlier_ratio_est_model
def matches2relapose_degensac(p1, p2, K1, K2, rthres=1):
import pydegensac
import cv2
# Move back to image center based coordinates
f1, f2 = K1[0, 0], K2[0, 0]
pc1 = np.array([K1[:2, 2]])
pc2 = np.array([K2[:2, 2]])
# Rescale to im2 's focal setting
p1 = (p1 - pc1) * f2 / f1
p2 = (p2 - pc2)
K = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1]])
K1 = K2 = K
F, inls = pydegensac.findFundamentalMatrix(p1, p2, rthres)
E = fund2ess(F, K1, K2)
inls = np.where(inls > 0)[0]
_, R, t, _ = cv2.recoverPose(E, p1[inls], p2[inls], K)
return E, inls, R, t
def __call__(self, left: ['N', 2], right: ['N', 2], K1: [3, 3], K2: [3,
3]):
left = left.cpu()
right = right.cpu()
K1 = K1.cpu()
K2 = K2.cpu()
if left.shape[0] < self.candidate_threshold:
raise EstimationFailedError()
F, mask = pydegensac.findFundamentalMatrix(
left.numpy(),
right.numpy(),
px_th=self.reprojection_threshold,
conf=self.confidence,
max_iters=self.max_iters)
# FIXME: how does pydegensac handle failure?
if mask is None:
raise EstimationFailedError()
mask = torch.from_numpy(mask)
F = torch.from_numpy(F).to(torch.float32)
E = K2.T @ F @ K1
try:
pose = _recover_pose(
E,
_normalize_coords(left[mask], K1),
_normalize_coords(right[mask], K2),
)
except cv2.error:
raise EstimationFailedError()
return pose, mask
def _cmp_estimate_E_without_intrinsics(cfg,
matches,
kps1,
kps2,
calib1,
calib2,
img1_fname=None,
img2_fname=None,
scales1=None,
scales2=None,
ori1=None,
ori2=None,
A1=None,
A2=None):
'''Estimate the Essential matrix from correspondences. Computes the
Fundamental Matrix first and then retrieves the Essential matrix assuming
known intrinsics.
'''
cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task)
geom = cfg.method_dict[cur_key]['geom']
min_matches = 8
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches)
if not is_valid:
return _fail()
if geom['method'] == 'cmp-degensac-f-laf':
sc1 = scales1[matches[0]]
sc2 = scales2[matches[1]]
ang1 = ori1[matches[0]]
ang2 = ori2[matches[1]]
if A1 is not None:
A1 = A1[matches[0]]
A2 = A2[matches[1]]
else:
A1 = None
A2 = None
laf1 = get_LAF(kp1, sc1, ang1, A1)
laf2 = get_LAF(kp2, sc2, ang2, A2)
# print (laf1[:2])
# print (laf2[:2])
F, mask_F = pyransac.findFundamentalMatrix(
laf1,
laf2,
geom['threshold'],
geom['confidence'],
geom['max_iter'],
2.0,
error_type=geom['error_type'],
symmetric_error_check=True,
enable_degeneracy_check=geom['degeneracy_check'])
elif geom['method'] == 'cmp-degensac-f':
F, mask_F = pyransac.findFundamentalMatrix(
kp1,
kp2,
geom['threshold'],
geom['confidence'],
geom['max_iter'],
0,
error_type=geom['error_type'],
symmetric_error_check=True,
enable_degeneracy_check=geom['degeneracy_check'])
elif geom['method'] == 'cmp-gc-ransac-f':
F, mask_F = pygcransac.findFundamentalMatrix(kp1, kp2,
geom['threshold'],
geom['confidence'],
geom['max_iter'])
elif geom['method'] == 'cmp-magsac-f':
F, mask_F = pymagsac.findFundamentalMatrix(kp1, kp2, geom['threshold'],
geom['confidence'],
geom['max_iter'])
else:
raise ValueError('Unknown method: {}'.format(geom['method']))
mask_F = mask_F.astype(bool).flatten()
# OpenCV can return multiple values as 6x3 or 9x3 matrices
if F is None:
return _fail()
elif F.shape[0] != 3:
Fs = np.split(F, len(F) / 3)
else:
Fs = [F]
if mask_F.sum() < 8:
return _fail()
# Find the best F
K1, K2 = calib1['K'], calib2['K']
kp1n = normalize_keypoints(kp1, K1)
kp2n = normalize_keypoints(kp2, K2)
E, num_inlier = None, 0
# mask_E_cheirality_check = None
for _F in Fs:
_E = np.matmul(np.matmul(K2.T, _F), K1)
_E = _E.astype(np.float64)
_num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1n[mask_F],
kp2n[mask_F])
if _num_inlier >= num_inlier:
num_inlier = _num_inlier
E = _E
# This is unused for now
# mask_E_cheirality_check = _mask.flatten().astype(bool)
# Return the initial list of matches (from F)
indices = matches[:, mask_F.flatten()]
return E, indices
def get_single_result(ms,
m,
method,
params,
w1=None,
h1=None,
w2=None,
h2=None):
mask = ms <= params['match_th']
tentatives = m[mask]
tentative_idxs = np.arange(len(mask))[mask]
src_pts = tentatives[:, :2]
dst_pts = tentatives[:, 2:]
if tentatives.shape[0] <= 10:
return np.eye(3), np.array([False] * len(mask))
if method == 'cv2f':
F, mask_inl = cv2.findFundamentalMat(src_pts,
dst_pts,
cv2.RANSAC,
params['inl_th'],
confidence=params['conf'])
if method == 'kornia':
#mask = ms <= params['match_th']
#tentatives = m[mask]
#tentative_idxs = np.arange(len(mask))[mask]
src_pts = m[:, :2]
dst_pts = m[:, 2:]
pts1 = torch.from_numpy(src_pts).view(1, -1, 2)
pts2 = torch.from_numpy(dst_pts).view(1, -1, 2)
weights = torch.from_numpy(1.0 - ms).view(1,
-1).pow(params['match_th'])
weights = TF.normalize(weights, dim=1)
F, mask_inl = kornia_find_fundamental_wdlt(pts1.float(), pts2.float(),
weights.float(), params)
elif method == 'cv2eimg':
tent_norm, T1, T2 = norm_test_data(tentatives, w1, h1, w2, h2)
#print (T1)
#K1 = compute_T_with_imagesize(w1,h1)
#K2 = compute_T_with_imagesize(w2,h2)
#print (K1, K2)
#src_pts = normalize_keypoints(src_pts, K1)
#dst_pts = normalize_keypoints(dst_pts, K2)
#print (src_pts)
E, mask_inl = cv2.findEssentialMat(tent_norm[:, :2],
tent_norm[:, 2:],
np.eye(3),
cv2.RANSAC,
threshold=params['inl_th'],
prob=params['conf'])
F = np.matmul(np.matmul(T2.T, E), T1)
elif method == 'pyransac':
F, mask_inl = pydegensac.findFundamentalMatrix(
src_pts,
dst_pts,
params['inl_th'],
conf=params['conf'],
max_iters=params['maxiter'],
enable_degeneracy_check=False)
elif method == 'degensac':
F, mask_inl = pydegensac.findFundamentalMatrix(
src_pts,
dst_pts,
params['inl_th'],
conf=params['conf'],
max_iters=params['maxiter'],
enable_degeneracy_check=True)
elif method == 'sklearn':
try:
#print(src_pts.shape, dst_pts.shape)
F, mask_inl = skransac([src_pts, dst_pts],
FundamentalMatrixTransform,
min_samples=8,
residual_threshold=params['inl_th'],
max_trials=params['maxiter'],
stop_probability=params['conf'])
mask_inl = mask_inl.astype(bool).flatten()
F = F.params
except Exception as e:
print("Fail!", e)
return np.eye(3), np.array([False] * len(mask))
else:
raise ValueError('Unknown method')
final_inliers = np.array([False] * len(mask))
if F is not None:
for i, x in enumerate(mask_inl):
final_inliers[tentative_idxs[i]] = x
return F, final_inliers