image0 = read_image_modified(image0, opt.resize,
opt.resize_float)
image1 = read_image_modified(image1, opt.resize,
opt.resize_float)
valid = matches > -1
mkpts0 = kpts0[valid]
mkpts1 = kpts1[matches[valid]]
mconf = conf[valid]
viz_path = eval_output_dir / '{}_matches.{}'.format(
str(i), opt.viz_extension)
color = cm.jet(mconf)
stem = pred['file_name']
text = []
make_matching_plot(image0, image1, kpts0, kpts1, mkpts0,
mkpts1, color, text, viz_path, stem, stem,
opt.show_keypoints, opt.fast_viz,
opt.opencv_display, 'Matches')
# process checkpoint for every 5e3 images
if (i + 1) % 5e3 == 0:
model_out_path = "model_epoch_{}.pth".format(epoch)
torch.save(superglue, model_out_path)
print('Epoch [{}/{}], Step [{}/{}], Checkpoint saved to {}'.
format(epoch, opt.epoch, i + 1, len(train_loader),
model_out_path))
# save checkpoint when an epoch finishes
epoch_loss /= len(train_loader)
model_out_path = "model_epoch_{}.pth".format(epoch)
torch.save(superglue, model_out_path)
print(
else:
out_file = None
if (stem1 - stem0) == 1:
line_alpha = 0.8
line_width = 1.5
else:
line_alpha = 0.2
line_width = 0
out = make_matching_plot(last_frame,
frame,
kpts0,
kpts1,
mkpts0,
mkpts1,
color,
text,
path=out_file,
show_keypoints=opt.show_keypoints,
small_text=small_text,
lw=line_width,
psm=15,
lalpha=line_alpha)
if fail:
print('Pose estimation failed, restart')
last_data = {k + '0': pred[k + '1'] for k in keys}
last_data['image0'] = frame_tensor
last_frame = frame
last_image_id = (vs.i - 1)
num = len(last_data['keypoints0'][0])
kp_colors = cm.hsv(np.random.rand(len(last_data['keypoints0'][0])))
# Visualize the matches.
color = cm.jet(mconf)
text = [
'SuperGlue',
'Keypoints: {}:{}'.format(len(kpts0), len(kpts1)),
'Matches: {}'.format(len(mkpts0)),
]
if rot0 != 0 or rot1 != 0:
text.append('Rotation: {}:{}'.format(rot0, rot1))
# Display extra parameter info.
k_thresh = matching.superpoint.config['keypoint_threshold']
m_thresh = matching.superglue.config['match_threshold']
small_text = [
'Keypoint Threshold: {:.4f}'.format(k_thresh),
'Match Threshold: {:.2f}'.format(m_thresh),
'Image Pair: {}:{}'.format(stem0, stem1),
]
image0 = cv2.imread(str(input_dir / name0), cv2.IMREAD_GRAYSCALE)
image1 = cv2.imread(str(input_dir / name1), cv2.IMREAD_GRAYSCALE)
make_matching_plot(image0, image1, kpts0, kpts1, mkpts0, mkpts1,
color, text, viz_path, True, True, True,
'Matches', small_text)
timer.update('viz_match')
timer.print('Finished pair {:5} of {:5}'.format(i, len(pairs)))
save_mvg_matches(matches_mvg, str(output_dir / 'matches.glue.txt'))
]
if rot0 != 0 or rot1 != 0:
text.append('Rotation: {}:{}'.format(rot0, rot1))
# Display extra parameter info.
k_thresh = matching.superpoint.config['keypoint_threshold']
m_thresh = matching.superglue.config['match_threshold']
small_text = [
'Keypoint Threshold: {:.4f}'.format(k_thresh),
'Match Threshold: {:.2f}'.format(m_thresh),
'Image Pair: {}:{}'.format(stem0, stem1),
]
make_matching_plot(image0, image1, kpts0, kpts1, mkpts0,
mkpts1, color, text, viz_path,
opt.show_keypoints, opt.fast_viz,
opt.opencv_display, 'Matches',
small_text)
timer.update('viz_match')
if do_viz_eval:
# Visualize the evaluation results for the image pair.
color = np.clip((epi_errs - 0) / (1e-3 - 0), 0, 1)
color = error_colormap(1 - color)
deg, delta = ' deg', 'Delta '
if not opt.fast_viz:
deg, delta = '°', '$\\Delta$'
e_t = 'FAIL' if np.isinf(err_t) else '{:.1f}{}'.format(
err_t, deg)
e_R = 'FAIL' if np.isinf(err_R) else '{:.1f}{}'.format(
# Visualize the matches.
color = cm.jet(mconf)
text = [
'SuperGlue',
'Keypoints: {}:{}'.format(len(kpts0), len(kpts1)),
'Matches: {}'.format(len(mkpts0)),
]
if rot0 != 0 or rot1 != 0:
text.append('Rotation: {}:{}'.format(rot0, rot1))
make_matching_plot(image0,
image1,
kpts0,
kpts1,
mkpts0,
mkpts1,
color,
text,
viz_path,
stem0,
stem1,
add_keypoints=opt.show_keypoints)
timer.update('viz_match')
if do_viz_eval:
# Visualize the evaluation results for the image pair.
color = np.clip((epi_errs - 0) / (1e-3 - 0), 0, 1)
color = error_colormap(1 - color)
e_t = 'FAIL' if np.isinf(err_t) else '{:.1f}°'.format(err_t)
e_R = 'FAIL' if np.isinf(err_R) else '{:.1f}°'.format(err_R)
text = [
'SuperGlue',
def get_keypoints(img1: np.ndarray, img2: np.ndarray, max_keypoints: int,
num_img: str, visualize: bool, resize: list,
match_path_exists: bool, dataset: str, mode: str):
'''
Retrieves pose from the keypoints of the images based on the given keypoint matching algorithm.
Inputs:
- img1: left image (undistorted)
- img2: right image (undistorted)
- max_keypoints: maximum number of keypoints matching tool should consider
- num_img: frame number
- visualize: indicates whether visualizations should be done and saved
- resize: dimensions at which images should be resized
- match_path_exists: indicates for SuperGlue if there are saved matches or if it should redo the matches
- mode: keypoint matching algorithm in use
- dataset: current dataset being evaluated
Outputs:
- R: recovered rotation matrix
- T: recovered translation vector
- mkpts1: matched keypoints in left image
- mkpts2: matched keypoints in right image
'''
left, _inp, left_scale = read_image(img1, device, resize, 0, False)
right, _inp, right_scale = read_image(img2, device, resize, 0, False)
left = left.astype('uint8')
right = right.astype('uint8')
i1, K1, distCoeffs1 = read("data/intrinsics/" + dataset + "_left.yaml")
i2, K2, distCoeffs2 = read("data/intrinsics/" + dataset + "_right.yaml")
K1 = scale_intrinsics(K1, left_scale)
K2 = scale_intrinsics(K2, right_scale)
if mode == "superglue":
input_pair = "data/pairs/kitti_pairs_" + num_img + ".txt"
npz_name = "left_" + num_img + "_right_" + num_img
out_dir = "data/matches/"
mkpts1, mkpts2 = get_SuperGlue_keypoints(input_pair, out_dir, npz_name,
max_keypoints, visualize,
resize, match_path_exists)
elif mode == "sift":
mkpts1, mkpts2 = get_SIFT_keypoints(left, right, max_keypoints)
elif mode == "orb":
mkpts1, mkpts2 = get_ORB_keypoints(left, right, max_keypoints)
R, T, F, _E = recover_pose(mkpts1, mkpts2, K1, K2)
left_rectified, right_rectified = rectify(left, right, K1, distCoeffs1, K2,
distCoeffs2, R, kitti_T_gt)
if visualize:
text = [mode, "Best 100 of " + str(max_keypoints) + " keypoints"]
colors = np.array(['red'] * len(mkpts1))
res_path = str("results/matches/" + mode + "/")
match_dir = Path(res_path)
match_dir.mkdir(parents=True, exist_ok=True)
path = res_path + dataset + "_" + mode + "_matches_" + num_img + ".png"
make_matching_plot(left,
right,
mkpts1,
mkpts2,
mkpts1,
mkpts2,
colors,
text,
path,
show_keypoints=False,
fast_viz=False,
opencv_display=False,
opencv_title='matches',
small_text=[])
save_disp_path = "results/disparity/" + mode + "/"
disp_dir = Path(save_disp_path)
disp_dir.mkdir(parents=True, exist_ok=True)
disp = get_disparity(left_rectified, right_rectified, maxDisparity=128)
plt.imsave(save_disp_path + dataset + "_" + mode + "_disp_" + num_img +
".png",
disp,
cmap="jet")
return R, T, mkpts1, mkpts2
