def _cv2_estimate_E_with_intrinsics(cfg, matches, kps1, kps2, calib1, calib2):
'''Estimate the Essential matrix from correspondences. Assumes known
intrinsics.
'''
# Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html
# Defalt values for confidence: 0.99 (F), 0.999 (E)
# Default value for the reprojection threshold: 3
cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task)
geom = cfg.method_dict[cur_key]['geom']
if geom['method'].lower() == 'cv2-ransac-e':
cv_method = 'FM_RANSAC'
cv_threshold = geom['threshold']
cv_confidence = geom['confidence']
elif geom['method'].lower() == 'cv2-lmeds-e':
cv_method = 'FM_LMEDS'
cv_threshold = None
cv_confidence = geom['confidence']
else:
raise ValueError('Unknown method to estimate E')
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, 5)
if not is_valid:
return _fail()
# Normalize keypoints with ground truth intrinsics
kp1_n = normalize_keypoints(kp1, calib1['K'])
kp2_n = normalize_keypoints(kp2, calib2['K'])
cv2.setRNGSeed(cfg.opencv_seed)
E, mask_E = cv2.findEssentialMat(kp1_n,
kp2_n,
method=getattr(cv2, cv_method),
threshold=cv_threshold,
prob=cv_confidence)
mask_E = mask_E.astype(bool).flatten()
# OpenCV can return multiple values as 6x3 or 9x3 matrices
if E is None:
return _fail()
elif E.shape[0] != 3:
Es = np.split(E, len(E) / 3)
# Or a single matrix
else:
Es = [E]
# Find the best E
E, num_inlier = None, 0
# mask_E_cheirality_check = None
for _E in Es:
_num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1_n[mask_E],
kp2_n[mask_E])
if _num_inlier >= num_inlier:
num_inlier = _num_inlier
E = _E
# This is unused for now
# mask_E_cheirality_check = _mask.flatten().astype(bool)
indices = matches[:, mask_E.flatten()]
return E, indices
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 _cmp_estimate_E_with_intrinsics(cfg,
matches,
kps1,
kps2,
calib1,
calib2,
img1_fname=None,
img2_fname=None):
'''Estimate the Essential matrix from correspondences. Assumes known
intrinsics.
'''
# Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html
# Defalt values for confidence: 0.99 (F), 0.999 (E)
# Default value for the reprojection threshold: 3
# (We set them to -1 when not applicable as OpenCV complains otherwise)
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, 5)
if not is_valid:
return _fail()
# Normalize keypoints with ground truth intrinsics
kp1_n = normalize_keypoints(kp1, calib1['K'])
kp2_n = normalize_keypoints(kp2, calib2['K'])
if img1_fname is not None:
s = (cv2.imread(img1_fname)).size
h1, w1 = s[0], s[1]
s = (cv2.imread(img2_fname)).size
h2, w2 = s[0], s[1]
else:
raise ValueError('Requires image filenames')
cv2.setRNGSeed(cfg.opencv_seed)
E, mask_E = pygcransac.findEssentialMatrix(kp1, kp2, calib1['K'],
calib2['K'], h1, w1, h2, w2,
cfg.method_geom['threshold'],
cfg.method_geom['confidence'],
cfg.method_geom['max_iter'])
mask_E = mask_E.astype(bool).flatten()
# OpenCV can return multiple values as 6x3 or 9x3 matrices
if E is None:
return _fail()
elif E.shape[0] != 3:
Es = np.split(E, len(E) / 3)
# Or a single matrix
else:
Es = [E]
# Find the best E
E, num_inlier = None, 0
# mask_E_cheirality_check = None
for _E in Es:
_num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1_n[mask_E],
kp2_n[mask_E])
if _num_inlier >= num_inlier:
num_inlier = _num_inlier
E = _E
# This is unused for now
# mask_E_cheirality_check = _mask.flatten().astype(bool)
indices = matches[:, mask_E.flatten()]
return E, indices
def main(cfg):
'''Visualization of stereo keypoints and matches.
Parameters
----------
cfg: Namespace
Configurations for running this part of the code.
'''
# Files should not be named to prevent (easy) abuse
# Instead we use 0, ..., cfg.num_viz_stereo_pairs
viz_folder_hq, viz_folder_lq = get_stereo_viz_folder(cfg)
print(' -- Visualizations, stereo: "{}/{}"'.format(cfg.dataset, cfg.scene))
t_start = time()
# Load deprecated images list
deprecated_images_all = load_json(cfg.json_deprecated_images)
if cfg.dataset in deprecated_images_all and cfg.scene in deprecated_images_all[
cfg.dataset]:
deprecated_images = deprecated_images_all[cfg.dataset][cfg.scene]
else:
deprecated_images = []
# Load keypoints, matches and errors
keypoints_dict = load_h5_valid_image(get_kp_file(cfg), deprecated_images)
matches_dict = load_h5_valid_image(get_match_file(cfg), deprecated_images)
ransac_inl_dict = load_h5_valid_image(get_geom_inl_file(cfg),
deprecated_images)
# Hacky: We need to recompute the errors, loading only for the keys
data_dir = get_data_path(cfg)
pairs_all = get_pairs_per_threshold(data_dir)['0.1']
pairs = []
for pair in pairs_all:
if all([key not in deprecated_images for key in pair.split('-')]):
pairs += [pair]
# Create results folder if it does not exist
if not os.path.exists(viz_folder_hq):
os.makedirs(viz_folder_hq)
if not os.path.exists(viz_folder_lq):
os.makedirs(viz_folder_lq)
# Sort alphabetically and pick different images
sorted_keys = sorted(pairs)
picked = []
pairs = []
for pair in sorted_keys:
fn1, fn2 = pair.split('-')
if fn1 not in picked and fn2 not in picked:
picked += [fn1, fn2]
pairs += [pair]
if len(pairs) == cfg.num_viz_stereo_pairs:
break
# Load depth maps
depth = {}
if cfg.dataset != 'googleurban':
for pair in pairs:
files = pair.split('-')
for f in files:
if f not in depth:
depth[f] = load_depth(
os.path.join(data_dir, 'depth_maps',
'{}.h5'.format(f)))
# Generate and save the images
for i, pair in enumerate(pairs):
# load metadata
fn1, fn2 = pair.split('-')
calib_dict = load_calib([
os.path.join(data_dir, 'calibration',
'calibration_{}.h5'.format(fn1)),
os.path.join(data_dir, 'calibration',
'calibration_{}.h5'.format(fn2))
])
calc1 = calib_dict[fn1]
calc2 = calib_dict[fn2]
inl = ransac_inl_dict[pair]
# Get depth for keypoints
kp1 = keypoints_dict[fn1]
kp2 = keypoints_dict[fn2]
# Normalize keypoints
kp1n = normalize_keypoints(kp1, calc1['K'])
kp2n = normalize_keypoints(kp2, calc2['K'])
# Get {R, t} from calibration information
R_1, t_1 = calc1['R'], calc1['T'].reshape((3, 1))
R_2, t_2 = calc2['R'], calc2['T'].reshape((3, 1))
# Compute dR, dt
dR = np.dot(R_2, R_1.T)
dT = t_2 - np.dot(dR, t_1)
if cfg.dataset == 'phototourism':
kp1_int = np.round(kp1).astype(int)
kp2_int = np.round(kp2).astype(int)
kp1_int[:, 1] = np.clip(kp1_int[:, 1], 0, depth[fn1].shape[0] - 1)
kp1_int[:, 0] = np.clip(kp1_int[:, 0], 0, depth[fn1].shape[1] - 1)
kp2_int[:, 1] = np.clip(kp2_int[:, 1], 0, depth[fn2].shape[0] - 1)
kp2_int[:, 0] = np.clip(kp2_int[:, 0], 0, depth[fn2].shape[1] - 1)
d1 = np.expand_dims(depth[fn1][kp1_int[:, 1], kp1_int[:, 0]],
axis=-1)
d2 = np.expand_dims(depth[fn2][kp2_int[:, 1], kp2_int[:, 0]],
axis=-1)
# Project with depth
kp1n_p, kp2n_p = get_projected_kp(kp1n, kp2n, d1, d2, dR, dT)
kp1_p = unnormalize_keypoints(kp1n_p, calc2['K'])
kp2_p = unnormalize_keypoints(kp2n_p, calc1['K'])
# Re-index keypoints from matches
kp1_inl = kp1[inl[0]]
kp2_inl = kp2[inl[1]]
kp1_p_inl = kp1_p[inl[0]]
kp2_p_inl = kp2_p[inl[1]]
kp1n_inl = kp1n[inl[0]]
kp2n_inl = kp2n[inl[1]]
kp1n_p_inl = kp1n_p[inl[0]]
kp2n_p_inl = kp2n_p[inl[1]]
d1_inl = d1[inl[0]]
d2_inl = d2[inl[1]]
# Filter out keypoints with invalid depth
nonzero_index = np.nonzero(np.squeeze(d1_inl * d2_inl))
zero_index = np.where(np.squeeze(d1_inl * d2_inl) == 0)[0]
kp1_inl_nonzero = kp1_inl[nonzero_index]
kp2_inl_nonzero = kp2_inl[nonzero_index]
kp1_p_inl_nonzero = kp1_p_inl[nonzero_index]
kp2_p_inl_nonzero = kp2_p_inl[nonzero_index]
kp1n_inl_nonzero = kp1n_inl[nonzero_index]
kp2n_inl_nonzero = kp2n_inl[nonzero_index]
kp1n_p_inl_nonzero = kp1n_p_inl[nonzero_index]
kp2n_p_inl_nonzero = kp2n_p_inl[nonzero_index]
# Compute symmetric distance using the depth image
d = get_truesym(kp1_inl_nonzero, kp2_inl_nonzero,
kp1_p_inl_nonzero, kp2_p_inl_nonzero)
else:
# All points are valid for computing the epipolar distance.
zero_index = []
# Compute symmetric epipolar distance for every match.
kp1_inl_nonzero = kp1[inl[0]]
kp2_inl_nonzero = kp2[inl[1]]
kp1n_inl_nonzero = kp1n[inl[0]]
kp2n_inl_nonzero = kp2n[inl[1]]
# d = np.zeros(inl.shape[1])
d = get_episym(kp1n_inl_nonzero, kp2n_inl_nonzero, dR, dT)
# canvas
im, v_offset, h_offset = build_composite_image(
os.path.join(
data_dir, 'images',
fn1 + ('.png' if cfg.dataset == 'googleurban' else '.jpg')),
os.path.join(
data_dir, 'images',
fn2 + ('.png' if cfg.dataset == 'googleurban' else '.jpg')),
margin=5,
axis=1 if
(not cfg.viz_composite_vert or cfg.dataset == 'googleurban'
or cfg.dataset == 'pragueparks') else 0)
plt.figure(figsize=(10, 10))
plt.imshow(im)
linewidth = 2
# Plot matches on points without depth
for idx in range(len(zero_index)):
plt.plot(
(kp1_inl[idx, 0] + h_offset[0], kp2_inl[idx, 0] + h_offset[1]),
(kp1_inl[idx, 1] + v_offset[0], kp2_inl[idx, 1] + v_offset[1]),
color='b',
linewidth=linewidth)
# Plot matches
# Points are normalized by the focals, which are on average ~670.
max_dist = 5
if cfg.dataset == 'googleurban':
max_dist = 2e-4
if cfg.dataset == 'pragueparks':
max_dist = 2e-4
cmap = matplotlib.cm.get_cmap('summer')
order = list(range(len(d)))
random.shuffle(order)
for idx in order:
if d[idx] <= max_dist:
min_val = 0
max_val = 255 - min_val
col = cmap(
int(max_val * (1 - (max_dist - d[idx]) / max_dist) +
min_val))
# col = cmap(255 * (max_dist - d[idx]) / max_dist)
else:
col = 'r'
plt.plot((kp1_inl_nonzero[idx, 0] + h_offset[0],
kp2_inl_nonzero[idx, 0] + h_offset[1]),
(kp1_inl_nonzero[idx, 1] + v_offset[0],
kp2_inl_nonzero[idx, 1] + v_offset[1]),
color=col,
linewidth=linewidth)
plt.tight_layout()
plt.axis('off')
viz_file_hq = os.path.join(viz_folder_hq, '{:05d}.png'.format(i))
viz_file_lq = os.path.join(viz_folder_lq, '{:05d}.jpg'.format(i))
plt.savefig(viz_file_hq, bbox_inches='tight')
# Convert with imagemagick
os.system('convert -quality 75 -resize \"500>\" {} {}'.format(
viz_file_hq, viz_file_lq))
plt.close()
print('Done [{:.02f} s.]'.format(time() - t_start))
def _skimage_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1,
calib2):
'''Estimate the Essential matrix from correspondences. Computes the
Fundamental Matrix first and then retrieves the Essential matrix assuming
known intrinsics.
'''
# Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html
# Defalt values for confidence: 0.99 (F), 0.999 (E)
# Default value for the reprojection threshold: 3
# (We set them to -1 when not applicable as OpenCV complains otherwise)
method_geom = cfg.method_geom['method']
if method_geom.lower() == 'skimage-ransac-f':
min_matches = 9
cv_reprojection_threshold = cfg.method_geom['threshold']
cv_confidence = cfg.method_geom['confidence']
max_iters = cfg.method_geom['max_iter']
else:
raise ValueError('Unknown method to estimate F')
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches)
if not is_valid:
return _fail()
if len(kp1) < 9:
return _fail()
try:
F, mask_F = skransac((kp1, kp2),
FundamentalMatrixTransform,
min_samples=8,
residual_threshold=cv_reprojection_threshold,
max_trials=max_iters,
stop_probability=cv_confidence)
except Exception:
return _fail()
mask_F = mask_F.astype(bool).flatten()
F = F.params
# 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]
# 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 main(cfg):
'''Visualization of stereo keypoints and matches.
Parameters
----------
cfg: Namespace
Configurations for running this part of the code.
'''
# Files should not be named to prevent (easy) abuse
# Instead we use 0, ..., cfg.num_viz_stereo_pairs
viz_folder_hq, viz_folder_lq = get_stereo_viz_folder(cfg)
# # Do not re-run if files already exist -- off for now
# if os.path.exists(viz_folder_lq):
# if all([
# os.path.exists(
# os.path.join(viz_folder_lq, 'stereo-{}.jpg'.format(i)))
# for i in range(cfg.num_viz_stereo_pairs)
# ]):
# print(' -- already exists, skipping stereo visualization')
# return
print(' -- Visualizations, stereo: "{}/{}"'.format(cfg.dataset, cfg.scene))
t_start = time()
# Load keypoints, matches and errors
keypoints_dict = load_h5(get_kp_file(cfg))
matches_dict = load_h5(get_match_file(cfg))
# Hacky: We need to recompute the errors, loading only for the keys
errors_dict = load_h5(get_stereo_epipolar_final_match_file(cfg, th='0.1'))
# Get data directory
data_dir = get_data_path(cfg)
# Create results folder if it does not exist
if not os.path.exists(viz_folder_hq):
os.makedirs(viz_folder_hq)
if not os.path.exists(viz_folder_lq):
os.makedirs(viz_folder_lq)
# Sort alphabetically and pick different images
sorted_keys = sorted(errors_dict)
picked = []
pairs = []
for pair in sorted_keys:
fn1, fn2 = pair.split('-')
if fn1 not in picked and fn2 not in picked:
picked += [fn1, fn2]
pairs += [pair]
if len(pairs) == cfg.num_viz_stereo_pairs:
break
# Load all depth maps
depth = {}
for pair in pairs:
files = pair.split('-')
for f in files:
if f not in depth:
depth[f] = load_depth(
os.path.join(data_dir, 'depth_maps', '{}.h5'.format(f)))
# Generate and save the images
for i, pair in enumerate(pairs):
# load metadata
fn1, fn2 = pair.split('-')
calib_dict = load_calib([
os.path.join(data_dir, 'calibration',
'calibration_{}.h5'.format(fn1)),
os.path.join(data_dir, 'calibration',
'calibration_{}.h5'.format(fn2))
])
calc1 = calib_dict[fn1]
calc2 = calib_dict[fn2]
matches = matches_dict[pair]
ransac_inl_dict = load_h5(get_geom_inl_file(cfg))
inl = ransac_inl_dict[pair]
# Get depth for keypoints
kp1 = keypoints_dict[fn1]
kp2 = keypoints_dict[fn2]
kp1_int = np.round(kp1).astype(int)
kp2_int = np.round(kp2).astype(int)
kp1_int[:, 1] = np.clip(kp1_int[:, 1], 0, depth[fn1].shape[0] - 1)
kp1_int[:, 0] = np.clip(kp1_int[:, 0], 0, depth[fn1].shape[1] - 1)
d1 = np.expand_dims(depth[fn1][kp1_int[:, 1], kp1_int[:, 0]], axis=-1)
d2 = np.expand_dims(depth[fn2][kp2_int[:, 1], kp2_int[:, 0]], axis=-1)
# Get {R, t} from calibration information
R_1, t_1 = calc1['R'], calc1['T'].reshape((3, 1))
R_2, t_2 = calc2['R'], calc2['T'].reshape((3, 1))
# Compute dR, dt
dR = np.dot(R_2, R_1.T)
dT = t_2 - np.dot(dR, t_1)
# Normalize keypoints
kp1n = normalize_keypoints(kp1, calc1['K'])
kp2n = normalize_keypoints(kp2, calc2['K'])
# Project with depth
kp1n_p, kp2n_p = get_projected_kp(kp1n, kp2n, d1, d2, dR, dT)
kp1_p = unnormalize_keypoints(kp1n_p, calc2['K'])
kp2_p = unnormalize_keypoints(kp2n_p, calc1['K'])
# Re-index keypoints from matches
kp1_inl = kp1[inl[0]]
kp2_inl = kp2[inl[1]]
kp1_p_inl = kp1_p[inl[0]]
kp2_p_inl = kp2_p[inl[1]]
kp1n_inl = kp1n[inl[0]]
kp2n_inl = kp2n[inl[1]]
kp1n_p_inl = kp1n_p[inl[0]]
kp2n_p_inl = kp2n_p[inl[1]]
d1_inl = d1[inl[0]]
d2_inl = d2[inl[1]]
# Filter out keypoints with invalid depth
nonzero_index = np.nonzero(np.squeeze(d1_inl * d2_inl))
zero_index = np.where(np.squeeze(d1_inl * d2_inl) == 0)[0]
kp1_inl_nonzero = kp1_inl[nonzero_index]
kp2_inl_nonzero = kp2_inl[nonzero_index]
kp1_p_inl_nonzero = kp1_p_inl[nonzero_index]
kp2_p_inl_nonzero = kp2_p_inl[nonzero_index]
kp1n_inl_nonzero = kp1n_inl[nonzero_index]
kp2n_inl_nonzero = kp2n_inl[nonzero_index]
kp1n_p_inl_nonzero = kp1n_p_inl[nonzero_index]
kp2n_p_inl_nonzero = kp2n_p_inl[nonzero_index]
# Compute symmetric distance using the depth image
true_d = get_truesym(kp1_inl_nonzero, kp2_inl_nonzero,
kp1_p_inl_nonzero, kp2_p_inl_nonzero)
# canvas
im, v_offset, h_offset = build_composite_image(
os.path.join(data_dir, 'images', fn1 + '.jpg'),
os.path.join(data_dir, 'images', fn2 + '.jpg'),
margin=5,
axis=0 if cfg.viz_composite_vert else 1)
plt.figure(figsize=(10, 10))
plt.imshow(im)
linewidth = 2
# Plot matches on points without depth
for idx in range(len(zero_index)):
plt.plot((kp1[idx, 0] + h_offset[0], kp2[idx, 0] + h_offset[1]),
(kp1[idx, 1] + v_offset[0], kp2[idx, 1] + v_offset[1]),
color='b',
linewidth=linewidth)
# Plot matches on points with depth
max_dist = 5
cmap = matplotlib.cm.get_cmap('summer')
order = list(range(len(true_d)))
random.shuffle(order)
for idx in order:
if true_d[idx] <= max_dist:
min_val = 0
max_val = 255 - min_val
col = cmap(
int(max_val * (1 - (max_dist - true_d[idx]) / max_dist) +
min_val))
# col = cmap(255 * (max_dist - true_d[idx]) / max_dist)
else:
col = 'r'
plt.plot((kp1_inl_nonzero[idx, 0] + h_offset[0],
kp2_inl_nonzero[idx, 0] + h_offset[1]),
(kp1_inl_nonzero[idx, 1] + v_offset[0],
kp2_inl_nonzero[idx, 1] + v_offset[1]),
color=col,
linewidth=linewidth)
plt.tight_layout()
plt.axis('off')
viz_file_hq = os.path.join(viz_folder_hq, '{:05d}.png'.format(i))
viz_file_lq = os.path.join(viz_folder_lq, '{:05d}.jpg'.format(i))
plt.savefig(viz_file_hq, bbox_inches='tight')
# Convert with imagemagick
os.system('convert -quality 75 -resize \"500>\" {} {}'.format(
viz_file_hq, viz_file_lq))
plt.close()
print('done [{:.02f} s.]'.format(time() - t_start))
def _cv2_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1,
calib2):
'''Estimate the Essential matrix from correspondences. Computes the
Fundamental Matrix first and then retrieves the Essential matrix assuming
known intrinsics.
'''
# Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html
# Defalt values for confidence: 0.99 (F), 0.999 (E)
# Default value for the reprojection threshold: 3
# (We set them to -1 when not applicable as OpenCV complains otherwise)
cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task)
geom = cfg.method_dict[cur_key]['geom']
if geom['method'].lower() in ['cv2-ransac-f', 'cv2-patched-ransac-f']:
min_matches = 8
cv_method = 'FM_RANSAC'
cv_reprojection_threshold = geom['threshold']
cv_confidence = geom['confidence']
if geom['method'].lower() == 'cv2-patched-ransac-f':
cv_max_iter = geom['max_iter']
elif geom['method'].lower() == 'cv2-lmeds-f':
min_matches = 8
cv_method = 'FM_LMEDS'
cv_reprojection_threshold = -1
cv_confidence = geom['confidence']
elif geom['method'].lower() == 'cv2-7pt':
# This should actually be *equal* to 7? We'll probably never use it...
min_matches = 7
cv_method = 'FM_7POINT'
cv_reprojection_threshold = -1
cv_confidence = -1
elif geom['method'].lower() == 'cv2-8pt':
min_matches = 8
cv_method = 'FM_8POINT'
cv_reprojection_threshold = -1
cv_confidence = -1
else:
raise ValueError('Unknown method to estimate F')
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches)
if not is_valid:
return _fail()
cv2.setRNGSeed(cfg.opencv_seed)
# Temporary fix to allow for patched opencv
if geom['method'].lower() == 'cv2-patched-ransac-f':
F, mask_F = cv2.findFundamentalMat(
kp1,
kp2,
method=getattr(cv2, cv_method),
ransacReprojThreshold=cv_reprojection_threshold,
confidence=cv_confidence,
maxIters=cv_max_iter)
else:
F, mask_F = cv2.findFundamentalMat(
kp1,
kp2,
method=getattr(cv2, cv_method),
ransacReprojThreshold=cv_reprojection_threshold,
confidence=cv_confidence)
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]
# 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 _intel_estimate_E_without_intrinsics(cfg,
matches,
kps1,
kps2,
calib1,
calib2,
scales1=None,
scales2=None,
ori1=None,
ori2=None,
descs1=None,
descs2=None):
'''Estimate the Essential matrix from correspondences. Computes the
Fundamental Matrix first and then retrieves the Essential matrix assuming
known intrinsics.
'''
method_geom = cfg.method_geom['method']
min_matches = 8
threshold = cfg.method_geom['threshold']
postprocess = cfg.method_geom['postprocess']
is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches)
if not is_valid:
return _fail()
pts_dfe = np.concatenate([kp1.reshape(-1, 2), kp2.reshape(-1, 2)], axis=1)
if (descs1 is not None) and (descs2 is not None):
desc_distance = np.sqrt((
(descs1[matches[0, :]] - descs2[matches[1, :]])**2).mean(axis=1) +
1e-9)
desc_distance = desc_distance / (desc_distance.max() + 1e-6)
desc_distance = desc_distance.reshape(-1, 1)
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
matcher = cv2.FlannBasedMatcher(index_params, search_params)
prematch = matcher.knnMatch(descs1, descs2, k=2)
matches1 = []
ratio = []
matches_set1 = set(matches[0, :])
for m, n in prematch:
if m.queryIdx in matches_set1:
ratio.append(m.distance / n.distance)
ratio = np.array(ratio).reshape(-1, 1)
ratio = ratio / (ratio.max() + 1e-6)
assert len(ratio) == len(desc_distance)
else:
warnings.warn('No descriptor provided for DFE', UserWarning)
desc_distance = np.zeros((len(pts_dfe), 1))
if (scales1 is not None) and (scales2 is not None):
rel_scale = np.abs(scales1[matches[0, :]] -
scales2[matches[1, :]]).reshape(-1, 1)
rel_scale = rel_scale / (1e-6 + rel_scale.max())
else:
warnings.warn('No scale provided for DFE', UserWarning)
rel_scale = np.zeros((len(pts_dfe), 1))
if (ori1 is not None) and (ori2 is not None):
rel_orient = np.minimum(
np.abs(ori1[matches[0, :]] - ori2[matches[1, :]]),
np.abs(ori2[matches[1, :]] - ori1[matches[0, :]])).reshape(-1, 1)
rel_orient = rel_orient / (1e-6 + rel_orient.max())
else:
warnings.warn('No orientation provided for DFE', UserWarning)
rel_orient = np.zeros((len(pts_dfe), 1))
side_info = np.concatenate([desc_distance, rel_scale, rel_orient, ratio],
axis=1)
model = NormalizedEightPointNet(depth=3, side_info_size=4)
# TODO re-upload weights
model.load_state_dict(
torch.load('DFE_phototourism120.pt', map_location=torch.device('cpu')))
try:
if torch.cuda.is_available():
device = torch.device('cuda:0')
else:
device = torch.device('cpu')
except:
device = torch.device('cpu')
model = model.eval()
model = model.to(device)
pts_orig = pts_dfe.copy()
pts_dfe = torch.from_numpy(pts_dfe).to(device).unsqueeze(0).float()
side_info = torch.from_numpy(side_info).to(
torch.float).to(device).unsqueeze(0)
with torch.no_grad():
F_est, rescaling_1, rescaling_2, weights = model(pts_dfe, side_info)
F_est = rescaling_1.permute(0, 2, 1).bmm(F_est[-1].bmm(rescaling_2))
F_est = F_est / F_est[:, -1, -1].unsqueeze(-1).unsqueeze(-1)
F = F_est[0].data.cpu().numpy()
mask_F = compute_residual(pts_orig, F) <= threshold
mask_F = mask_F.astype(bool).flatten()
score = mask_F.sum()
F_best = F.T
inliers_best = score
if postprocess:
import pyransac
inliers_best = score
for th in [25, 50, 75]:
perc = np.percentile(weights, threshold)
good = np.where(weights > perc)[0]
if len(good) < 9:
continue
pts_ = pts_orig[good]
#_F, _ = cv2.findFundamentalMat(pts_[:, 2:], pts_[:, :2], cv2.FM_LMEDS)
_F, _mask_F = pyransac.findFundamentalMatrix(
kp1[good],
kp2[good],
0.25,
0.99999,
500000,
0,
error_type='sampson',
symmetric_error_check=True,
enable_degeneracy_check=False)
if _F is None:
continue
_mask_F = compute_residual(pts_orig, _F) <= threshold
inliers = _mask_F.sum()
if inliers > inliers_best:
F_best = _F
inliers_best = inliers
mask_F = _mask_F
if inliers_best < 8:
return _fail()
# OpenCV can return multiple values as 6x3 or 9x3 matrices
F = F_best
if F_best is None:
return _fail()
elif F.shape[0] != 3:
Fs = np.split(F, len(F) / 3)
else:
Fs = [F]
# 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